PX4FirmwarePlugin.cc 8.3 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 "AutoPilotPlugins/PX4/PX4AutoPilotPlugin.h"    // FIXME: Hack
Don Gagne's avatar
Don Gagne committed
30 31 32 33 34 35 36 37 38 39 40

#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,
41
    PX4_CUSTOM_MAIN_MODE_RATTITUDE
Don Gagne's avatar
Don Gagne committed
42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71
};

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

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

/// Tranlates from PX4 custom modes to flight mode names
72

Don Gagne's avatar
Don Gagne committed
73 74 75 76
static const struct Modes2Name rgModes2Name[] = {
    { PX4_CUSTOM_MAIN_MODE_MANUAL,      0,                                  "Manual",           true },
    { PX4_CUSTOM_MAIN_MODE_ACRO,        0,                                  "Acro",             true },
    { PX4_CUSTOM_MAIN_MODE_STABILIZED,  0,                                  "Stabilized",       true },
77
    { PX4_CUSTOM_MAIN_MODE_RATTITUDE,   0,                                  "Rattitude",        true },
dogmaphobic's avatar
dogmaphobic committed
78 79 80
    { PX4_CUSTOM_MAIN_MODE_ALTCTL,      0,                                  "Altitude Control", true },
    { PX4_CUSTOM_MAIN_MODE_POSCTL,      0,                                  "Position Control", true },
    { PX4_CUSTOM_MAIN_MODE_OFFBOARD,    0,                                  "Offboard Control", true },
81 82 83 84 85 86
    { PX4_CUSTOM_MAIN_MODE_AUTO,        PX4_CUSTOM_SUB_MODE_AUTO_READY,     "Auto: Ready",       false },
    { PX4_CUSTOM_MAIN_MODE_AUTO,        PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF,   "Auto: Takeoff",       false },
    { PX4_CUSTOM_MAIN_MODE_AUTO,        PX4_CUSTOM_SUB_MODE_AUTO_LOITER,    "Auto: Pause",           true },
    { PX4_CUSTOM_MAIN_MODE_AUTO,        PX4_CUSTOM_SUB_MODE_AUTO_MISSION,   "Auto: Mission",          true },
    { PX4_CUSTOM_MAIN_MODE_AUTO,        PX4_CUSTOM_SUB_MODE_AUTO_RTL,       "Auto: Return To Land",   true },
    { PX4_CUSTOM_MAIN_MODE_AUTO,        PX4_CUSTOM_SUB_MODE_AUTO_LAND,      "Auto: Landing",          false },
Don Gagne's avatar
Don Gagne committed
87 88 89 90 91 92
};


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

Don Gagne's avatar
Don Gagne committed
94 95 96 97 98 99
    return QList<VehicleComponent*>();
}

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

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

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

Don Gagne's avatar
Don Gagne committed
106 107 108 109
        if (pModes2Name->canBeSet) {
            flightModes += pModes2Name->name;
        }
    }
dogmaphobic's avatar
dogmaphobic committed
110

Don Gagne's avatar
Don Gagne committed
111 112 113 114 115 116
    return flightModes;
}

QString PX4FirmwarePlugin::flightMode(uint8_t base_mode, uint32_t custom_mode)
{
    QString flightMode = "Unknown";
dogmaphobic's avatar
dogmaphobic committed
117

Don Gagne's avatar
Don Gagne committed
118 119 120
    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
121

Don Gagne's avatar
Don Gagne committed
122 123 124 125 126
        // 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
127

Don Gagne's avatar
Don Gagne committed
128 129 130 131 132 133
            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
134

Don Gagne's avatar
Don Gagne committed
135 136 137 138 139 140
        if (!found) {
            qWarning() << "Unknown flight mode" << custom_mode;
        }
    } else {
        qWarning() << "PX4 Flight Stack flight mode without custom mode enabled?";
    }
dogmaphobic's avatar
dogmaphobic committed
141

Don Gagne's avatar
Don Gagne committed
142 143 144 145 146 147 148
    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
149

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

Don Gagne's avatar
Don Gagne committed
154 155 156 157 158 159
        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
160

Don Gagne's avatar
Don Gagne committed
161 162
            *base_mode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
            *custom_mode = px4_mode.data;
dogmaphobic's avatar
dogmaphobic committed
163

Don Gagne's avatar
Don Gagne committed
164 165 166 167
            found = true;
            break;
        }
    }
dogmaphobic's avatar
dogmaphobic committed
168

Don Gagne's avatar
Don Gagne committed
169 170 171
    if (!found) {
        qWarning() << "Unknown flight Mode" << flightMode;
    }
dogmaphobic's avatar
dogmaphobic committed
172

Don Gagne's avatar
Don Gagne committed
173 174
    return found;
}
175 176 177

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

181
void PX4FirmwarePlugin::adjustMavlinkMessage(Vehicle* vehicle, mavlink_message_t* message)
Don Gagne's avatar
Don Gagne committed
182
{
183
    Q_UNUSED(vehicle);
Don Gagne's avatar
Don Gagne committed
184
    Q_UNUSED(message);
dogmaphobic's avatar
dogmaphobic committed
185

Don Gagne's avatar
Don Gagne committed
186 187
    // PX4 Flight Stack plugin does no message adjustment
}
Don Gagne's avatar
Don Gagne committed
188 189 190

bool PX4FirmwarePlugin::isCapable(FirmwareCapabilities capabilities)
{
191
    return (capabilities & (MavCmdPreflightStorageCapability | SetFlightModeCapability)) == capabilities;
Don Gagne's avatar
Don Gagne committed
192
}
193 194 195 196

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

198 199
    // PX4 Flight Stack doesn't need to do any extra work
}
200 201 202 203 204 205 206

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

208
void PX4FirmwarePlugin::addMetaDataToFact(QObject* parameterMetaData, Fact* fact, MAV_TYPE vehicleType)
209
{
210 211 212 213 214 215 216
    PX4ParameterMetaData* px4MetaData = qobject_cast<PX4ParameterMetaData*>(parameterMetaData);

    if (px4MetaData) {
        px4MetaData->addMetaDataToFact(fact, vehicleType);
    } else {
        qWarning() << "Internal error: pointer passed to PX4FirmwarePlugin::addMetaDataToFact not PX4ParameterMetaData";
    }
217
}
Don Gagne's avatar
Don Gagne committed
218 219 220 221 222

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

223
    list << MAV_CMD_NAV_WAYPOINT
224
         << MAV_CMD_NAV_LOITER_UNLIM << MAV_CMD_NAV_LOITER_TIME
Don Gagne's avatar
Don Gagne committed
225
         << MAV_CMD_NAV_LAND << MAV_CMD_NAV_TAKEOFF
226
         << MAV_CMD_DO_JUMP
Don Gagne's avatar
Don Gagne committed
227
         << MAV_CMD_DO_VTOL_TRANSITION << MAV_CMD_NAV_VTOL_TAKEOFF << MAV_CMD_NAV_VTOL_LAND
228
         << MAV_CMD_DO_DIGICAM_CONTROL
229
         << MAV_CMD_DO_SET_CAM_TRIGG_DIST
230
         << MAV_CMD_DO_SET_SERVO
Don Gagne's avatar
Don Gagne committed
231
         << MAV_CMD_DO_CHANGE_SPEED
Don Gagne's avatar
Don Gagne committed
232
         << MAV_CMD_NAV_PATHPLANNING;
Don Gagne's avatar
Don Gagne committed
233 234
    return list;
}
235 236 237 238 239 240 241 242

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");
}
243 244 245 246 247 248 249

QObject* PX4FirmwarePlugin::loadParameterMetaData(const QString& metaDataFile)
{
    PX4ParameterMetaData* metaData = new PX4ParameterMetaData;
    metaData->loadParameterFactMetaDataFile(metaDataFile);
    return metaData;
}