ArduSubFirmwarePlugin.cc 14.1 KB
Newer Older
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27
/*=====================================================================

 QGroundControl Open Source Ground Control Station

 (c) 2009 - 2015 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>

 This file is part of the QGROUNDCONTROL project

 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.

 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.

 You should have received a copy of the GNU General Public License
 along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.

 ======================================================================*/

/// @file
///     @author Rustom Jehangir <rusty@bluerobotics.com>

#include "ArduSubFirmwarePlugin.h"
28 29 30

bool ArduSubFirmwarePlugin::_remapParamNameIntialized = false;
FirmwarePlugin::remapParamNameMajorVersionMap_t ArduSubFirmwarePlugin::_remapParamName;
31 32 33 34

APMSubMode::APMSubMode(uint32_t mode, bool settable) :
    APMCustomMode(mode, settable)
{
35 36 37 38 39 40 41 42 43 44 45 46
    setEnumToStringMapping({
        {MANUAL, "Manual"},
        {STABILIZE, "Stabilize"},
        {ACRO, "Acro"},
        {ALT_HOLD,  "Depth Hold"},
        {AUTO, "Auto"},
        {GUIDED, "Guided"},
        {CIRCLE, "Circle"},
        {SURFACE, "Surface"},
        {POSHOLD, "Position Hold"},
        {MOTORDETECTION, "Motor Detection"},
    });
47 48
}

49 50
ArduSubFirmwarePlugin::ArduSubFirmwarePlugin(void):
    _infoFactGroup(this)
51
{
52 53 54 55 56 57 58 59 60 61 62 63
    setSupportedModes({
        APMSubMode(APMSubMode::MANUAL ,true),
        APMSubMode(APMSubMode::STABILIZE ,true),
        APMSubMode(APMSubMode::ACRO ,true),
        APMSubMode(APMSubMode::ALT_HOLD  ,true),
        APMSubMode(APMSubMode::AUTO ,true),
        APMSubMode(APMSubMode::GUIDED ,true),
        APMSubMode(APMSubMode::CIRCLE ,true),
        APMSubMode(APMSubMode::SURFACE ,false),
        APMSubMode(APMSubMode::POSHOLD ,true),
        APMSubMode(APMSubMode::MOTORDETECTION, false),
    });
64 65 66 67 68 69 70 71 72 73 74 75 76 77 78

    if (!_remapParamNameIntialized) {
        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
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 111
        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");

112 113
        remapV3_5["FENCE_ALT_MIN"] = QStringLiteral("FENCE_DEPTH_MAX");

114 115
        FirmwarePlugin::remapParamNameMap_t& remapV3_6 = _remapParamName[3][6];

116 117
        remapV3_6["BATT_ARM_VOLT"] = QStringLiteral("ARMING_MIN_VOLT");
        remapV3_6["BATT2_ARM_VOLT"] = QStringLiteral("ARMING_MIN_VOLT2");
118 119 120 121 122
        remapV3_6["BATT_AMP_PERVLT"] =  QStringLiteral("BATT_AMP_PERVOLT");
        remapV3_6["BATT2_AMP_PERVLT"] = QStringLiteral("BATT2_AMP_PERVOL");
        remapV3_6["BATT_LOW_MAH"] = QStringLiteral("FS_BATT_MAH");
        remapV3_6["BATT_LOW_VOLT"] = QStringLiteral("FS_BATT_VOLTAGE");
        remapV3_6["BATT_FS_LOW_ACT"] = QStringLiteral("FS_BATT_ENABLE");
123 124 125 126 127 128 129 130 131 132 133
        remapV3_6["PSC_POSXY_P"] = QStringLiteral("POS_XY_P");
        remapV3_6["PSC_POSZ_P"] = QStringLiteral("POS_Z_P");
        remapV3_6["PSC_VELXY_P"] = QStringLiteral("VEL_XY_P");
        remapV3_6["PSC_VELXY_I"] = QStringLiteral("VEL_XY_I");
        remapV3_6["PSC_VELXY_IMAX"] = QStringLiteral("VEL_XY_IMAX");
        remapV3_6["PSC_VELZ_P"] = QStringLiteral("VEL_Z_P");
        remapV3_6["PSC_ACCZ_I"] = QStringLiteral("ACCEL_Z_I");
        remapV3_6["PSC_ACCZ_D"] = QStringLiteral("ACCEL_Z_D");
        remapV3_6["PSC_ACCZ_P"] = QStringLiteral("ACCEL_Z_P");
        remapV3_6["PSC_ACCZ_IMAX"] = QStringLiteral("ACCEL_Z_IMAX");
        remapV3_6["PSC_ACCZ_FILT"] = QStringLiteral("ACCEL_Z_FILT");
134

135 136
        _remapParamNameIntialized = true;
    }
137

138
    _nameToFactGroupMap.insert("apmSubInfo", &_infoFactGroup);
139 140 141

    _factRenameMap[QStringLiteral("altitudeRelative")] = QStringLiteral("Depth");
    _factRenameMap[QStringLiteral("flightTime")] = QStringLiteral("Dive Time");
142 143 144
    _factRenameMap[QStringLiteral("altitudeAMSL")] = QStringLiteral("");
    _factRenameMap[QStringLiteral("hobbs")] = QStringLiteral("");
    _factRenameMap[QStringLiteral("airSpeed")] = QStringLiteral("");
145 146 147 148
}

int ArduSubFirmwarePlugin::remapParamNameHigestMinorVersionNumber(int majorVersionNumber) const
{
149 150
    // Remapping supports up to 3.6
    return majorVersionNumber == 3 ? 6 : Vehicle::versionNotSetValue;
151
}
152

153 154 155 156 157 158 159 160 161 162
void ArduSubFirmwarePlugin::initializeStreamRates(Vehicle* vehicle) {
    vehicle->requestDataStream(MAV_DATA_STREAM_RAW_SENSORS,     2);
    vehicle->requestDataStream(MAV_DATA_STREAM_EXTENDED_STATUS, 2);
    vehicle->requestDataStream(MAV_DATA_STREAM_RC_CHANNELS,     2);
    vehicle->requestDataStream(MAV_DATA_STREAM_POSITION,        3);
    vehicle->requestDataStream(MAV_DATA_STREAM_EXTRA1,          20);
    vehicle->requestDataStream(MAV_DATA_STREAM_EXTRA2,          10);
    vehicle->requestDataStream(MAV_DATA_STREAM_EXTRA3,          3);
}

163 164 165 166 167 168
bool ArduSubFirmwarePlugin::isCapable(const Vehicle* vehicle, FirmwareCapabilities capabilities)
{
    Q_UNUSED(vehicle);
    uint32_t available = SetFlightModeCapability | PauseVehicleCapability;
    return (capabilities & available) == capabilities;
}
169

170 171 172 173
bool ArduSubFirmwarePlugin::supportsThrottleModeCenterZero(void)
{
    return false;
}
174

175 176 177 178
bool ArduSubFirmwarePlugin::supportsRadio(void)
{
    return false;
}
179 180 181 182 183

bool ArduSubFirmwarePlugin::supportsJSButton(void)
{
    return true;
}
184

185 186 187 188
bool ArduSubFirmwarePlugin::supportsMotorInterference(void)
{
    return false;
}
189

190 191 192 193 194 195 196 197 198 199 200 201 202 203 204
const QVariantList& ArduSubFirmwarePlugin::toolIndicators(const Vehicle* vehicle)
{
    Q_UNUSED(vehicle);
    //-- Sub specific list of indicators (Enter your modified list here)
    if(_toolIndicators.size() == 0) {
        _toolIndicators = QVariantList({
            QVariant::fromValue(QUrl::fromUserInput("qrc:/toolbar/MessageIndicator.qml")),
            QVariant::fromValue(QUrl::fromUserInput("qrc:/toolbar/BatteryIndicator.qml")),
            QVariant::fromValue(QUrl::fromUserInput("qrc:/toolbar/JoystickIndicator.qml")),
        });
    }
    return _toolIndicators;
}

const QVariantList& ArduSubFirmwarePlugin::modeIndicators(const Vehicle* vehicle)
205 206 207
{
    Q_UNUSED(vehicle);
    //-- Sub specific list of indicators (Enter your modified list here)
208 209 210 211 212
    if(_modeIndicators.size() == 0) {
        _modeIndicators = QVariantList({
            QVariant::fromValue(QUrl::fromUserInput("qrc:/toolbar/ModeIndicator.qml")),
            QVariant::fromValue(QUrl::fromUserInput("qrc:/toolbar/ArmedIndicator.qml")),
        });
213
    }
214
    return _modeIndicators;
215
}
216 217 218 219 220 221 222 223 224

void ArduSubFirmwarePlugin::_handleNamedValueFloat(mavlink_message_t* message)
{
    mavlink_named_value_float_t value;
    mavlink_msg_named_value_float_decode(message, &value);

    QString name = QString(value.name);

    if (name == "CamTilt") {
225
        _infoFactGroup.getFact("cameraTilt")->setRawValue(value.value * 100);
226
    } else if (name == "TetherTrn") {
227
        _infoFactGroup.getFact("tetherTurns")->setRawValue(value.value);
228
    } else if (name == "Lights1") {
229
        _infoFactGroup.getFact("lights1")->setRawValue(value.value * 100);
230
    } else if (name == "Lights2") {
231
        _infoFactGroup.getFact("lights2")->setRawValue(value.value * 100);
232
    } else if (name == "PilotGain") {
233
        _infoFactGroup.getFact("pilotGain")->setRawValue(value.value * 100);
234
    } else if (name == "InputHold") {
235 236 237
        _infoFactGroup.getFact("inputHold")->setRawValue(value.value);
    } else if (name == "RollPitch") {
        _infoFactGroup.getFact("rollPitchToggle")->setRawValue(value.value);
238 239 240 241 242 243 244 245
    }
}

void ArduSubFirmwarePlugin::_handleMavlinkMessage(mavlink_message_t* message)
{
    switch (message->msgid) {
    case (MAVLINK_MSG_ID_NAMED_VALUE_FLOAT):
        _handleNamedValueFloat(message);
246 247 248 249 250
        break;
    case (MAVLINK_MSG_ID_RANGEFINDER):
    {
        mavlink_rangefinder_t msg;
        mavlink_msg_rangefinder_decode(message, &msg);
251
        _infoFactGroup.getFact("rangefinderDistance")->setRawValue(msg.distance);
252 253
        break;
    }
254 255 256 257 258 259 260 261 262
    }
}

bool ArduSubFirmwarePlugin::adjustIncomingMavlinkMessage(Vehicle* vehicle, mavlink_message_t* message)
{
    _handleMavlinkMessage(message);
    return APMFirmwarePlugin::adjustIncomingMavlinkMessage(vehicle, message);
}

263 264 265 266
QMap<QString, FactGroup*>* ArduSubFirmwarePlugin::factGroups(void) {
    return &_nameToFactGroupMap;
}

267 268 269 270 271 272 273 274
const char* APMSubmarineFactGroup::_camTiltFactName             = "cameraTilt";
const char* APMSubmarineFactGroup::_tetherTurnsFactName         = "tetherTurns";
const char* APMSubmarineFactGroup::_lightsLevel1FactName        = "lights1";
const char* APMSubmarineFactGroup::_lightsLevel2FactName        = "lights2";
const char* APMSubmarineFactGroup::_pilotGainFactName           = "pilotGain";
const char* APMSubmarineFactGroup::_inputHoldFactName           = "inputHold";
const char* APMSubmarineFactGroup::_rollPitchToggleFactName     = "rollPitchToggle";
const char* APMSubmarineFactGroup::_rangefinderDistanceFactName = "rangefinderDistance";
275 276 277

APMSubmarineFactGroup::APMSubmarineFactGroup(QObject* parent)
    : FactGroup(300, ":/json/Vehicle/SubmarineFact.json", parent)
278 279 280 281 282 283
    , _camTiltFact             (0, _camTiltFactName,             FactMetaData::valueTypeDouble)
    , _tetherTurnsFact         (0, _tetherTurnsFactName,         FactMetaData::valueTypeDouble)
    , _lightsLevel1Fact        (0, _lightsLevel1FactName,        FactMetaData::valueTypeDouble)
    , _lightsLevel2Fact        (0, _lightsLevel2FactName,        FactMetaData::valueTypeDouble)
    , _pilotGainFact           (0, _pilotGainFactName,           FactMetaData::valueTypeDouble)
    , _inputHoldFact           (0, _inputHoldFactName,           FactMetaData::valueTypeDouble)
284
    , _rollPitchToggleFact     (0, _rollPitchToggleFactName,     FactMetaData::valueTypeDouble)
285
    , _rangefinderDistanceFact (0, _rangefinderDistanceFactName, FactMetaData::valueTypeDouble)
286
{
287 288 289 290 291 292
    _addFact(&_camTiltFact,             _camTiltFactName);
    _addFact(&_tetherTurnsFact,         _tetherTurnsFactName);
    _addFact(&_lightsLevel1Fact,        _lightsLevel1FactName);
    _addFact(&_lightsLevel2Fact,        _lightsLevel2FactName);
    _addFact(&_pilotGainFact,           _pilotGainFactName);
    _addFact(&_inputHoldFact,           _inputHoldFactName);
293
    _addFact(&_rollPitchToggleFact    , _rollPitchToggleFactName);
294
    _addFact(&_rangefinderDistanceFact, _rangefinderDistanceFactName);
295 296

    // Start out as not available "--.--"
297 298 299 300 301 302
    _camTiltFact.setRawValue             (std::numeric_limits<float>::quiet_NaN());
    _tetherTurnsFact.setRawValue         (std::numeric_limits<float>::quiet_NaN());
    _lightsLevel1Fact.setRawValue        (std::numeric_limits<float>::quiet_NaN());
    _lightsLevel2Fact.setRawValue        (std::numeric_limits<float>::quiet_NaN());
    _pilotGainFact.setRawValue           (std::numeric_limits<float>::quiet_NaN());
    _inputHoldFact.setRawValue           (std::numeric_limits<float>::quiet_NaN());
303
    _rollPitchToggleFact.setRawValue     (2); // 2 shows "Unavailable" in older firmwares
304 305
    _rangefinderDistanceFact.setRawValue (std::numeric_limits<float>::quiet_NaN());

306
}
307 308 309 310 311 312 313 314 315 316 317

QString ArduSubFirmwarePlugin::vehicleImageOpaque(const Vehicle* vehicle) const
{
    Q_UNUSED(vehicle);
    return QStringLiteral("/qmlimages/subVehicleArrowOpaque.png");
}

QString ArduSubFirmwarePlugin::vehicleImageOutline(const Vehicle* vehicle) const
{
    return vehicleImageOpaque(vehicle);
}
318 319 320 321 322 323 324 325 326 327 328

void ArduSubFirmwarePlugin::adjustMetaData(MAV_TYPE vehicleType, FactMetaData* metaData)
{
    Q_UNUSED(vehicleType);
    if (!metaData) {
        return;
    }
    if (_factRenameMap.contains(metaData->name())) {
        metaData->setShortDescription(QString(_factRenameMap[metaData->name()]));
    }
}