PX4FirmwarePlugin.cc 14.2 KB
Newer Older
Don Gagne's avatar
Don Gagne committed
1
/*=====================================================================
dogmaphobic's avatar
dogmaphobic committed
2

Don Gagne's avatar
Don Gagne committed
3
 QGroundControl Open Source Ground Control Station
dogmaphobic's avatar
dogmaphobic committed
4

Don Gagne's avatar
Don Gagne committed
5
 (c) 2009 - 2015 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
dogmaphobic's avatar
dogmaphobic committed
6

Don Gagne's avatar
Don Gagne committed
7
 This file is part of the QGROUNDCONTROL project
dogmaphobic's avatar
dogmaphobic committed
8

Don Gagne's avatar
Don Gagne committed
9 10 11 12
 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.
dogmaphobic's avatar
dogmaphobic committed
13

Don Gagne's avatar
Don Gagne committed
14 15 16 17
 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.
dogmaphobic's avatar
dogmaphobic committed
18

Don Gagne's avatar
Don Gagne committed
19 20
 You should have received a copy of the GNU General Public License
 along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.
dogmaphobic's avatar
dogmaphobic committed
21

Don Gagne's avatar
Don Gagne committed
22 23 24 25 26 27
 ======================================================================*/

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

#include "PX4FirmwarePlugin.h"
28
#include "PX4ParameterMetaData.h"
29
#include "QGCApplication.h"
30
#include "AutoPilotPlugins/PX4/PX4AutoPilotPlugin.h"    // FIXME: Hack
Don Gagne's avatar
Don Gagne committed
31 32 33 34 35 36 37 38 39 40 41

#include <QDebug>

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,
    PX4_CUSTOM_MAIN_MODE_STABILIZED,
42
    PX4_CUSTOM_MAIN_MODE_RATTITUDE
Don Gagne's avatar
Don Gagne committed
43 44 45 46 47 48 49 50 51
};

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,
Jimmy Johnson's avatar
Jimmy Johnson committed
52 53
    PX4_CUSTOM_SUB_MODE_AUTO_RTGS,
    PX4_CUSTOM_SUB_MODE_AUTO_FOLLOW_ME
Don Gagne's avatar
Don Gagne committed
54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72
};

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

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
};

Don Gagne's avatar
Don Gagne committed
73 74 75 76 77 78 79 80 81
const char* PX4FirmwarePlugin::manualFlightMode =       "Manual";
const char* PX4FirmwarePlugin::acroFlightMode =         "Acro";
const char* PX4FirmwarePlugin::stabilizedFlightMode =   "Stabilized";
const char* PX4FirmwarePlugin::rattitudeFlightMode =    "Rattitude";
const char* PX4FirmwarePlugin::altCtlFlightMode =       "Altitude Control";
const char* PX4FirmwarePlugin::posCtlFlightMode =       "Position Control";
const char* PX4FirmwarePlugin::offboardFlightMode =     "Offboard Control";
const char* PX4FirmwarePlugin::readyFlightMode =        "Ready";
const char* PX4FirmwarePlugin::takeoffFlightMode =      "Takeoff";
82
const char* PX4FirmwarePlugin::pauseFlightMode =        "Hold";
Don Gagne's avatar
Don Gagne committed
83 84 85 86
const char* PX4FirmwarePlugin::missionFlightMode =      "Mission";
const char* PX4FirmwarePlugin::rtlFlightMode =          "Return To Land";
const char* PX4FirmwarePlugin::landingFlightMode =      "Landing";
const char* PX4FirmwarePlugin::rtgsFlightMode =         "Return, Link Loss";
Jimmy Johnson's avatar
Jimmy Johnson committed
87
const char* PX4FirmwarePlugin::followMeFlightMode =     "Follow Me";
Don Gagne's avatar
Don Gagne committed
88

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

Don Gagne's avatar
Don Gagne committed
91
static const struct Modes2Name rgModes2Name[] = {
Don Gagne's avatar
Don Gagne committed
92 93 94 95 96 97 98 99 100 101 102 103 104 105
    { PX4_CUSTOM_MAIN_MODE_MANUAL,      0,                                  PX4FirmwarePlugin::manualFlightMode,        true },
    { PX4_CUSTOM_MAIN_MODE_ACRO,        0,                                  PX4FirmwarePlugin::acroFlightMode,          true },
    { PX4_CUSTOM_MAIN_MODE_STABILIZED,  0,                                  PX4FirmwarePlugin::stabilizedFlightMode,    true },
    { PX4_CUSTOM_MAIN_MODE_RATTITUDE,   0,                                  PX4FirmwarePlugin::rattitudeFlightMode,     true },
    { PX4_CUSTOM_MAIN_MODE_ALTCTL,      0,                                  PX4FirmwarePlugin::altCtlFlightMode,        true },
    { PX4_CUSTOM_MAIN_MODE_POSCTL,      0,                                  PX4FirmwarePlugin::posCtlFlightMode,        true },
    { PX4_CUSTOM_MAIN_MODE_OFFBOARD,    0,                                  PX4FirmwarePlugin::offboardFlightMode,      true },
    { PX4_CUSTOM_MAIN_MODE_AUTO,        PX4_CUSTOM_SUB_MODE_AUTO_READY,     PX4FirmwarePlugin::readyFlightMode,         false },
    { PX4_CUSTOM_MAIN_MODE_AUTO,        PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF,   PX4FirmwarePlugin::takeoffFlightMode,       false },
    { PX4_CUSTOM_MAIN_MODE_AUTO,        PX4_CUSTOM_SUB_MODE_AUTO_LOITER,    PX4FirmwarePlugin::pauseFlightMode,         true },
    { PX4_CUSTOM_MAIN_MODE_AUTO,        PX4_CUSTOM_SUB_MODE_AUTO_MISSION,   PX4FirmwarePlugin::missionFlightMode,       true },
    { PX4_CUSTOM_MAIN_MODE_AUTO,        PX4_CUSTOM_SUB_MODE_AUTO_RTL,       PX4FirmwarePlugin::rtlFlightMode,           true },
    { PX4_CUSTOM_MAIN_MODE_AUTO,        PX4_CUSTOM_SUB_MODE_AUTO_LAND,      PX4FirmwarePlugin::landingFlightMode,       false },
    { PX4_CUSTOM_MAIN_MODE_AUTO,        PX4_CUSTOM_SUB_MODE_AUTO_RTGS,      PX4FirmwarePlugin::rtgsFlightMode,          false },
Jimmy Johnson's avatar
Jimmy Johnson committed
106
    { PX4_CUSTOM_MAIN_MODE_AUTO,        PX4_CUSTOM_SUB_MODE_AUTO_FOLLOW_ME, PX4FirmwarePlugin::followMeFlightMode,      true },
Don Gagne's avatar
Don Gagne committed
107 108 109 110 111
};

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

Don Gagne's avatar
Don Gagne committed
113 114 115 116 117 118
    return QList<VehicleComponent*>();
}

QStringList PX4FirmwarePlugin::flightModes(void)
{
    QStringList flightModes;
dogmaphobic's avatar
dogmaphobic committed
119

Don Gagne's avatar
Don Gagne committed
120
    // FIXME: fixed-wing/multi-rotor differences?
dogmaphobic's avatar
dogmaphobic committed
121

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

Don Gagne's avatar
Don Gagne committed
125 126 127 128
        if (pModes2Name->canBeSet) {
            flightModes += pModes2Name->name;
        }
    }
dogmaphobic's avatar
dogmaphobic committed
129

Don Gagne's avatar
Don Gagne committed
130 131 132
    return flightModes;
}

Don Gagne's avatar
Don Gagne committed
133
QString PX4FirmwarePlugin::flightMode(uint8_t base_mode, uint32_t custom_mode) const
Don Gagne's avatar
Don Gagne committed
134 135
{
    QString flightMode = "Unknown";
dogmaphobic's avatar
dogmaphobic committed
136

Don Gagne's avatar
Don Gagne committed
137 138 139
    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
140

Don Gagne's avatar
Don Gagne committed
141 142 143 144 145
        // FIXME: fixed-wing/multi-rotor differences?

        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
146

Don Gagne's avatar
Don Gagne committed
147 148 149 150 151 152
            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
153

Don Gagne's avatar
Don Gagne committed
154 155 156 157 158 159
        if (!found) {
            qWarning() << "Unknown flight mode" << custom_mode;
        }
    } else {
        qWarning() << "PX4 Flight Stack flight mode without custom mode enabled?";
    }
dogmaphobic's avatar
dogmaphobic committed
160

Don Gagne's avatar
Don Gagne committed
161 162 163 164 165 166 167
    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
168

Don Gagne's avatar
Don Gagne committed
169 170 171
    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
172

Don Gagne's avatar
Don Gagne committed
173 174 175 176 177 178
        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
179

Don Gagne's avatar
Don Gagne committed
180 181
            *base_mode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
            *custom_mode = px4_mode.data;
dogmaphobic's avatar
dogmaphobic committed
182

Don Gagne's avatar
Don Gagne committed
183 184 185 186
            found = true;
            break;
        }
    }
dogmaphobic's avatar
dogmaphobic committed
187

Don Gagne's avatar
Don Gagne committed
188 189 190
    if (!found) {
        qWarning() << "Unknown flight Mode" << flightMode;
    }
dogmaphobic's avatar
dogmaphobic committed
191

Don Gagne's avatar
Don Gagne committed
192 193
    return found;
}
194 195 196

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

Don Gagne's avatar
Don Gagne committed
200 201
bool PX4FirmwarePlugin::isCapable(FirmwareCapabilities capabilities)
{
202
    return (capabilities & (MavCmdPreflightStorageCapability | GuidedModeCapability | SetFlightModeCapability | PauseVehicleCapability)) == capabilities;
Don Gagne's avatar
Don Gagne committed
203
}
204 205 206 207

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

209 210
    // PX4 Flight Stack doesn't need to do any extra work
}
211 212 213 214 215 216 217

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

219
void PX4FirmwarePlugin::addMetaDataToFact(QObject* parameterMetaData, Fact* fact, MAV_TYPE vehicleType)
220
{
221 222 223 224 225 226 227
    PX4ParameterMetaData* px4MetaData = qobject_cast<PX4ParameterMetaData*>(parameterMetaData);

    if (px4MetaData) {
        px4MetaData->addMetaDataToFact(fact, vehicleType);
    } else {
        qWarning() << "Internal error: pointer passed to PX4FirmwarePlugin::addMetaDataToFact not PX4ParameterMetaData";
    }
228
}
Don Gagne's avatar
Don Gagne committed
229 230 231 232 233

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

234
    list << MAV_CMD_NAV_WAYPOINT
235
         << MAV_CMD_NAV_LOITER_UNLIM << MAV_CMD_NAV_LOITER_TIME
Don Gagne's avatar
Don Gagne committed
236
         << MAV_CMD_NAV_LAND << MAV_CMD_NAV_TAKEOFF
237
         << MAV_CMD_DO_JUMP
Don Gagne's avatar
Don Gagne committed
238
         << MAV_CMD_DO_VTOL_TRANSITION << MAV_CMD_NAV_VTOL_TAKEOFF << MAV_CMD_NAV_VTOL_LAND
239
         << MAV_CMD_DO_DIGICAM_CONTROL
240
         << MAV_CMD_DO_SET_CAM_TRIGG_DIST
241
         << MAV_CMD_DO_SET_SERVO
Don Gagne's avatar
Don Gagne committed
242
         << MAV_CMD_DO_CHANGE_SPEED
Don Gagne's avatar
Don Gagne committed
243
         << MAV_CMD_NAV_PATHPLANNING;
Jimmy Johnson's avatar
Jimmy Johnson committed
244

Don Gagne's avatar
Don Gagne committed
245 246
    return list;
}
247 248 249 250 251 252 253 254

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");
}
255 256 257 258 259 260 261

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

void PX4FirmwarePlugin::pauseVehicle(Vehicle* vehicle)
{
265
    // kick it into hold mode
Don Gagne's avatar
Don Gagne committed
266
    vehicle->setFlightMode(pauseFlightMode);
267 268 269 270

    // then tell it to loiter at the current position
    // above the takeoff (home) position
    mavlink_message_t msg;
Lorenz Meier's avatar
Lorenz Meier committed
271
    mavlink_command_long_t cmd;
272 273 274 275 276 277 278 279 280 281 282 283 284 285 286 287 288

    cmd.command = (uint16_t)MAV_CMD_DO_REPOSITION;
    cmd.confirmation = 0;
    cmd.param1 = -1.0f;
    cmd.param2 = 0.0;
    cmd.param3 = 0.0f;
    cmd.param4 = NAN;
    cmd.param5 = NAN;
    cmd.param6 = NAN;
    cmd.param7 = NAN;
    cmd.target_system = vehicle->id();
    cmd.target_component = 0;

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

    vehicle->sendMessage(msg);
Don Gagne's avatar
Don Gagne committed
289
}
290 291 292 293 294 295 296 297 298 299 300 301 302 303 304 305 306 307 308

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;
    }

309 310
    // tell the system first to take off in its internal,
    // airframe specific takeoff action
311 312
    vehicle->setFlightMode(takeoffFlightMode);

313 314
    // then tell it to loiter at the user-selected location
    // above the takeoff (home) position
315
    mavlink_message_t msg;
Lorenz Meier's avatar
Lorenz Meier committed
316
    mavlink_command_long_t cmd;
317

318
    cmd.command = (uint16_t)MAV_CMD_DO_REPOSITION;
319
    cmd.confirmation = 0;
320 321
    cmd.param1 = -1.0f;
    cmd.param2 = 0.0;
322
    cmd.param3 = 0.0f;
323 324 325 326
    cmd.param4 = NAN;
    cmd.param5 = NAN;
    cmd.param6 = NAN;
    cmd.param7 = vehicle->altitudeAMSL()->rawValue().toDouble() + altitudeRel;
327 328 329 330
    cmd.target_system = vehicle->id();
    cmd.target_component = 0;

    MAVLinkProtocol* mavlink = qgcApp()->toolbox()->mavlinkProtocol();
331
    mavlink_msg_command_long_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &cmd);
332 333 334 335 336 337 338 339 340 341 342 343

    vehicle->sendMessage(msg);
}

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
344
    mavlink_command_long_t cmd;
345 346 347 348 349 350 351 352 353 354 355 356 357 358 359 360 361 362 363 364 365 366 367 368 369 370 371

    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();
    cmd.target_component = 0;

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

    vehicle->sendMessage(msg);
}

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
372
    mavlink_command_long_t cmd;
373 374 375 376 377 378 379 380 381 382 383 384 385 386 387 388 389 390 391 392 393 394

    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();
    cmd.target_component = 0;

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

    vehicle->sendMessage(msg);
}

void PX4FirmwarePlugin::setGuidedMode(Vehicle* vehicle, bool guidedMode)
{
    if (guidedMode) {
395
        vehicle->setFlightMode(pauseFlightMode);
396 397 398 399
    } else {
        pauseVehicle(vehicle);
    }
}
400 401 402 403 404 405

bool PX4FirmwarePlugin::isGuidedMode(const Vehicle* vehicle) const
{
    // Not supported by generic vehicle
    return (vehicle->flightMode() == pauseFlightMode);
}