ArduCopterFirmwarePlugin.cc 13.5 KB
Newer Older
1 2 3 4 5 6 7 8 9
/****************************************************************************
 *
 *   (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.
 *
 ****************************************************************************/

10 11 12 13 14

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

#include "ArduCopterFirmwarePlugin.h"
Don Gagne's avatar
Don Gagne committed
15 16
#include "QGCApplication.h"
#include "MissionManager.h"
17
#include "ParameterManager.h"
18

19 20 21
bool ArduCopterFirmwarePlugin::_remapParamNameIntialized = false;
FirmwarePlugin::remapParamNameMajorVersionMap_t ArduCopterFirmwarePlugin::_remapParamName;

Don Gagne's avatar
Don Gagne committed
22 23
APMCopterMode::APMCopterMode(uint32_t mode, bool settable) :
    APMCustomMode(mode, settable)
24
{
25 26 27
    QMap<uint32_t,QString> enumToString;
    enumToString.insert(STABILIZE, "Stabilize");
    enumToString.insert(ACRO,      "Acro");
28
    enumToString.insert(ALT_HOLD,  "Altitude Hold");
29 30 31 32 33 34 35 36 37 38
    enumToString.insert(AUTO,      "Auto");
    enumToString.insert(GUIDED,    "Guided");
    enumToString.insert(LOITER,    "Loiter");
    enumToString.insert(RTL,       "RTL");
    enumToString.insert(CIRCLE,    "Circle");
    enumToString.insert(LAND,      "Land");
    enumToString.insert(DRIFT,     "Drift");
    enumToString.insert(SPORT,     "Sport");
    enumToString.insert(FLIP,      "Flip");
    enumToString.insert(AUTOTUNE,  "Autotune");
39
    enumToString.insert(POS_HOLD,  "Position Hold");
40
    enumToString.insert(BRAKE,     "Brake");
41 42 43
    enumToString.insert(THROW,     "Throw");
    enumToString.insert(AVOID_ADSB,"Avoid ADSB");
    enumToString.insert(GUIDED_NOGPS,"Guided No GPS");
44

45
    setEnumToStringMapping(enumToString);
46 47
}

48
ArduCopterFirmwarePlugin::ArduCopterFirmwarePlugin(void)
49
{
50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65
    QList<APMCustomMode> supportedFlightModes;
    supportedFlightModes << APMCopterMode(APMCopterMode::STABILIZE ,true);
    supportedFlightModes << APMCopterMode(APMCopterMode::ACRO      ,true);
    supportedFlightModes << APMCopterMode(APMCopterMode::ALT_HOLD  ,true);
    supportedFlightModes << APMCopterMode(APMCopterMode::AUTO      ,true);
    supportedFlightModes << APMCopterMode(APMCopterMode::GUIDED    ,true);
    supportedFlightModes << APMCopterMode(APMCopterMode::LOITER    ,true);
    supportedFlightModes << APMCopterMode(APMCopterMode::RTL       ,true);
    supportedFlightModes << APMCopterMode(APMCopterMode::CIRCLE    ,true);
    supportedFlightModes << APMCopterMode(APMCopterMode::LAND      ,true);
    supportedFlightModes << APMCopterMode(APMCopterMode::DRIFT     ,true);
    supportedFlightModes << APMCopterMode(APMCopterMode::SPORT     ,true);
    supportedFlightModes << APMCopterMode(APMCopterMode::FLIP      ,true);
    supportedFlightModes << APMCopterMode(APMCopterMode::AUTOTUNE  ,true);
    supportedFlightModes << APMCopterMode(APMCopterMode::POS_HOLD  ,true);
    supportedFlightModes << APMCopterMode(APMCopterMode::BRAKE     ,true);
66 67 68 69 70
    supportedFlightModes << APMCopterMode(APMCopterMode::THROW     ,true);
    supportedFlightModes << APMCopterMode(APMCopterMode::AVOID_ADSB,true);
    supportedFlightModes << APMCopterMode(APMCopterMode::GUIDED_NOGPS,true);


71
    setSupportedModes(supportedFlightModes);
72 73

    if (!_remapParamNameIntialized) {
74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110
        FirmwarePlugin::remapParamNameMap_t& remapV3_4 = _remapParamName[3][4];

        remapV3_4["ATC_ANG_RLL_P"] =    QStringLiteral("STB_RLL_P");
        remapV3_4["ATC_ANG_PIT_P"] =    QStringLiteral("STB_PIT_P");
        remapV3_4["ATC_ANG_YAW_P"] =    QStringLiteral("STB_YAW_P");

        remapV3_4["ATC_RAT_RLL_P"] =    QStringLiteral("RATE_RLL_P");
        remapV3_4["ATC_RAT_RLL_I"] =    QStringLiteral("RATE_RLL_I");
        remapV3_4["ATC_RAT_RLL_IMAX"] = QStringLiteral("RATE_RLL_IMAX");
        remapV3_4["ATC_RAT_RLL_D"] =    QStringLiteral("RATE_RLL_D");
        remapV3_4["ATC_RAT_RLL_FILT"] = QStringLiteral("RATE_RLL_FILT_HZ");

        remapV3_4["ATC_RAT_PIT_P"] =    QStringLiteral("RATE_PIT_P");
        remapV3_4["ATC_RAT_PIT_I"] =    QStringLiteral("RATE_PIT_I");
        remapV3_4["ATC_RAT_PIT_IMAX"] = QStringLiteral("RATE_PIT_IMAX");
        remapV3_4["ATC_RAT_PIT_D"] =    QStringLiteral("RATE_PIT_D");
        remapV3_4["ATC_RAT_PIT_FILT"] = QStringLiteral("RATE_PIT_FILT_HZ");

        remapV3_4["ATC_RAT_YAW_P"] =    QStringLiteral("RATE_YAW_P");
        remapV3_4["ATC_RAT_YAW_I"] =    QStringLiteral("RATE_YAW_I");
        remapV3_4["ATC_RAT_YAW_IMAX"] = QStringLiteral("RATE_YAW_IMAX");
        remapV3_4["ATC_RAT_YAW_D"] =    QStringLiteral("RATE_YAW_D");
        remapV3_4["ATC_RAT_YAW_FILT"] = QStringLiteral("RATE_YAW_FILT_HZ");

        FirmwarePlugin::remapParamNameMap_t& remapV3_5 = _remapParamName[3][5];

        remapV3_5["SERVO5_FUNCTION"] = QStringLiteral("RC5_FUNCTION");
        remapV3_5["SERVO6_FUNCTION"] = QStringLiteral("RC6_FUNCTION");
        remapV3_5["SERVO7_FUNCTION"] = QStringLiteral("RC7_FUNCTION");
        remapV3_5["SERVO8_FUNCTION"] = QStringLiteral("RC8_FUNCTION");
        remapV3_5["SERVO9_FUNCTION"] = QStringLiteral("RC9_FUNCTION");
        remapV3_5["SERVO10_FUNCTION"] = QStringLiteral("RC10_FUNCTION");
        remapV3_5["SERVO11_FUNCTION"] = QStringLiteral("RC11_FUNCTION");
        remapV3_5["SERVO12_FUNCTION"] = QStringLiteral("RC12_FUNCTION");
        remapV3_5["SERVO13_FUNCTION"] = QStringLiteral("RC13_FUNCTION");
        remapV3_5["SERVO14_FUNCTION"] = QStringLiteral("RC14_FUNCTION");

Don Gagne's avatar
Don Gagne committed
111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143
        remapV3_5["SERVO5_MIN"] = QStringLiteral("RC5_MIN");
        remapV3_5["SERVO6_MIN"] = QStringLiteral("RC6_MIN");
        remapV3_5["SERVO7_MIN"] = QStringLiteral("RC7_MIN");
        remapV3_5["SERVO8_MIN"] = QStringLiteral("RC8_MIN");
        remapV3_5["SERVO9_MIN"] = QStringLiteral("RC9_MIN");
        remapV3_5["SERVO10_MIN"] = QStringLiteral("RC10_MIN");
        remapV3_5["SERVO11_MIN"] = QStringLiteral("RC11_MIN");
        remapV3_5["SERVO12_MIN"] = QStringLiteral("RC12_MIN");
        remapV3_5["SERVO13_MIN"] = QStringLiteral("RC13_MIN");
        remapV3_5["SERVO14_MIN"] = QStringLiteral("RC14_MIN");

        remapV3_5["SERVO5_MAX"] = QStringLiteral("RC5_MAX");
        remapV3_5["SERVO6_MAX"] = QStringLiteral("RC6_MAX");
        remapV3_5["SERVO7_MAX"] = QStringLiteral("RC7_MAX");
        remapV3_5["SERVO8_MAX"] = QStringLiteral("RC8_MAX");
        remapV3_5["SERVO9_MAX"] = QStringLiteral("RC9_MAX");
        remapV3_5["SERVO10_MAX"] = QStringLiteral("RC10_MAX");
        remapV3_5["SERVO11_MAX"] = QStringLiteral("RC11_MAX");
        remapV3_5["SERVO12_MAX"] = QStringLiteral("RC12_MAX");
        remapV3_5["SERVO13_MAX"] = QStringLiteral("RC13_MAX");
        remapV3_5["SERVO14_MAX"] = QStringLiteral("RC14_MAX");

        remapV3_5["SERVO5_REVERSED"] = QStringLiteral("RC5_REVERSED");
        remapV3_5["SERVO6_REVERSED"] = QStringLiteral("RC6_REVERSED");
        remapV3_5["SERVO7_REVERSED"] = QStringLiteral("RC7_REVERSED");
        remapV3_5["SERVO8_REVERSED"] = QStringLiteral("RC8_REVERSED");
        remapV3_5["SERVO9_REVERSED"] = QStringLiteral("RC9_REVERSED");
        remapV3_5["SERVO10_REVERSED"] = QStringLiteral("RC10_REVERSED");
        remapV3_5["SERVO11_REVERSED"] = QStringLiteral("RC11_REVERSED");
        remapV3_5["SERVO12_REVERSED"] = QStringLiteral("RC12_REVERSED");
        remapV3_5["SERVO13_REVERSED"] = QStringLiteral("RC13_REVERSED");
        remapV3_5["SERVO14_REVERSED"] = QStringLiteral("RC14_REVERSED");

144
        _remapParamNameIntialized = true;
145 146 147 148 149
    }
}

int ArduCopterFirmwarePlugin::remapParamNameHigestMinorVersionNumber(int majorVersionNumber) const
{
150 151
    // Remapping supports up to 3.5
    return majorVersionNumber == 3 ? 5 : Vehicle::versionNotSetValue;
152
}
Don Gagne's avatar
Don Gagne committed
153

154
bool ArduCopterFirmwarePlugin::isCapable(const Vehicle* vehicle, FirmwareCapabilities capabilities)
Don Gagne's avatar
Don Gagne committed
155
{
156 157 158 159 160
    Q_UNUSED(vehicle);

    uint32_t vehicleCapabilities = SetFlightModeCapability | GuidedModeCapability | PauseVehicleCapability;

    return (capabilities & vehicleCapabilities) == capabilities;
Don Gagne's avatar
Don Gagne committed
161 162 163 164
}

void ArduCopterFirmwarePlugin::guidedModeRTL(Vehicle* vehicle)
{
165
    _setFlightModeAndValidate(vehicle, "RTL");
Don Gagne's avatar
Don Gagne committed
166 167 168 169
}

void ArduCopterFirmwarePlugin::guidedModeLand(Vehicle* vehicle)
{
170
    _setFlightModeAndValidate(vehicle, "Land");
Don Gagne's avatar
Don Gagne committed
171 172
}

173
void ArduCopterFirmwarePlugin::guidedModeTakeoff(Vehicle* vehicle)
174 175 176 177 178
{
    _guidedModeTakeoff(vehicle);
}

bool ArduCopterFirmwarePlugin::_guidedModeTakeoff(Vehicle* vehicle)
Don Gagne's avatar
Don Gagne committed
179
{
180 181
    QString takeoffAltParam("PILOT_TKOFF_ALT");

182 183 184 185
    float takeoffAlt = 0;
    if (vehicle->parameterManager()->parameterExists(FactSystem::defaultComponentId, takeoffAltParam)) {
        Fact* takeoffAltFact = vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, takeoffAltParam);
        takeoffAlt = takeoffAltFact->rawValue().toDouble();
186 187 188 189 190 191 192
    }
    if (takeoffAlt <= 0) {
        takeoffAlt = 2.5;
    } else {
        takeoffAlt /= 100;   // centimeters -> meters
    }

193 194 195 196
    if (!_setFlightModeAndValidate(vehicle, "Guided")) {
        qgcApp()->showMessage(tr("Unable to takeoff: Vehicle failed to change to Guided mode."));
        return false;
    }
Don Gagne's avatar
Don Gagne committed
197

198
    if (!_armVehicleAndValidate(vehicle)) {
199
        qgcApp()->showMessage(tr("Unable to takeoff: Vehicle failed to arm."));
200
        return false;
201 202
    }

203 204 205 206
    vehicle->sendMavCommand(vehicle->defaultComponentId(),
                            MAV_CMD_NAV_TAKEOFF,
                            true, // show error
                            0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f,
207
                            takeoffAlt);
208 209

    return true;
Don Gagne's avatar
Don Gagne committed
210 211 212 213 214 215 216 217 218 219 220 221 222 223
}

void ArduCopterFirmwarePlugin::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;
    }

    QGeoCoordinate coordWithAltitude = gotoCoord;
    coordWithAltitude.setAltitude(vehicle->altitudeRelative()->rawValue().toDouble());
    vehicle->missionManager()->writeArduPilotGuidedMissionItem(coordWithAltitude, false /* altChangeOnly */);
}

224
void ArduCopterFirmwarePlugin::guidedModeChangeAltitude(Vehicle* vehicle, double altitudeChange)
Don Gagne's avatar
Don Gagne committed
225 226 227 228 229 230
{
    if (qIsNaN(vehicle->altitudeRelative()->rawValue().toDouble())) {
        qgcApp()->showMessage(QStringLiteral("Unable to change altitude, vehicle altitude not known."));
        return;
    }

DonLakeFlyer's avatar
DonLakeFlyer committed
231 232
    setGuidedMode(vehicle, true);

Don Gagne's avatar
Don Gagne committed
233 234 235
    mavlink_message_t msg;
    mavlink_set_position_target_local_ned_t cmd;

Don Gagne's avatar
Don Gagne committed
236
    memset(&cmd, 0, sizeof(cmd));
Don Gagne's avatar
Don Gagne committed
237 238

    cmd.target_system = vehicle->id();
239
    cmd.target_component = vehicle->defaultComponentId();
Don Gagne's avatar
Don Gagne committed
240 241 242 243
    cmd.coordinate_frame = MAV_FRAME_LOCAL_OFFSET_NED;
    cmd.type_mask = 0xFFF8; // Only x/y/z valid
    cmd.x = 0.0f;
    cmd.y = 0.0f;
244
    cmd.z = -(altitudeChange);
Don Gagne's avatar
Don Gagne committed
245 246

    MAVLinkProtocol* mavlink = qgcApp()->toolbox()->mavlinkProtocol();
247 248 249 250 251
    mavlink_msg_set_position_target_local_ned_encode_chan(mavlink->getSystemId(),
                                                          mavlink->getComponentId(),
                                                          vehicle->priorityLink()->mavlinkChannel(),
                                                          &msg,
                                                          &cmd);
Don Gagne's avatar
Don Gagne committed
252

253
    vehicle->sendMessageOnLink(vehicle->priorityLink(), msg);
Don Gagne's avatar
Don Gagne committed
254 255 256 257
}

void ArduCopterFirmwarePlugin::pauseVehicle(Vehicle* vehicle)
{
258
    _setFlightModeAndValidate(vehicle, "Brake");
Don Gagne's avatar
Don Gagne committed
259 260 261 262 263
}

void ArduCopterFirmwarePlugin::setGuidedMode(Vehicle* vehicle, bool guidedMode)
{
    if (guidedMode) {
264
        _setFlightModeAndValidate(vehicle, "Guided");
Don Gagne's avatar
Don Gagne committed
265 266 267 268
    } else {
        pauseVehicle(vehicle);
    }
}
Don Gagne's avatar
Don Gagne committed
269 270 271 272 273 274 275 276 277

bool ArduCopterFirmwarePlugin::multiRotorCoaxialMotors(Vehicle* vehicle)
{
    Q_UNUSED(vehicle);
    return _coaxialMotors;
}

bool ArduCopterFirmwarePlugin::multiRotorXConfig(Vehicle* vehicle)
{
278
    return vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, "FRAME")->rawValue().toInt() != 0;
Don Gagne's avatar
Don Gagne committed
279
}
280 281 282 283 284 285

QString ArduCopterFirmwarePlugin::geoFenceRadiusParam(Vehicle* vehicle)
{
    Q_UNUSED(vehicle);
    return QStringLiteral("FENCE_RADIUS");
}
286

287 288
bool ArduCopterFirmwarePlugin::vehicleYawsToNextWaypointInMission(const Vehicle* vehicle) const
{
289 290 291 292 293 294 295
    if (vehicle->isOfflineEditingVehicle()) {
        return FirmwarePlugin::vehicleYawsToNextWaypointInMission(vehicle);
    } else {
        if (vehicle->multiRotor() && vehicle->parameterManager()->parameterExists(FactSystem::defaultComponentId, QStringLiteral("WP_YAW_BEHAVIOR"))) {
            Fact* yawMode = vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, QStringLiteral("WP_YAW_BEHAVIOR"));
            return yawMode && yawMode->rawValue().toInt() != 0;
        }
296
    }
297
    return true;
298
}
Don Gagne's avatar
Don Gagne committed
299 300 301 302 303 304

void ArduCopterFirmwarePlugin::startMission(Vehicle* vehicle)
{
    double currentAlt = vehicle->altitudeRelative()->rawValue().toDouble();

    if (!vehicle->flying()) {
305 306 307 308 309 310 311 312 313 314 315
        if (_guidedModeTakeoff(vehicle)) {

            // Wait for vehicle to get off ground before switching to auto (10 seconds)
            bool didTakeoff = false;
            for (int i=0; i<100; i++) {
                if (vehicle->altitudeRelative()->rawValue().toDouble() >= currentAlt + 1.0) {
                    didTakeoff = true;
                    break;
                }
                QGC::SLEEP::msleep(100);
                qgcApp()->processEvents(QEventLoop::ExcludeUserInputEvents);
Don Gagne's avatar
Don Gagne committed
316 317
            }

318 319 320 321
            if (!didTakeoff) {
                qgcApp()->showMessage(QStringLiteral("Unable to start mission. Vehicle takeoff failed."));
                return;
            }
Don Gagne's avatar
Don Gagne committed
322 323 324
        }
    }

325 326 327 328
    if (!_setFlightModeAndValidate(vehicle, missionFlightMode())) {
        qgcApp()->showMessage(QStringLiteral("Unable to start mission. Vehicle failed to change to auto."));
        return;
    }
Don Gagne's avatar
Don Gagne committed
329 330
}