PX4AutoPilotPlugin.cc 6.01 KB
Newer Older
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24
/*=====================================================================
 
 QGroundControl Open Source Ground Control Station
 
 (c) 2009 - 2014 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
 
 This file is part of the QGROUNDCONTROL project
 
 QGROUNDCONTROL is free software: you can redistribute it and/or modify
 it under the terms of the GNU General Public License as published by
 the Free Software Foundation, either version 3 of the License, or
 (at your option) any later version.
 
 QGROUNDCONTROL is distributed in the hope that it will be useful,
 but WITHOUT ANY WARRANTY; without even the implied warranty of
 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 GNU General Public License for more details.
 
 You should have received a copy of the GNU General Public License
 along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.
 
 ======================================================================*/

#include "PX4AutoPilotPlugin.h"
Don Gagne's avatar
Don Gagne committed
25
#include "AutoPilotPluginManager.h"
26
#include "PX4ParameterLoader.h"
27
#include "PX4AirframeLoader.h"
Don Gagne's avatar
Don Gagne committed
28
#include "FlightModesComponentController.h"
29
#include "AirframeComponentController.h"
30
#include "QGCMessageBox.h"
31
#include "UAS.h"
32 33 34 35 36 37 38 39 40 41 42 43

/// @file
///     @brief This is the AutoPilotPlugin implementatin for the MAV_AUTOPILOT_PX4 type.
///     @author Don Gagne <don@thegagnes.com>

enum PX4_CUSTOM_MAIN_MODE {
    PX4_CUSTOM_MAIN_MODE_MANUAL = 1,
    PX4_CUSTOM_MAIN_MODE_ALTCTL,
    PX4_CUSTOM_MAIN_MODE_POSCTL,
    PX4_CUSTOM_MAIN_MODE_AUTO,
    PX4_CUSTOM_MAIN_MODE_ACRO,
    PX4_CUSTOM_MAIN_MODE_OFFBOARD,
44
    PX4_CUSTOM_MAIN_MODE_STABILIZED,
45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66
};

enum PX4_CUSTOM_SUB_MODE_AUTO {
    PX4_CUSTOM_SUB_MODE_AUTO_READY = 1,
    PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF,
    PX4_CUSTOM_SUB_MODE_AUTO_LOITER,
    PX4_CUSTOM_SUB_MODE_AUTO_MISSION,
    PX4_CUSTOM_SUB_MODE_AUTO_RTL,
    PX4_CUSTOM_SUB_MODE_AUTO_LAND,
    PX4_CUSTOM_SUB_MODE_AUTO_RTGS
};

union px4_custom_mode {
    struct {
        uint16_t reserved;
        uint8_t main_mode;
        uint8_t sub_mode;
    };
    uint32_t data;
    float data_float;
};

67 68
PX4AutoPilotPlugin::PX4AutoPilotPlugin(Vehicle* vehicle, QObject* parent) :
    AutoPilotPlugin(vehicle, parent),
Don Gagne's avatar
Don Gagne committed
69 70 71 72 73
    _parameterFacts(NULL),
    _airframeComponent(NULL),
    _radioComponent(NULL),
    _flightModesComponent(NULL),
    _sensorsComponent(NULL),
74
    _safetyComponent(NULL),
dogmaphobic's avatar
dogmaphobic committed
75
    _powerComponent(NULL),
76
    _incorrectParameterVersion(false)
77
{
78
    Q_ASSERT(vehicle);
79
    
80
    _parameterFacts = new PX4ParameterLoader(this, vehicle, this);
81
    Q_CHECK_PTR(_parameterFacts);
82
    
83
    connect(_parameterFacts, &PX4ParameterLoader::parametersReady, this, &PX4AutoPilotPlugin::_parametersReadyPreChecks);
84
    connect(_parameterFacts, &PX4ParameterLoader::parameterListProgress, this, &PX4AutoPilotPlugin::parameterListProgress);
85

86
    _airframeFacts = new PX4AirframeLoader(this, _vehicle->uas(), this);
87
    Q_CHECK_PTR(_airframeFacts);
Don Gagne's avatar
Don Gagne committed
88
    
89
    PX4ParameterLoader::loadParameterFactMetaData();
90
    PX4AirframeLoader::loadAirframeFactMetaData();
Don Gagne's avatar
Don Gagne committed
91 92 93 94
}

PX4AutoPilotPlugin::~PX4AutoPilotPlugin()
{
95
    delete _parameterFacts;
96
    delete _airframeFacts;
97 98
}

99
void PX4AutoPilotPlugin::clearStaticData(void)
Don Gagne's avatar
Don Gagne committed
100
{
101
    PX4ParameterLoader::clearStaticData();
102
    PX4AirframeLoader::clearStaticData();
Don Gagne's avatar
Don Gagne committed
103 104
}

105
const QVariantList& PX4AutoPilotPlugin::vehicleComponents(void)
106
{
107
    if (_components.count() == 0 && !_incorrectParameterVersion) {
108
        Q_ASSERT(_vehicle);
109
        
110
        if (parametersReady()) {
111
            _airframeComponent = new AirframeComponent(_vehicle->uas(), this);
112 113 114 115
            Q_CHECK_PTR(_airframeComponent);
            _airframeComponent->setupTriggerSignals();
            _components.append(QVariant::fromValue((VehicleComponent*)_airframeComponent));
            
116 117 118 119 120 121 122 123 124
            _radioComponent = new RadioComponent(_vehicle->uas(), this);
            Q_CHECK_PTR(_radioComponent);
            _radioComponent->setupTriggerSignals();
            _components.append(QVariant::fromValue((VehicleComponent*)_radioComponent));
            
            _flightModesComponent = new FlightModesComponent(_vehicle->uas(), this);
            Q_CHECK_PTR(_flightModesComponent);
            _flightModesComponent->setupTriggerSignals();
            _components.append(QVariant::fromValue((VehicleComponent*)_flightModesComponent));
125
            
126
            _sensorsComponent = new SensorsComponent(_vehicle->uas(), this);
127 128 129 130
            Q_CHECK_PTR(_sensorsComponent);
            _sensorsComponent->setupTriggerSignals();
            _components.append(QVariant::fromValue((VehicleComponent*)_sensorsComponent));
            
131
            _powerComponent = new PowerComponent(_vehicle->uas(), this);
132 133 134 135
            Q_CHECK_PTR(_powerComponent);
            _powerComponent->setupTriggerSignals();
            _components.append(QVariant::fromValue((VehicleComponent*)_powerComponent));
            
136
            _safetyComponent = new SafetyComponent(_vehicle->uas(), this);
137 138 139 140
            Q_CHECK_PTR(_safetyComponent);
            _safetyComponent->setupTriggerSignals();
            _components.append(QVariant::fromValue((VehicleComponent*)_safetyComponent));
        } else {
141
            qWarning() << "Call to vehicleCompenents prior to parametersReady";
142
        }
143 144 145 146 147
    }
    
    return _components;
}

Don Gagne's avatar
Don Gagne committed
148
/// This will perform various checks prior to signalling that the plug in ready
149
void PX4AutoPilotPlugin::_parametersReadyPreChecks(bool missingParameters)
150
{
Don Gagne's avatar
Don Gagne committed
151 152 153
    // Check for older parameter version set
    // FIXME: Firmware is moving to version stamp parameter set. Once that is complete the version stamp
    // should be used instead.
Don Gagne's avatar
Don Gagne committed
154
    if (parameterExists(FactSystem::defaultComponentId, "SENS_GYRO_XOFF")) {
155
        _incorrectParameterVersion = true;
156 157 158 159
        QGCMessageBox::warning("Setup", "This version of GroundControl can only perform vehicle setup on a newer version of firmware. "
										"Please perform a Firmware Upgrade if you wish to use Vehicle Setup.");
	}
	
160 161 162 163
    _parametersReady = true;
    _missingParameters = missingParameters;
    emit missingParametersChanged(_missingParameters);
    emit parametersReadyChanged(_parametersReady);
164
}