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

Don Gagne's avatar
Don Gagne committed
10 11 12 13 14

/// @file
///     @author Don Gagne <don@thegagnes.com>

#include "PX4FirmwarePlugin.h"
15
#include "PX4ParameterMetaData.h"
16
#include "QGCApplication.h"
17
#include "AutoPilotPlugins/PX4/PX4AutoPilotPlugin.h"    // FIXME: Hack
Don Gagne's avatar
Don Gagne committed
18 19 20

#include <QDebug>

Daniel Agar's avatar
Daniel Agar committed
21
#include "px4_custom_mode.h"
Don Gagne's avatar
Don Gagne committed
22 23 24 25 26 27

struct Modes2Name {
    uint8_t     main_mode;
    uint8_t     sub_mode;
    const char* name;       ///< Name for flight mode
    bool        canBeSet;   ///< true: Vehicle can be set to this flight mode
Daniel Agar's avatar
Daniel Agar committed
28 29
    bool        fixedWing;  /// fixed wing compatible
    bool        multiRotor;  /// multi rotor compatible
Don Gagne's avatar
Don Gagne committed
30 31
};

Don Gagne's avatar
Don Gagne committed
32
const char* PX4FirmwarePlugin::manualFlightMode =       "Manual";
Daniel Agar's avatar
Daniel Agar committed
33 34 35 36 37 38 39
const char* PX4FirmwarePlugin::altCtlFlightMode =       "Altitude";
const char* PX4FirmwarePlugin::posCtlFlightMode =       "Position";
const char* PX4FirmwarePlugin::missionFlightMode =      "Mission";
const char* PX4FirmwarePlugin::holdFlightMode =         "Hold";
const char* PX4FirmwarePlugin::takeoffFlightMode =      "Takeoff";
const char* PX4FirmwarePlugin::landingFlightMode =      "Land";
const char* PX4FirmwarePlugin::rtlFlightMode =          "Return";
Don Gagne's avatar
Don Gagne committed
40
const char* PX4FirmwarePlugin::acroFlightMode =         "Acro";
Daniel Agar's avatar
Daniel Agar committed
41
const char* PX4FirmwarePlugin::offboardFlightMode =     "Offboard";
Don Gagne's avatar
Don Gagne committed
42 43
const char* PX4FirmwarePlugin::stabilizedFlightMode =   "Stabilized";
const char* PX4FirmwarePlugin::rattitudeFlightMode =    "Rattitude";
Jimmy Johnson's avatar
Jimmy Johnson committed
44
const char* PX4FirmwarePlugin::followMeFlightMode =     "Follow Me";
Don Gagne's avatar
Don Gagne committed
45

Daniel Agar's avatar
Daniel Agar committed
46 47 48 49
const char* PX4FirmwarePlugin::rtgsFlightMode =         "Return to Groundstation";

const char* PX4FirmwarePlugin::readyFlightMode =        "Ready"; // unused

Don Gagne's avatar
Don Gagne committed
50
/// Tranlates from PX4 custom modes to flight mode names
51

Don Gagne's avatar
Don Gagne committed
52
static const struct Modes2Name rgModes2Name[] = {
Daniel Agar's avatar
Daniel Agar committed
53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69
    //main_mode                         sub_mode                                name                                      canBeSet  FW      MC
    { PX4_CUSTOM_MAIN_MODE_MANUAL,      0,                                      PX4FirmwarePlugin::manualFlightMode,        true,   true,   true },
    { PX4_CUSTOM_MAIN_MODE_STABILIZED,  0,                                      PX4FirmwarePlugin::stabilizedFlightMode,    true,   true,   true },
    { PX4_CUSTOM_MAIN_MODE_ACRO,        0,                                      PX4FirmwarePlugin::acroFlightMode,          true,   false,  true },
    { PX4_CUSTOM_MAIN_MODE_RATTITUDE,   0,                                      PX4FirmwarePlugin::rattitudeFlightMode,     true,   false,  true },
    { PX4_CUSTOM_MAIN_MODE_ALTCTL,      0,                                      PX4FirmwarePlugin::altCtlFlightMode,        true,   true,   true },
    { PX4_CUSTOM_MAIN_MODE_POSCTL,      0,                                      PX4FirmwarePlugin::posCtlFlightMode,        true,   true,   true },
    { PX4_CUSTOM_MAIN_MODE_AUTO,        PX4_CUSTOM_SUB_MODE_AUTO_LOITER,        PX4FirmwarePlugin::holdFlightMode,          true,   true,   true },
    { PX4_CUSTOM_MAIN_MODE_AUTO,        PX4_CUSTOM_SUB_MODE_AUTO_MISSION,       PX4FirmwarePlugin::missionFlightMode,       true,   true,   true },
    { PX4_CUSTOM_MAIN_MODE_AUTO,        PX4_CUSTOM_SUB_MODE_AUTO_RTL,           PX4FirmwarePlugin::rtlFlightMode,           true,   true,   true },
    { PX4_CUSTOM_MAIN_MODE_AUTO,        PX4_CUSTOM_SUB_MODE_AUTO_FOLLOW_TARGET, PX4FirmwarePlugin::followMeFlightMode,      true,   true,   true },
    { PX4_CUSTOM_MAIN_MODE_OFFBOARD,    0,                                      PX4FirmwarePlugin::offboardFlightMode,      true,   true,   true },
    // modes that can't be directly set by the user
    { PX4_CUSTOM_MAIN_MODE_AUTO,        PX4_CUSTOM_SUB_MODE_AUTO_LAND,          PX4FirmwarePlugin::landingFlightMode,       false,  true,   true },
    { PX4_CUSTOM_MAIN_MODE_AUTO,        PX4_CUSTOM_SUB_MODE_AUTO_READY,         PX4FirmwarePlugin::readyFlightMode,         false,  true,   true },
    { PX4_CUSTOM_MAIN_MODE_AUTO,        PX4_CUSTOM_SUB_MODE_AUTO_RTGS,          PX4FirmwarePlugin::rtgsFlightMode,          false,  true,   true },
    { PX4_CUSTOM_MAIN_MODE_AUTO,        PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF,       PX4FirmwarePlugin::takeoffFlightMode,       false,  true,   true },
Don Gagne's avatar
Don Gagne committed
70 71 72 73 74
};

QList<VehicleComponent*> PX4FirmwarePlugin::componentsForVehicle(AutoPilotPlugin* vehicle)
{
    Q_UNUSED(vehicle);
dogmaphobic's avatar
dogmaphobic committed
75

Don Gagne's avatar
Don Gagne committed
76 77 78
    return QList<VehicleComponent*>();
}

Daniel Agar's avatar
Daniel Agar committed
79
QStringList PX4FirmwarePlugin::flightModes(Vehicle* vehicle)
Don Gagne's avatar
Don Gagne committed
80 81
{
    QStringList flightModes;
dogmaphobic's avatar
dogmaphobic committed
82

Don Gagne's avatar
Don Gagne committed
83 84
    for (size_t i=0; i<sizeof(rgModes2Name)/sizeof(rgModes2Name[0]); i++) {
        const struct Modes2Name* pModes2Name = &rgModes2Name[i];
dogmaphobic's avatar
dogmaphobic committed
85

Don Gagne's avatar
Don Gagne committed
86
        if (pModes2Name->canBeSet) {
Daniel Agar's avatar
Daniel Agar committed
87 88 89 90 91 92 93 94 95
            bool fw = (vehicle->fixedWing() && pModes2Name->fixedWing);
            bool mc = (vehicle->multiRotor() && pModes2Name->multiRotor);

            // show all modes for generic, vtol, etc
            bool other = !vehicle->fixedWing() && !vehicle->multiRotor();

            if (fw || mc || other) {
                flightModes += pModes2Name->name;
            }
Don Gagne's avatar
Don Gagne committed
96 97
        }
    }
dogmaphobic's avatar
dogmaphobic committed
98

Don Gagne's avatar
Don Gagne committed
99 100 101
    return flightModes;
}

Don Gagne's avatar
Don Gagne committed
102
QString PX4FirmwarePlugin::flightMode(uint8_t base_mode, uint32_t custom_mode) const
Don Gagne's avatar
Don Gagne committed
103 104
{
    QString flightMode = "Unknown";
dogmaphobic's avatar
dogmaphobic committed
105

Don Gagne's avatar
Don Gagne committed
106 107 108
    if (base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) {
        union px4_custom_mode px4_mode;
        px4_mode.data = custom_mode;
dogmaphobic's avatar
dogmaphobic committed
109

Don Gagne's avatar
Don Gagne committed
110 111 112
        bool found = false;
        for (size_t i=0; i<sizeof(rgModes2Name)/sizeof(rgModes2Name[0]); i++) {
            const struct Modes2Name* pModes2Name = &rgModes2Name[i];
dogmaphobic's avatar
dogmaphobic committed
113

Don Gagne's avatar
Don Gagne committed
114 115 116 117 118 119
            if (pModes2Name->main_mode == px4_mode.main_mode && pModes2Name->sub_mode == px4_mode.sub_mode) {
                flightMode = pModes2Name->name;
                found = true;
                break;
            }
        }
dogmaphobic's avatar
dogmaphobic committed
120

Don Gagne's avatar
Don Gagne committed
121 122 123 124 125 126
        if (!found) {
            qWarning() << "Unknown flight mode" << custom_mode;
        }
    } else {
        qWarning() << "PX4 Flight Stack flight mode without custom mode enabled?";
    }
dogmaphobic's avatar
dogmaphobic committed
127

Don Gagne's avatar
Don Gagne committed
128 129 130 131 132 133 134
    return flightMode;
}

bool PX4FirmwarePlugin::setFlightMode(const QString& flightMode, uint8_t* base_mode, uint32_t* custom_mode)
{
    *base_mode = 0;
    *custom_mode = 0;
dogmaphobic's avatar
dogmaphobic committed
135

Don Gagne's avatar
Don Gagne committed
136 137 138
    bool found = false;
    for (size_t i=0; i<sizeof(rgModes2Name)/sizeof(rgModes2Name[0]); i++) {
        const struct Modes2Name* pModes2Name = &rgModes2Name[i];
dogmaphobic's avatar
dogmaphobic committed
139

Don Gagne's avatar
Don Gagne committed
140 141 142 143 144 145
        if (flightMode.compare(pModes2Name->name, Qt::CaseInsensitive) == 0) {
            union px4_custom_mode px4_mode;

            px4_mode.data = 0;
            px4_mode.main_mode = pModes2Name->main_mode;
            px4_mode.sub_mode = pModes2Name->sub_mode;
dogmaphobic's avatar
dogmaphobic committed
146

Don Gagne's avatar
Don Gagne committed
147 148
            *base_mode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
            *custom_mode = px4_mode.data;
dogmaphobic's avatar
dogmaphobic committed
149

Don Gagne's avatar
Don Gagne committed
150 151 152 153
            found = true;
            break;
        }
    }
dogmaphobic's avatar
dogmaphobic committed
154

Don Gagne's avatar
Don Gagne committed
155 156 157
    if (!found) {
        qWarning() << "Unknown flight Mode" << flightMode;
    }
dogmaphobic's avatar
dogmaphobic committed
158

Don Gagne's avatar
Don Gagne committed
159 160
    return found;
}
161 162 163

int PX4FirmwarePlugin::manualControlReservedButtonCount(void)
{
164
    return 0;   // 0 buttons reserved for rc switch simulation
165
}
Don Gagne's avatar
Don Gagne committed
166

Don Gagne's avatar
Don Gagne committed
167 168
bool PX4FirmwarePlugin::isCapable(FirmwareCapabilities capabilities)
{
169
    return (capabilities & (MavCmdPreflightStorageCapability | GuidedModeCapability | SetFlightModeCapability | PauseVehicleCapability)) == capabilities;
Don Gagne's avatar
Don Gagne committed
170
}
171 172 173 174

void PX4FirmwarePlugin::initializeVehicle(Vehicle* vehicle)
{
    Q_UNUSED(vehicle);
dogmaphobic's avatar
dogmaphobic committed
175

176 177
    // PX4 Flight Stack doesn't need to do any extra work
}
178 179 180 181 182 183 184

bool PX4FirmwarePlugin::sendHomePositionToVehicle(void)
{
    // PX4 stack does not want home position sent in the first position.
    // Subsequent sequence numbers must be adjusted.
    return false;
}
185

186
void PX4FirmwarePlugin::addMetaDataToFact(QObject* parameterMetaData, Fact* fact, MAV_TYPE vehicleType)
187
{
188 189 190 191 192 193 194
    PX4ParameterMetaData* px4MetaData = qobject_cast<PX4ParameterMetaData*>(parameterMetaData);

    if (px4MetaData) {
        px4MetaData->addMetaDataToFact(fact, vehicleType);
    } else {
        qWarning() << "Internal error: pointer passed to PX4FirmwarePlugin::addMetaDataToFact not PX4ParameterMetaData";
    }
195
}
Don Gagne's avatar
Don Gagne committed
196 197 198 199 200

QList<MAV_CMD> PX4FirmwarePlugin::supportedMissionCommands(void)
{
    QList<MAV_CMD> list;

201
    list << MAV_CMD_NAV_WAYPOINT
202
         << MAV_CMD_NAV_LOITER_UNLIM << MAV_CMD_NAV_LOITER_TIME << MAV_CMD_NAV_LOITER_TO_ALT
Don Gagne's avatar
Don Gagne committed
203
         << MAV_CMD_NAV_LAND << MAV_CMD_NAV_TAKEOFF
204
         << MAV_CMD_DO_JUMP
Don Gagne's avatar
Don Gagne committed
205
         << MAV_CMD_DO_VTOL_TRANSITION << MAV_CMD_NAV_VTOL_TAKEOFF << MAV_CMD_NAV_VTOL_LAND
206
         << MAV_CMD_DO_DIGICAM_CONTROL
207
         << MAV_CMD_DO_SET_CAM_TRIGG_DIST
208
         << MAV_CMD_DO_SET_SERVO
Don Gagne's avatar
Don Gagne committed
209
         << MAV_CMD_DO_CHANGE_SPEED
210
         << MAV_CMD_DO_LAND_START
211
         << MAV_CMD_DO_MOUNT_CONFIGURE
212
         << MAV_CMD_DO_MOUNT_CONTROL;
Jimmy Johnson's avatar
Jimmy Johnson committed
213

Don Gagne's avatar
Don Gagne committed
214 215
    return list;
}
216 217 218 219 220 221 222 223

void PX4FirmwarePlugin::missionCommandOverrides(QString& commonJsonFilename, QString& fixedWingJsonFilename, QString& multiRotorJsonFilename) const
{
    // No overrides
    commonJsonFilename = QStringLiteral(":/json/PX4/MavCmdInfoCommon.json");
    fixedWingJsonFilename = QStringLiteral(":/json/PX4/MavCmdInfoFixedWing.json");
    multiRotorJsonFilename = QStringLiteral(":/json/PX4/MavCmdInfoMultiRotor.json");
}
224 225 226 227 228 229 230

QObject* PX4FirmwarePlugin::loadParameterMetaData(const QString& metaDataFile)
{
    PX4ParameterMetaData* metaData = new PX4ParameterMetaData;
    metaData->loadParameterFactMetaDataFile(metaDataFile);
    return metaData;
}
Don Gagne's avatar
Don Gagne committed
231 232 233

void PX4FirmwarePlugin::pauseVehicle(Vehicle* vehicle)
{
234 235
    // then tell it to loiter at the current position
    mavlink_message_t msg;
Lorenz Meier's avatar
Lorenz Meier committed
236
    mavlink_command_long_t cmd;
237 238 239 240

    cmd.command = (uint16_t)MAV_CMD_DO_REPOSITION;
    cmd.confirmation = 0;
    cmd.param1 = -1.0f;
241
    cmd.param2 = MAV_DO_REPOSITION_FLAGS_CHANGE_MODE;
242 243 244 245 246 247
    cmd.param3 = 0.0f;
    cmd.param4 = NAN;
    cmd.param5 = NAN;
    cmd.param6 = NAN;
    cmd.param7 = NAN;
    cmd.target_system = vehicle->id();
248
    cmd.target_component = vehicle->defaultComponentId();
249 250 251 252

    MAVLinkProtocol* mavlink = qgcApp()->toolbox()->mavlinkProtocol();
    mavlink_msg_command_long_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &cmd);

253
    vehicle->sendMessageOnPriorityLink(msg);
Don Gagne's avatar
Don Gagne committed
254
}
255 256 257 258 259 260 261 262 263 264 265 266 267 268 269 270 271 272 273

void PX4FirmwarePlugin::guidedModeRTL(Vehicle* vehicle)
{
    vehicle->setFlightMode(rtlFlightMode);
}

void PX4FirmwarePlugin::guidedModeLand(Vehicle* vehicle)
{
    vehicle->setFlightMode(landingFlightMode);
}

void PX4FirmwarePlugin::guidedModeTakeoff(Vehicle* vehicle, double altitudeRel)
{
    Q_UNUSED(altitudeRel);
    if (qIsNaN(vehicle->altitudeAMSL()->rawValue().toDouble())) {
        qgcApp()->showMessage(QStringLiteral("Unable to takeoff, vehicle position not known."));
        return;
    }

274
    MAVLinkProtocol* mavlink = qgcApp()->toolbox()->mavlinkProtocol();
275

276
    // Set destination altitude
277
    mavlink_message_t msg;
Lorenz Meier's avatar
Lorenz Meier committed
278
    mavlink_command_long_t cmd;
279

Lorenz Meier's avatar
Lorenz Meier committed
280
    cmd.command = (uint16_t)MAV_CMD_NAV_TAKEOFF;
281
    cmd.confirmation = 0;
282
    cmd.param1 = -1.0f;
283
    cmd.param2 = 0.0f;
284
    cmd.param3 = 0.0f;
285 286 287 288
    cmd.param4 = NAN;
    cmd.param5 = NAN;
    cmd.param6 = NAN;
    cmd.param7 = vehicle->altitudeAMSL()->rawValue().toDouble() + altitudeRel;
289
    cmd.target_system = vehicle->id();
290
    cmd.target_component = vehicle->defaultComponentId();
291

292
    mavlink_msg_command_long_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &cmd);
293
    vehicle->sendMessageOnPriorityLink(msg);
294 295 296 297 298 299 300 301 302 303
}

void PX4FirmwarePlugin::guidedModeGotoLocation(Vehicle* vehicle, const QGeoCoordinate& gotoCoord)
{
    if (qIsNaN(vehicle->altitudeRelative()->rawValue().toDouble())) {
        qgcApp()->showMessage(QStringLiteral("Unable to go to location, vehicle position not known."));
        return;
    }

    mavlink_message_t msg;
Lorenz Meier's avatar
Lorenz Meier committed
304
    mavlink_command_long_t cmd;
305 306 307 308 309 310 311 312 313 314 315

    cmd.command = (uint16_t)MAV_CMD_DO_REPOSITION;
    cmd.confirmation = 0;
    cmd.param1 = -1.0f;
    cmd.param2 = MAV_DO_REPOSITION_FLAGS_CHANGE_MODE;
    cmd.param3 = 0.0f;
    cmd.param4 = NAN;
    cmd.param5 = gotoCoord.latitude() * 1e7;
    cmd.param6 = gotoCoord.longitude() * 1e7;
    cmd.param7 = vehicle->altitudeAMSL()->rawValue().toDouble();
    cmd.target_system = vehicle->id();
316
    cmd.target_component = vehicle->defaultComponentId();
317 318 319 320

    MAVLinkProtocol* mavlink = qgcApp()->toolbox()->mavlinkProtocol();
    mavlink_msg_command_long_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &cmd);

321
    vehicle->sendMessageOnPriorityLink(msg);
322 323 324 325 326 327 328 329 330 331
}

void PX4FirmwarePlugin::guidedModeChangeAltitude(Vehicle* vehicle, double altitudeRel)
{
    if (qIsNaN(vehicle->altitudeRelative()->rawValue().toDouble())) {
        qgcApp()->showMessage(QStringLiteral("Unable to change altitude, vehicle altitude not known."));
        return;
    }

    mavlink_message_t msg;
Lorenz Meier's avatar
Lorenz Meier committed
332
    mavlink_command_long_t cmd;
333 334 335 336 337 338 339 340 341 342 343

    cmd.command = (uint16_t)MAV_CMD_DO_REPOSITION;
    cmd.confirmation = 0;
    cmd.param1 = -1.0f;
    cmd.param2 = MAV_DO_REPOSITION_FLAGS_CHANGE_MODE;
    cmd.param3 = 0.0f;
    cmd.param4 = NAN;
    cmd.param5 = NAN;
    cmd.param6 = NAN;
    cmd.param7 = vehicle->altitudeAMSL()->rawValue().toDouble() + altitudeRel;
    cmd.target_system = vehicle->id();
344
    cmd.target_component = vehicle->defaultComponentId();
345 346 347 348

    MAVLinkProtocol* mavlink = qgcApp()->toolbox()->mavlinkProtocol();
    mavlink_msg_command_long_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &cmd);

349
    vehicle->sendMessageOnPriorityLink(msg);
350 351 352 353 354
}

void PX4FirmwarePlugin::setGuidedMode(Vehicle* vehicle, bool guidedMode)
{
    if (guidedMode) {
Daniel Agar's avatar
Daniel Agar committed
355
        vehicle->setFlightMode(holdFlightMode);
356 357 358 359
    } else {
        pauseVehicle(vehicle);
    }
}
360 361 362 363

bool PX4FirmwarePlugin::isGuidedMode(const Vehicle* vehicle) const
{
    // Not supported by generic vehicle
Daniel Agar's avatar
Daniel Agar committed
364
    return (vehicle->flightMode() == holdFlightMode || vehicle->flightMode() == takeoffFlightMode
365
            || vehicle->flightMode() == landingFlightMode);
366
}