PX4AutoPilotPlugin.cc 7.31 KB
Newer Older
1 2
/****************************************************************************
 *
Gus Grubba's avatar
Gus Grubba committed
3
 * (c) 2009-2020 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
4 5 6 7 8
 *
 * QGroundControl is licensed according to the terms in the file
 * COPYING.md in the root of the source code directory.
 *
 ****************************************************************************/
9

10 11

#include "PX4AutoPilotPlugin.h"
12
#include "PX4AirframeLoader.h"
Don Gagne's avatar
Don Gagne committed
13
#include "PX4AdvancedFlightModesController.h"
14
#include "AirframeComponentController.h"
15
#include "UAS.h"
16
#include "FirmwarePlugin/PX4/PX4ParameterMetaData.h"  // FIXME: Hack
17
#include "FirmwarePlugin/PX4/PX4FirmwarePlugin.h"  // FIXME: Hack
18
#include "QGCApplication.h"
19 20 21 22 23 24
#include "FlightModesComponent.h"
#include "PX4RadioComponent.h"
#include "PX4TuningComponent.h"
#include "PowerComponent.h"
#include "SafetyComponent.h"
#include "SensorsComponent.h"
25 26 27 28 29

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

Don Gagne's avatar
Don Gagne committed
30 31
PX4AutoPilotPlugin::PX4AutoPilotPlugin(Vehicle* vehicle, QObject* parent)
    : AutoPilotPlugin(vehicle, parent)
32
    , _incorrectParameterVersion(false)
33 34 35 36 37 38 39 40 41 42
    , _airframeComponent(nullptr)
    , _radioComponent(nullptr)
    , _esp8266Component(nullptr)
    , _flightModesComponent(nullptr)
    , _sensorsComponent(nullptr)
    , _safetyComponent(nullptr)
    , _powerComponent(nullptr)
    , _motorComponent(nullptr)
    , _tuningComponent(nullptr)
    , _syslinkComponent(nullptr)
43
{
DonLakeFlyer's avatar
DonLakeFlyer committed
44 45 46 47
    if (!vehicle) {
        qWarning() << "Internal error";
        return;
    }
48

49
    _airframeFacts = new PX4AirframeLoader(this, _vehicle->uas(), this);
50
    Q_CHECK_PTR(_airframeFacts);
51

52
    PX4AirframeLoader::loadAirframeMetaData();
Don Gagne's avatar
Don Gagne committed
53 54 55 56
}

PX4AutoPilotPlugin::~PX4AutoPilotPlugin()
{
57
    delete _airframeFacts;
58 59
}

60
const QVariantList& PX4AutoPilotPlugin::vehicleComponents(void)
61
{
62
    if (_components.count() == 0 && !_incorrectParameterVersion) {
DonLakeFlyer's avatar
DonLakeFlyer committed
63 64 65 66 67 68 69 70 71 72 73 74
        if (_vehicle) {
            if (_vehicle->parameterManager()->parametersReady()) {
                _airframeComponent = new AirframeComponent(_vehicle, this);
                _airframeComponent->setupTriggerSignals();
                _components.append(QVariant::fromValue((VehicleComponent*)_airframeComponent));

                if (!_vehicle->hilMode()) {
                    _sensorsComponent = new SensorsComponent(_vehicle, this);
                    _sensorsComponent->setupTriggerSignals();
                    _components.append(QVariant::fromValue((VehicleComponent*)_sensorsComponent));
                }

75 76 77 78
                _radioComponent = new PX4RadioComponent(_vehicle, this);
                _radioComponent->setupTriggerSignals();
                _components.append(QVariant::fromValue((VehicleComponent*)_radioComponent));

DonLakeFlyer's avatar
DonLakeFlyer committed
79 80 81 82 83 84 85
                _flightModesComponent = new FlightModesComponent(_vehicle, this);
                _flightModesComponent->setupTriggerSignals();
                _components.append(QVariant::fromValue((VehicleComponent*)_flightModesComponent));

                _powerComponent = new PowerComponent(_vehicle, this);
                _powerComponent->setupTriggerSignals();
                _components.append(QVariant::fromValue((VehicleComponent*)_powerComponent));
86

DonLakeFlyer's avatar
DonLakeFlyer committed
87 88 89
                _motorComponent = new MotorComponent(_vehicle, this);
                _motorComponent->setupTriggerSignals();
                _components.append(QVariant::fromValue((VehicleComponent*)_motorComponent));
Don Gagne's avatar
Don Gagne committed
90

DonLakeFlyer's avatar
DonLakeFlyer committed
91 92 93
                _safetyComponent = new SafetyComponent(_vehicle, this);
                _safetyComponent->setupTriggerSignals();
                _components.append(QVariant::fromValue((VehicleComponent*)_safetyComponent));
Don Gagne's avatar
Don Gagne committed
94

DonLakeFlyer's avatar
DonLakeFlyer committed
95 96 97
                _tuningComponent = new PX4TuningComponent(_vehicle, this);
                _tuningComponent->setupTriggerSignals();
                _components.append(QVariant::fromValue((VehicleComponent*)_tuningComponent));
Don Gagne's avatar
Don Gagne committed
98

DonLakeFlyer's avatar
DonLakeFlyer committed
99 100 101 102 103 104 105 106 107 108 109 110 111 112 113
                //-- Is there support for cameras?
                if(_vehicle->parameterManager()->parameterExists(_vehicle->id(), "TRIG_MODE")) {
                    _cameraComponent = new CameraComponent(_vehicle, this);
                    _cameraComponent->setupTriggerSignals();
                    _components.append(QVariant::fromValue((VehicleComponent*)_cameraComponent));
                }

                //-- Is there an ESP8266 Connected?
                if(_vehicle->parameterManager()->parameterExists(MAV_COMP_ID_UDP_BRIDGE, "SW_VER")) {
                    _esp8266Component = new ESP8266Component(_vehicle, this);
                    _esp8266Component->setupTriggerSignals();
                    _components.append(QVariant::fromValue((VehicleComponent*)_esp8266Component));
                }
            } else {
                qWarning() << "Call to vehicleCompenents prior to parametersReady";
Don Gagne's avatar
Don Gagne committed
114
            }
115 116 117 118 119 120

            if(_vehicle->parameterManager()->parameterExists(_vehicle->id(), "SLNK_RADIO_CHAN")) {
                _syslinkComponent = new SyslinkComponent(_vehicle, this);
                _syslinkComponent->setupTriggerSignals();
                _components.append(QVariant::fromValue((VehicleComponent*)_syslinkComponent));
            }
121
        } else {
DonLakeFlyer's avatar
DonLakeFlyer committed
122
            qWarning() << "Internal error";
123
        }
124
    }
125

126 127 128
    return _components;
}

129
void PX4AutoPilotPlugin::parametersReadyPreChecks(void)
130
{
131 132 133
    // Base class must be called
    AutoPilotPlugin::parametersReadyPreChecks();

Don Gagne's avatar
Don Gagne committed
134 135 136 137
    QString hitlParam("SYS_HITL");
    if (_vehicle->parameterManager()->parameterExists(FactSystem::defaultComponentId, hitlParam) &&
            _vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, hitlParam)->rawValue().toBool()) {
        qgcApp()->showMessage(tr("Warning: Hardware In The Loop (HITL) simulation is enabled for this vehicle."));
138
    }
139
}
140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179

QString PX4AutoPilotPlugin::prerequisiteSetup(VehicleComponent* component) const
{
    bool requiresAirframeCheck = false;

    if (qobject_cast<const FlightModesComponent*>(component)) {
        if (_vehicle->parameterManager()->getParameter(-1, "COM_RC_IN_MODE")->rawValue().toInt() == 1) {
            // No RC input
            return QString();
        } else {
            if (_airframeComponent && !_airframeComponent->setupComplete()) {
                return _airframeComponent->name();
            } else if (_radioComponent && !_radioComponent->setupComplete()) {
                return _radioComponent->name();
            } else if (_sensorsComponent && !_vehicle->hilMode() && !_sensorsComponent->setupComplete()) {
                return _sensorsComponent->name();
            }
        }
    } else if (qobject_cast<const PX4RadioComponent*>(component)) {
        if (_vehicle->parameterManager()->getParameter(-1, "COM_RC_IN_MODE")->rawValue().toInt() != 1) {
            requiresAirframeCheck = true;
        }
    } else if (qobject_cast<const PX4TuningComponent*>(component)) {
        requiresAirframeCheck = true;
    } else if (qobject_cast<const PowerComponent*>(component)) {
        requiresAirframeCheck = true;
    } else if (qobject_cast<const SafetyComponent*>(component)) {
        requiresAirframeCheck = true;
    } else if (qobject_cast<const SensorsComponent*>(component)) {
        requiresAirframeCheck = true;
    }

    if (requiresAirframeCheck) {
        if (_airframeComponent && !_airframeComponent->setupComplete()) {
            return _airframeComponent->name();
        }
    }

    return QString();
}