APMFirmwarePlugin.cc 13.5 KB
Newer Older
Don Gagne's avatar
Don Gagne committed
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 28
/*=====================================================================
 
 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 Don Gagne <don@thegagnes.com>

#include "APMFirmwarePlugin.h"
#include "Generic/GenericFirmwarePlugin.h"
29
#include "AutoPilotPlugins/APM/APMAutoPilotPlugin.h"    // FIXME: Hack
30
#include "QGCMAVLink.h"
Don Gagne's avatar
Don Gagne committed
31

Pritam Ghanghas's avatar
Pritam Ghanghas committed
32
QGC_LOGGING_CATEGORY(APMFirmwarePluginLog, "APMFirmwarePluginLog")
Don Gagne's avatar
Don Gagne committed
33

34 35 36 37 38 39 40 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 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93
static const QRegExp APM_COPTER_REXP("^(ArduCopter|APM:Copter)");
static const QRegExp APM_PLANE_REXP("^(ArduPlane|APM:Plane)");
static const QRegExp APM_ROVER_REXP("^(ArduRover|APM:Rover)");

// Regex to parse version text coming from APM, gives out firmware type, major, minor and patch level numbers
static const QRegExp VERSION_REXP("^(APM:Copter|APM:Plane|APM:Rover|ArduCopter|ArduPlane|ArduRover) +[vV](\\d*)\\.*(\\d*)*\\.*(\\d*)*");

// minimum firmware versions that don't suffer from mavlink severity inversion bug.
// https://github.com/diydrones/apm_planner/issues/788
static const QString MIN_COPTER_VERSION_WITH_CORRECT_SEVERITY_MSGS("APM:Copter V3.4.0");
static const QString MIN_PLANE_VERSION_WITH_CORRECT_SEVERITY_MSGS("APM:Plane V3.4.0");
static const QString MIN_ROVER_VERSION_WITH_CORRECT_SEVERITY_MSGS("APM:Rover V2.6.0");


/*
 * @brief APMFirmwareVersion is a small class to represent the firmware version
 * It encabsules vehicleType, major version, minor version and patch level version
 * and provides accessors for the same.
 * isValid() can be used, to know whether version infromation is available or not
 * supports < operator
 */
APMFirmwareVersion::APMFirmwareVersion(const QString &versionText)
{
    _major         = 0;
    _minor         = 0;
    _patch         = 0;

    _parseVersion(versionText);
}

bool APMFirmwareVersion::isValid() const
{
    return !_versionString.isEmpty();
}

bool APMFirmwareVersion::isBeta() const
{
    return _versionString.contains(QStringLiteral(".rc"));
}

bool APMFirmwareVersion::isDev() const
{
    return _versionString.contains(QStringLiteral(".dev"));
}

bool APMFirmwareVersion::operator <(const APMFirmwareVersion& other) const
{
    int myVersion = _major << 16 | _minor << 8 | _patch ;
    int otherVersion = other.majorNumber() << 16 | other.minorNumber() << 8 | other.patchNumber();
    return myVersion < otherVersion;
}

void APMFirmwareVersion::_parseVersion(const QString &versionText)
{
    if (versionText.isEmpty()) {
        return;
    }


    if (VERSION_REXP.indexIn(versionText) == -1) {
Pritam Ghanghas's avatar
Pritam Ghanghas committed
94 95
        qCWarning(APMFirmwarePluginLog) << "firmware version regex didn't match anything"
                                        << "version text to be parsed" << versionText;
96 97 98 99 100 101
        return;
    }

    QStringList capturedTexts = VERSION_REXP.capturedTexts();

    if (capturedTexts.count() < 5) {
Pritam Ghanghas's avatar
Pritam Ghanghas committed
102 103
        qCWarning(APMFirmwarePluginLog) << "something wrong with parsing the version text, not hitting anything"
                                        << VERSION_REXP.captureCount() << VERSION_REXP.capturedTexts();
104 105 106 107 108 109 110 111 112 113 114 115 116
        return;
    }

    // successful extraction of version numbers
    // even though we could have collected the version string atleast
    // but if the parsing has faild, not much point
    _versionString = versionText;
    _vehicleType   = capturedTexts[1];
    _major         = capturedTexts[2].toInt();
    _minor         = capturedTexts[3].toInt();
    _patch         = capturedTexts[4].toInt();
}

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

/*
 * @brief APMCustomMode encapsulates the custom modes for APM
 */
APMCustomMode::APMCustomMode(uint32_t mode, bool settable) :
    _mode(mode),
    _settable(settable)
{
}


void APMCustomMode::setEnumToStringMapping(const QMap<uint32_t, QString>& enumToString)
{
    _enumToString = enumToString;
}

QString APMCustomMode::modeString() const
{
    QString mode = _enumToString.value(modeAsInt());
    if (mode.isEmpty()) {
        mode = "mode" + QString::number(modeAsInt());
    }
    return mode;
}

142
APMFirmwarePlugin::APMFirmwarePlugin(void)
Don Gagne's avatar
Don Gagne committed
143
{
Pritam Ghanghas's avatar
Pritam Ghanghas committed
144
     _textSeverityAdjustmentNeeded = false;
Don Gagne's avatar
Don Gagne committed
145 146 147 148
}

bool APMFirmwarePlugin::isCapable(FirmwareCapabilities capabilities)
{
149
    return (capabilities & (MavCmdPreflightStorageCapability | SetFlightModeCapability)) == capabilities;
Don Gagne's avatar
Don Gagne committed
150 151 152 153 154 155 156 157 158 159
}

QList<VehicleComponent*> APMFirmwarePlugin::componentsForVehicle(AutoPilotPlugin* vehicle)
{
    Q_UNUSED(vehicle);
    
    return QList<VehicleComponent*>();
}

QStringList APMFirmwarePlugin::flightModes(void)
160 161 162 163 164 165 166 167
{   
    QStringList flightModesList;
    foreach (const APMCustomMode& customMode, _supportedModes) {
        if (customMode.canBeSet()) {
            flightModesList << customMode.modeString();
        }
    }
    return flightModesList;
Don Gagne's avatar
Don Gagne committed
168 169 170 171
}

QString APMFirmwarePlugin::flightMode(uint8_t base_mode, uint32_t custom_mode)
{
172 173 174 175 176 177 178 179 180 181
    QString flightMode = "Unknown";

    if (base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) {
        foreach (const APMCustomMode& customMode, _supportedModes) {
            if (customMode.modeAsInt() == custom_mode) {
                flightMode = customMode.modeString();
            }
        }
    }
    return flightMode;
Don Gagne's avatar
Don Gagne committed
182 183 184 185
}

bool APMFirmwarePlugin::setFlightMode(const QString& flightMode, uint8_t* base_mode, uint32_t* custom_mode)
{
186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204
    *base_mode = 0;
    *custom_mode = 0;

    bool found = false;

    foreach(const APMCustomMode& mode, _supportedModes) {
        if (flightMode.compare(mode.modeString(), Qt::CaseInsensitive) == 0) {
            *base_mode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
            *custom_mode = mode.modeAsInt();
            found = true;
            break;
        }
    }

    if (!found) {
        qCWarning(APMFirmwarePluginLog) << "Unknown flight Mode" << flightMode;
    }

    return found;
Don Gagne's avatar
Don Gagne committed
205 206 207 208 209 210 211 212 213 214 215 216 217 218 219 220 221 222 223 224 225 226 227 228 229 230 231 232 233 234 235 236 237 238 239 240 241 242 243 244
}

int APMFirmwarePlugin::manualControlReservedButtonCount(void)
{
    // We don't know whether the firmware is going to used any of these buttons.
    // So reserve them all.
    return -1;
}

void APMFirmwarePlugin::adjustMavlinkMessage(mavlink_message_t* message)
{
    if (message->msgid == MAVLINK_MSG_ID_PARAM_VALUE) {
        mavlink_param_value_t paramValue;
        mavlink_param_union_t paramUnion;
        
        // APM stack passes all parameter values in mavlink_param_union_t.param_float no matter what
        // type they are. Fix that up to correct usage.
        
        mavlink_msg_param_value_decode(message, &paramValue);
        
        switch (paramValue.param_type) {
            case MAV_PARAM_TYPE_UINT8:
                paramUnion.param_uint8 = (uint8_t)paramValue.param_value;
                break;
            case MAV_PARAM_TYPE_INT8:
                paramUnion.param_int8 = (int8_t)paramValue.param_value;
                break;
            case MAV_PARAM_TYPE_UINT16:
                paramUnion.param_uint16 = (uint16_t)paramValue.param_value;
                break;
            case MAV_PARAM_TYPE_INT16:
                paramUnion.param_int16 = (int16_t)paramValue.param_value;
                break;
            case MAV_PARAM_TYPE_UINT32:
                paramUnion.param_uint32 = (uint32_t)paramValue.param_value;
                break;
            case MAV_PARAM_TYPE_INT32:
                paramUnion.param_int32 = (int32_t)paramValue.param_value;
                break;
            case MAV_PARAM_TYPE_REAL32:
Don Gagne's avatar
Don Gagne committed
245
                paramUnion.param_float = paramValue.param_value;
Don Gagne's avatar
Don Gagne committed
246 247
                break;
            default:
Pritam Ghanghas's avatar
Pritam Ghanghas committed
248
                qCCritical(APMFirmwarePluginLog) << "Invalid/Unsupported data type used in parameter:" << paramValue.param_type;
Don Gagne's avatar
Don Gagne committed
249 250 251 252
        }
        
        paramValue.param_value = paramUnion.param_float;
        
Don Gagne's avatar
Don Gagne committed
253 254
        mavlink_msg_param_value_encode(message->sysid, message->compid, message, &paramValue);
        
Don Gagne's avatar
Don Gagne committed
255 256 257 258 259 260 261 262 263 264 265 266 267 268 269 270 271 272 273 274 275 276 277 278 279 280 281 282 283 284 285 286 287 288
    } else if (message->msgid == MAVLINK_MSG_ID_PARAM_SET) {
        mavlink_param_set_t     paramSet;
        mavlink_param_union_t   paramUnion;
        
        // APM stack passes all parameter values in mavlink_param_union_t.param_float no matter what
        // type they are. Fix it back to the wrong way on the way out.
        
        mavlink_msg_param_set_decode(message, &paramSet);
        
        paramUnion.param_float = paramSet.param_value;

        switch (paramSet.param_type) {
            case MAV_PARAM_TYPE_UINT8:
                paramSet.param_value = paramUnion.param_uint8;
                break;
            case MAV_PARAM_TYPE_INT8:
                paramSet.param_value = paramUnion.param_int8;
                break;
            case MAV_PARAM_TYPE_UINT16:
                paramSet.param_value = paramUnion.param_uint16;
                break;
            case MAV_PARAM_TYPE_INT16:
                paramSet.param_value = paramUnion.param_int16;
                break;
            case MAV_PARAM_TYPE_UINT32:
                paramSet.param_value = paramUnion.param_uint32;
                break;
            case MAV_PARAM_TYPE_INT32:
                paramSet.param_value = paramUnion.param_int32;
                break;
            case MAV_PARAM_TYPE_REAL32:
                // Already in param_float
                break;
            default:
Pritam Ghanghas's avatar
Pritam Ghanghas committed
289
                qCCritical(APMFirmwarePluginLog) << "Invalid/Unsupported data type used in parameter:" << paramSet.param_type;
Don Gagne's avatar
Don Gagne committed
290
        }
Don Gagne's avatar
Don Gagne committed
291 292
        
        mavlink_msg_param_set_encode(message->sysid, message->compid, message, &paramSet);
Don Gagne's avatar
Don Gagne committed
293 294
    }

295 296
    if (message->msgid == MAVLINK_MSG_ID_STATUSTEXT)
    {
Pritam Ghanghas's avatar
Pritam Ghanghas committed
297
        if (!_firmwareVersion.isValid()) {
298 299 300 301 302 303 304 305 306
            QByteArray b;
            b.resize(MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1);
            mavlink_msg_statustext_get_text(message, b.data());
            // Ensure NUL-termination
            b[b.length()-1] = '\0';
            QString text = QString(b);
            qCDebug(APMFirmwarePluginLog) << text;

            // if don't know firmwareVersion yet, try and see this message contains it
Pritam Ghanghas's avatar
Pritam Ghanghas committed
307 308 309 310 311
            if (text.contains(APM_COPTER_REXP) || text.contains(APM_PLANE_REXP) || text.contains(APM_ROVER_REXP)) {
                // found version string
                _firmwareVersion = APMFirmwareVersion(text);
                _textSeverityAdjustmentNeeded = _isTextSeverityAdjustmentNeeded(_firmwareVersion);
            }
312 313 314
        }

        // adjust mesasge if needed
Pritam Ghanghas's avatar
Pritam Ghanghas committed
315 316 317
        if (_textSeverityAdjustmentNeeded) {
            _adjustSeverity(message);
        }
318 319 320
    }
}

Pritam Ghanghas's avatar
Pritam Ghanghas committed
321
bool APMFirmwarePlugin::_isTextSeverityAdjustmentNeeded(const APMFirmwareVersion& firmwareVersion)
322
{
Pritam Ghanghas's avatar
Pritam Ghanghas committed
323 324 325 326 327 328 329 330 331 332 333 334 335 336 337 338 339 340 341 342 343
   if (!firmwareVersion.isValid()) {
       return false;
   }

   bool adjustmentNeeded = false;
   if (firmwareVersion.vehicleType().contains(APM_COPTER_REXP)) {
       if (firmwareVersion < APMFirmwareVersion(MIN_COPTER_VERSION_WITH_CORRECT_SEVERITY_MSGS)) {
           adjustmentNeeded = true;
       }
   } else if (firmwareVersion.vehicleType().contains(APM_PLANE_REXP)) {
       if (firmwareVersion < APMFirmwareVersion(MIN_PLANE_VERSION_WITH_CORRECT_SEVERITY_MSGS)) {
           adjustmentNeeded = true;
       }
   } else if (firmwareVersion.vehicleType().contains(APM_ROVER_REXP)) {
       if (firmwareVersion < APMFirmwareVersion(MIN_ROVER_VERSION_WITH_CORRECT_SEVERITY_MSGS)) {
           adjustmentNeeded = true;
       }
   }

   return adjustmentNeeded;
}
344

Pritam Ghanghas's avatar
Pritam Ghanghas committed
345 346 347
void APMFirmwarePlugin::_adjustSeverity(mavlink_message_t* message) const
{
    // lets make QGC happy with right severity values
348 349 350
    mavlink_statustext_t statusText;
    mavlink_msg_statustext_decode(message, &statusText);
    switch(statusText.severity) {
Pritam Ghanghas's avatar
Pritam Ghanghas committed
351 352 353 354 355 356 357 358 359
        case MAV_SEVERITY_ALERT:    /* SEVERITY_LOW according to old codes */
            statusText.severity = MAV_SEVERITY_WARNING;
            break;
        case MAV_SEVERITY_CRITICAL: /*SEVERITY_MEDIUM according to old codes  */
            statusText.severity = MAV_SEVERITY_ALERT;
            break;
        case MAV_SEVERITY_ERROR:    /*SEVERITY_HIGH according to old codes */
            statusText.severity = MAV_SEVERITY_CRITICAL;
            break;
360 361 362
    }

    mavlink_msg_statustext_encode(message->sysid, message->compid, message, &statusText);
Don Gagne's avatar
Don Gagne committed
363
}
364 365 366 367 368 369 370 371 372 373 374 375

void APMFirmwarePlugin::initializeVehicle(Vehicle* vehicle)
{
    // Streams are not started automatically on APM stack
    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,             10);
    vehicle->requestDataStream(MAV_DATA_STREAM_EXTRA2,             10);
    vehicle->requestDataStream(MAV_DATA_STREAM_EXTRA3,             3);
}
376 377 378 379 380

void APMFirmwarePlugin::setSupportedModes(QList<APMCustomMode> supportedModes)
{
    _supportedModes = supportedModes;
}
381 382 383 384 385 386

bool APMFirmwarePlugin::sendHomePositionToVehicle(void)
{
    // APM stack wants the home position sent in the first position
    return true;
}
387

388
void APMFirmwarePlugin::addMetaDataToFact(Fact* fact)
389
{
390
    _parameterMetaData.addMetaDataToFact(fact);
391
}