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

#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,
40
    PX4_CUSTOM_MAIN_MODE_RATTITUDE
Don Gagne's avatar
Don Gagne committed
41 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 72 73 74 75
};

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
// FIXME: Doens't handle fixed-wing/multi-rotor name differences
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 },
76
    { PX4_CUSTOM_MAIN_MODE_RATTITUDE,   0,                                  "Rattitude",        true },
dogmaphobic's avatar
dogmaphobic committed
77 78 79 80 81
    { 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 },
    { 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,   "Taking Off",       false },
Don Gagne's avatar
Don Gagne committed
82 83
    { PX4_CUSTOM_MAIN_MODE_AUTO,        PX4_CUSTOM_SUB_MODE_AUTO_LOITER,    "Loiter",           true },
    { PX4_CUSTOM_MAIN_MODE_AUTO,        PX4_CUSTOM_SUB_MODE_AUTO_MISSION,   "Mission",          true },
dogmaphobic's avatar
dogmaphobic committed
84
    { PX4_CUSTOM_MAIN_MODE_AUTO,        PX4_CUSTOM_SUB_MODE_AUTO_RTL,       "Return To Land",   true },
Don Gagne's avatar
Don Gagne committed
85 86 87 88 89 90 91
    { PX4_CUSTOM_MAIN_MODE_AUTO,        PX4_CUSTOM_SUB_MODE_AUTO_LAND,      "Landing",          false },
};


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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

void PX4FirmwarePlugin::adjustMavlinkMessage(mavlink_message_t* message)
{
    Q_UNUSED(message);
dogmaphobic's avatar
dogmaphobic committed
183

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

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

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

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

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

206
void PX4FirmwarePlugin::addMetaDataToFact(Fact* fact, MAV_TYPE vehicleType)
207
{
208
    _parameterMetaData.addMetaDataToFact(fact, vehicleType);
209
}
Don Gagne's avatar
Don Gagne committed
210 211 212 213 214

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

215 216
    list << MAV_CMD_NAV_WAYPOINT
         << MAV_CMD_NAV_LOITER_UNLIM << MAV_CMD_NAV_LOITER_TURNS << MAV_CMD_NAV_LOITER_TIME
217
         << MAV_CMD_NAV_RETURN_TO_LAUNCH << MAV_CMD_NAV_LAND << MAV_CMD_NAV_TAKEOFF
218
         << MAV_CMD_NAV_ROI
219 220
         << MAV_CMD_DO_JUMP
         << MAV_CMD_CONDITION_DELAY;
Don Gagne's avatar
Don Gagne committed
221 222
    return list;
}