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

Don Gagne's avatar
Don Gagne committed
10 11 12 13 14

#include "APMSensorsComponentController.h"
#include "QGCMAVLink.h"
#include "UAS.h"
#include "QGCApplication.h"
15
#include "APMAutoPilotPlugin.h"
16
#include "ParameterManager.h"
Don Gagne's avatar
Don Gagne committed
17 18 19 20 21

#include <QVariant>
#include <QQmlProperty>

QGC_LOGGING_CATEGORY(APMSensorsComponentControllerLog, "APMSensorsComponentControllerLog")
22 23 24 25 26
QGC_LOGGING_CATEGORY(APMSensorsComponentControllerVerboseLog, "APMSensorsComponentControllerVerboseLog")

const char* APMSensorsComponentController::_compassCalFitnessParam = "COMPASS_CAL_FIT";

APMSensorsComponentController::APMSensorsComponentController(void)
27 28
    : _sensorsComponent(NULL)
    , _statusLog(NULL)
29 30 31 32 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
    , _progressBar(NULL)
    , _nextButton(NULL)
    , _cancelButton(NULL)
    , _showOrientationCalArea(false)
    , _calTypeInProgress(CalTypeNone)
    , _orientationCalDownSideDone(false)
    , _orientationCalUpsideDownSideDone(false)
    , _orientationCalLeftSideDone(false)
    , _orientationCalRightSideDone(false)
    , _orientationCalNoseDownSideDone(false)
    , _orientationCalTailDownSideDone(false)
    , _orientationCalDownSideVisible(false)
    , _orientationCalUpsideDownSideVisible(false)
    , _orientationCalLeftSideVisible(false)
    , _orientationCalRightSideVisible(false)
    , _orientationCalNoseDownSideVisible(false)
    , _orientationCalTailDownSideVisible(false)
    , _orientationCalDownSideInProgress(false)
    , _orientationCalUpsideDownSideInProgress(false)
    , _orientationCalLeftSideInProgress(false)
    , _orientationCalRightSideInProgress(false)
    , _orientationCalNoseDownSideInProgress(false)
    , _orientationCalTailDownSideInProgress(false)
    , _orientationCalDownSideRotate(false)
    , _orientationCalUpsideDownSideRotate(false)
    , _orientationCalLeftSideRotate(false)
    , _orientationCalRightSideRotate(false)
    , _orientationCalNoseDownSideRotate(false)
    , _orientationCalTailDownSideRotate(false)
    , _waitingForCancel(false)
    , _restoreCompassCalFitness(false)
Don Gagne's avatar
Don Gagne committed
60
{
61 62 63
    _compassCal.setVehicle(_vehicle);
    connect(&_compassCal, &APMCompassCal::vehicleTextMessage, this, &APMSensorsComponentController::_handleUASTextMessage);

64
    APMAutoPilotPlugin * apmPlugin = qobject_cast<APMAutoPilotPlugin*>(_vehicle->autopilotPlugin());
Don Gagne's avatar
Don Gagne committed
65

66 67 68 69 70 71 72 73 74 75 76 77 78
    // Find the sensors component
    foreach (const QVariant& varVehicleComponent, apmPlugin->vehicleComponents()) {
        _sensorsComponent = qobject_cast<APMSensorsComponent*>(varVehicleComponent.value<VehicleComponent*>());
        if (_sensorsComponent) {
            break;
        }
    }

    if (_sensorsComponent) {
        connect(_sensorsComponent, &VehicleComponent::setupCompleteChanged, this, &APMSensorsComponentController::setupNeededChanged);
    } else {
        qWarning() << "Sensors component is missing";
    }
Don Gagne's avatar
Don Gagne committed
79 80

    connect(qgcApp()->toolbox()->mavlinkProtocol(), &MAVLinkProtocol::messageReceived, this, &APMSensorsComponentController::_mavlinkMessageReceived);
Don Gagne's avatar
Don Gagne committed
81 82
}

83 84 85 86 87
APMSensorsComponentController::~APMSensorsComponentController()
{
    _restorePreviousCompassCalFitness();
}

Don Gagne's avatar
Don Gagne committed
88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104
/// Appends the specified text to the status log area in the ui
void APMSensorsComponentController::_appendStatusLog(const QString& text)
{
    Q_ASSERT(_statusLog);
    
    QVariant returnedValue;
    QVariant varText = text;
    QMetaObject::invokeMethod(_statusLog,
                              "append",
                              Q_RETURN_ARG(QVariant, returnedValue),
                              Q_ARG(QVariant, varText));
}

void APMSensorsComponentController::_startLogCalibration(void)
{
    _hideAllCalAreas();
    
105
    connect(_vehicle, &Vehicle::textMessageReceived, this, &APMSensorsComponentController::_handleUASTextMessage);
Don Gagne's avatar
Don Gagne committed
106
    
107
    emit setAllCalButtonsEnabled(false);
108
    if (_calTypeInProgress == CalTypeAccel || _calTypeInProgress == CalTypeCompassMot) {
109 110
        _nextButton->setEnabled(true);
    }
111
    _cancelButton->setEnabled(_calTypeInProgress == CalTypeOnboardCompass);
Don Gagne's avatar
Don Gagne committed
112 113 114 115
}

void APMSensorsComponentController::_startVisualCalibration(void)
{
116
    emit setAllCalButtonsEnabled(false);
Don Gagne's avatar
Don Gagne committed
117
    _cancelButton->setEnabled(true);
118
    _nextButton->setEnabled(false);
Don Gagne's avatar
Don Gagne committed
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 144 145 146 147 148 149 150 151 152

    _resetInternalState();
    
    _progressBar->setProperty("value", 0);
}

void APMSensorsComponentController::_resetInternalState(void)
{
    _orientationCalDownSideDone = true;
    _orientationCalUpsideDownSideDone = true;
    _orientationCalLeftSideDone = true;
    _orientationCalRightSideDone = true;
    _orientationCalTailDownSideDone = true;
    _orientationCalNoseDownSideDone = true;
    _orientationCalDownSideInProgress = false;
    _orientationCalUpsideDownSideInProgress = false;
    _orientationCalLeftSideInProgress = false;
    _orientationCalRightSideInProgress = false;
    _orientationCalNoseDownSideInProgress = false;
    _orientationCalTailDownSideInProgress = false;
    _orientationCalDownSideRotate = false;
    _orientationCalUpsideDownSideRotate = false;
    _orientationCalLeftSideRotate = false;
    _orientationCalRightSideRotate = false;
    _orientationCalNoseDownSideRotate = false;
    _orientationCalTailDownSideRotate = false;

    emit orientationCalSidesRotateChanged();
    emit orientationCalSidesDoneChanged();
    emit orientationCalSidesInProgressChanged();
}

void APMSensorsComponentController::_stopCalibration(APMSensorsComponentController::StopCalibrationCode code)
{
153
    _vehicle->setConnectionLostEnabled(true);
154

155
    disconnect(_vehicle, &Vehicle::textMessageReceived, this, &APMSensorsComponentController::_handleUASTextMessage);
Don Gagne's avatar
Don Gagne committed
156
    
157
    emit setAllCalButtonsEnabled(true);
Don Gagne's avatar
Don Gagne committed
158 159
    _nextButton->setEnabled(false);
    _cancelButton->setEnabled(false);
160

161 162 163 164
    if (_calTypeInProgress == CalTypeOnboardCompass) {
        _restorePreviousCompassCalFitness();
    }

Don Gagne's avatar
Don Gagne committed
165 166 167
    if (code == StopCalibrationSuccess) {
        _resetInternalState();
        _progressBar->setProperty("value", 1);
168 169
        if (parameterExists(FactSystem::defaultComponentId, QStringLiteral("COMPASS_LEARN"))) {
            getParameterFact(FactSystem::defaultComponentId, QStringLiteral("COMPASS_LEARN"))->setRawValue(0);
170
        }
Don Gagne's avatar
Don Gagne committed
171 172 173 174 175 176 177 178 179 180
    } else {
        _progressBar->setProperty("value", 0);
    }
    
    _waitingForCancel = false;
    emit waitingForCancelChanged();

    _refreshParams();
    
    switch (code) {
181
    case StopCalibrationSuccess:
Don Gagne's avatar
Don Gagne committed
182
        _orientationCalAreaHelpText->setProperty("text", tr("Calibration complete"));
183
        emit resetStatusTextArea();
184
        emit calibrationComplete(_calTypeInProgress);
185 186
        break;

Don Gagne's avatar
Don Gagne committed
187
    case StopCalibrationSuccessShowLog:
188
        emit calibrationComplete(_calTypeInProgress);
Don Gagne's avatar
Don Gagne committed
189 190
        break;

191 192 193 194 195 196 197 198
    case StopCalibrationCancelled:
        emit resetStatusTextArea();
        _hideAllCalAreas();
        break;

    default:
        // Assume failed
        _hideAllCalAreas();
199
        qgcApp()->showMessage(tr("Calibration failed. Calibration log will be displayed."));
200
        break;
Don Gagne's avatar
Don Gagne committed
201 202
    }
    
203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 218 219 220 221 222 223 224 225 226 227 228
    _calTypeInProgress = CalTypeNone;
}

void APMSensorsComponentController::_mavCommandResult(int vehicleId, int component, int command, int result, bool noReponseFromVehicle)
{
    Q_UNUSED(component);
    Q_UNUSED(noReponseFromVehicle);

    if (_vehicle->id() != vehicleId) {
        return;
    }

    if (command == MAV_CMD_DO_CANCEL_MAG_CAL) {
        disconnect(_vehicle, &Vehicle::mavCommandResult, this, &APMSensorsComponentController::_mavCommandResult);
        if (result == MAV_RESULT_ACCEPTED) {
            // Onboard mag cal is supported
            _calTypeInProgress = CalTypeOnboardCompass;
            _rgCompassCalProgress[0] = 0;
            _rgCompassCalProgress[1] = 0;
            _rgCompassCalProgress[2] = 0;
            _rgCompassCalComplete[0] = false;
            _rgCompassCalComplete[1] = false;
            _rgCompassCalComplete[2] = false;

            _startLogCalibration();
            uint8_t compassBits = 0;
Don Gagne's avatar
Don Gagne committed
229
            if (getParameterFact(FactSystem::defaultComponentId, QStringLiteral("COMPASS_DEV_ID"))->rawValue().toInt() > 0 &&
230
                getParameterFact(FactSystem::defaultComponentId, QStringLiteral("COMPASS_USE"))->rawValue().toBool()) {
231 232 233 234 235 236 237
                compassBits |= 1 << 0;
                qCDebug(APMSensorsComponentControllerLog) << "Performing onboard compass cal for compass 1";
            } else {
                _rgCompassCalComplete[0] = true;
                _rgCompassCalSucceeded[0] = true;
                _rgCompassCalFitness[0] = 0;
            }
Don Gagne's avatar
Don Gagne committed
238
            if (getParameterFact(FactSystem::defaultComponentId, QStringLiteral("COMPASS_DEV_ID2"))->rawValue().toInt() > 0 &&
239
                getParameterFact(FactSystem::defaultComponentId, QStringLiteral("COMPASS_USE2"))->rawValue().toBool()) {
240 241 242 243
                compassBits |= 1 << 1;
                qCDebug(APMSensorsComponentControllerLog) << "Performing onboard compass cal for compass 2";
            } else {
                _rgCompassCalComplete[1] = true;
Don Gagne's avatar
Don Gagne committed
244
                _rgCompassCalSucceeded[1] = true;
245 246
                _rgCompassCalFitness[1] = 0;
            }
Don Gagne's avatar
Don Gagne committed
247
            if (getParameterFact(FactSystem::defaultComponentId, QStringLiteral("COMPASS_DEV_ID3"))->rawValue().toInt() > 0 &&
248
                getParameterFact(FactSystem::defaultComponentId, QStringLiteral("COMPASS_USE3"))->rawValue().toBool()) {
249 250 251 252
                compassBits |= 1 << 2;
                qCDebug(APMSensorsComponentControllerLog) << "Performing onboard compass cal for compass 3";
            } else {
                _rgCompassCalComplete[2] = true;
Don Gagne's avatar
Don Gagne committed
253
                _rgCompassCalSucceeded[2] = true;
254 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
                _rgCompassCalFitness[2] = 0;
            }

            // We bump up the fitness value so calibration will always succeed
            Fact* compassCalFitness = getParameterFact(FactSystem::defaultComponentId, _compassCalFitnessParam);
            _restoreCompassCalFitness = true;
            _previousCompassCalFitness = compassCalFitness->rawValue().toFloat();
            getParameterFact(FactSystem::defaultComponentId, _compassCalFitnessParam)->setRawValue(100.0);

            _appendStatusLog(tr("Rotate the vehicle randomly around all axes until the progress bar fills all the way to the right ."));
            _vehicle->sendMavCommand(_vehicle->defaultComponentId(),
                                     MAV_CMD_DO_START_MAG_CAL,
                                     true,          // showError
                                     compassBits,   // which compass(es) to calibrate
                                     0,             // no retry on failure
                                     1,             // save values after complete
                                     0,             // no delayed start
                                     0);            // no auto-reboot

        } else {
            // Onboard mag cal is not supported
            _compassCal.startCalibration();
        }
    } else if (command == MAV_CMD_DO_START_MAG_CAL && result != MAV_RESULT_ACCEPTED) {
        _restorePreviousCompassCalFitness();
    }
Don Gagne's avatar
Don Gagne committed
280 281 282 283
}

void APMSensorsComponentController::calibrateCompass(void)
{
284 285 286 287 288 289
    // First we need to determine if the vehicle support onboard compass cal. There isn't an easy way to
    // do this. A hack is to send the mag cancel command and see if it is accepted.
    connect(_vehicle, &Vehicle::mavCommandResult, this, &APMSensorsComponentController::_mavCommandResult);
    _vehicle->sendMavCommand(_vehicle->defaultComponentId(), MAV_CMD_DO_CANCEL_MAG_CAL, false /* showError */);

    // Now we wait for the result to come back
Don Gagne's avatar
Don Gagne committed
290 291 292 293
}

void APMSensorsComponentController::calibrateAccel(void)
{
294
    _calTypeInProgress = CalTypeAccel;
295
    _vehicle->setConnectionLostEnabled(false);
Don Gagne's avatar
Don Gagne committed
296 297 298 299
    _startLogCalibration();
    _uas->startCalibration(UASInterface::StartCalibrationAccel);
}

300 301
void APMSensorsComponentController::calibrateMotorInterference(void)
{
302
    _calTypeInProgress = CalTypeCompassMot;
303 304 305 306 307 308 309 310
    _vehicle->setConnectionLostEnabled(false);
    _startLogCalibration();
    _appendStatusLog(tr("Raise the throttle slowly to between 50% ~ 75% (the props will spin!) for 5 ~ 10 seconds."));
    _appendStatusLog(tr("Quickly bring the throttle back down to zero"));
    _appendStatusLog(tr("Press the Next button to complete the calibration"));
    _uas->startCalibration(UASInterface::StartCalibrationCompassMot);
}

Don Gagne's avatar
Don Gagne committed
311 312
void APMSensorsComponentController::levelHorizon(void)
{
313
    _calTypeInProgress = CalTypeLevelHorizon;
Don Gagne's avatar
Don Gagne committed
314 315 316 317 318 319
    _vehicle->setConnectionLostEnabled(false);
    _startLogCalibration();
    _appendStatusLog(tr("Hold the vehicle in its level flight position."));
    _uas->startCalibration(UASInterface::StartCalibrationLevel);
}

320 321 322 323 324 325 326 327 328
void APMSensorsComponentController::calibratePressure(void)
{
    _calTypeInProgress = CalTypePressure;
    _vehicle->setConnectionLostEnabled(false);
    _startLogCalibration();
    _appendStatusLog(tr("Requesting pressure calibration..."));
    _uas->startCalibration(UASInterface::StartCalibrationPressure);
}

Don Gagne's avatar
Don Gagne committed
329 330 331 332 333
void APMSensorsComponentController::_handleUASTextMessage(int uasId, int compId, int severity, QString text)
{
    Q_UNUSED(compId);
    Q_UNUSED(severity);
    
334
    if (uasId != _vehicle->id()) {
Don Gagne's avatar
Don Gagne committed
335 336 337
        return;
    }

338 339
    if (text.startsWith(QLatin1Literal("PreArm:")) || text.startsWith(QLatin1Literal("EKF"))
            || text.startsWith(QLatin1Literal("Arm")) || text.startsWith(QLatin1Literal("Initialising"))) {
Don Gagne's avatar
Don Gagne committed
340 341 342
        return;
    }

343
    if (text.contains(QLatin1Literal("progress <"))) {
344 345 346
        QString percent = text.split("<").last().split(">").first();
        bool ok;
        int p = percent.toInt(&ok);
347
        if (ok && _progressBar) {
348 349 350 351 352
            _progressBar->setProperty("value", (float)(p / 100.0));
        }
        return;
    }

353
    QString anyKey(QStringLiteral("and press any"));
Don Gagne's avatar
Don Gagne committed
354
    if (text.contains(anyKey)) {
355
        text = text.left(text.indexOf(anyKey)) + QStringLiteral("and click Next to continue.");
356
        _nextButton->setEnabled(true);
Don Gagne's avatar
Don Gagne committed
357 358 359 360 361
    }

    _appendStatusLog(text);
    qCDebug(APMSensorsComponentControllerLog) << text << severity;

362
    if (text.contains(QLatin1String("Calibration successful"))) {
Don Gagne's avatar
Don Gagne committed
363 364 365 366
        _stopCalibration(StopCalibrationSuccess);
        return;
    }

367
    if (text.contains(QLatin1String("FAILED"))) {
Don Gagne's avatar
Don Gagne committed
368 369 370 371 372
        _stopCalibration(StopCalibrationFailed);
        return;
    }

    // All calibration messages start with [cal]
373
    QString calPrefix(QStringLiteral("[cal] "));
Don Gagne's avatar
Don Gagne committed
374 375 376 377 378
    if (!text.startsWith(calPrefix)) {
        return;
    }
    text = text.right(text.length() - calPrefix.length());

379
    QString calStartPrefix(QStringLiteral("calibration started: "));
Don Gagne's avatar
Don Gagne committed
380 381 382 383 384
    if (text.startsWith(calStartPrefix)) {
        text = text.right(text.length() - calStartPrefix.length());
        
        _startVisualCalibration();
        
385
        if (text == QLatin1Literal("accel") || text == QLatin1Literal("mag") || text == QLatin1Literal("gyro")) {
Don Gagne's avatar
Don Gagne committed
386 387 388 389 390 391 392 393 394 395 396 397 398 399 400 401 402 403 404 405 406 407 408 409 410
            // Reset all progress indication
            _orientationCalDownSideDone = false;
            _orientationCalUpsideDownSideDone = false;
            _orientationCalLeftSideDone = false;
            _orientationCalRightSideDone = false;
            _orientationCalTailDownSideDone = false;
            _orientationCalNoseDownSideDone = false;
            _orientationCalDownSideInProgress = false;
            _orientationCalUpsideDownSideInProgress = false;
            _orientationCalLeftSideInProgress = false;
            _orientationCalRightSideInProgress = false;
            _orientationCalNoseDownSideInProgress = false;
            _orientationCalTailDownSideInProgress = false;
            
            // Reset all visibility
            _orientationCalDownSideVisible = false;
            _orientationCalUpsideDownSideVisible = false;
            _orientationCalLeftSideVisible = false;
            _orientationCalRightSideVisible = false;
            _orientationCalTailDownSideVisible = false;
            _orientationCalNoseDownSideVisible = false;
            
            _orientationCalAreaHelpText->setProperty("text", "Place your vehicle into one of the Incomplete orientations shown below and hold it still");
            
            if (text == "accel") {
411
                _calTypeInProgress = CalTypeAccel;
Don Gagne's avatar
Don Gagne committed
412 413 414 415 416 417 418
                _orientationCalDownSideVisible = true;
                _orientationCalUpsideDownSideVisible = true;
                _orientationCalLeftSideVisible = true;
                _orientationCalRightSideVisible = true;
                _orientationCalTailDownSideVisible = true;
                _orientationCalNoseDownSideVisible = true;
            } else if (text == "mag") {
419
                _calTypeInProgress = CalTypeOffboardCompass;
Don Gagne's avatar
Don Gagne committed
420 421 422 423 424 425 426 427 428 429 430 431 432 433 434 435 436
                _orientationCalDownSideVisible = true;
                _orientationCalUpsideDownSideVisible = true;
                _orientationCalLeftSideVisible = true;
                _orientationCalRightSideVisible = true;
                _orientationCalTailDownSideVisible = true;
                _orientationCalNoseDownSideVisible = true;
            } else {
                Q_ASSERT(false);
            }
            emit orientationCalSidesDoneChanged();
            emit orientationCalSidesVisibleChanged();
            emit orientationCalSidesInProgressChanged();
            _updateAndEmitShowOrientationCalArea(true);
        }
        return;
    }
    
437
    if (text.endsWith(QLatin1Literal("orientation detected"))) {
Don Gagne's avatar
Don Gagne committed
438 439 440
        QString side = text.section(" ", 0, 0);
        qDebug() << "Side started" << side;
        
441
        if (side == QLatin1Literal("down")) {
Don Gagne's avatar
Don Gagne committed
442
            _orientationCalDownSideInProgress = true;
443
            if (_calTypeInProgress == CalTypeOffboardCompass) {
Don Gagne's avatar
Don Gagne committed
444 445
                _orientationCalDownSideRotate = true;
            }
446
        } else if (side == QLatin1Literal("up")) {
Don Gagne's avatar
Don Gagne committed
447
            _orientationCalUpsideDownSideInProgress = true;
448
            if (_calTypeInProgress == CalTypeOffboardCompass) {
Don Gagne's avatar
Don Gagne committed
449 450
                _orientationCalUpsideDownSideRotate = true;
            }
451
        } else if (side == QLatin1Literal("left")) {
Don Gagne's avatar
Don Gagne committed
452
            _orientationCalLeftSideInProgress = true;
453
            if (_calTypeInProgress == CalTypeOffboardCompass) {
Don Gagne's avatar
Don Gagne committed
454 455
                _orientationCalLeftSideRotate = true;
            }
456
        } else if (side == QLatin1Literal("right")) {
Don Gagne's avatar
Don Gagne committed
457
            _orientationCalRightSideInProgress = true;
458
            if (_calTypeInProgress == CalTypeOffboardCompass) {
Don Gagne's avatar
Don Gagne committed
459 460
                _orientationCalRightSideRotate = true;
            }
461
        } else if (side == QLatin1Literal("front")) {
Don Gagne's avatar
Don Gagne committed
462
            _orientationCalNoseDownSideInProgress = true;
463
            if (_calTypeInProgress == CalTypeOffboardCompass) {
Don Gagne's avatar
Don Gagne committed
464 465
                _orientationCalNoseDownSideRotate = true;
            }
466
        } else if (side == QLatin1Literal("back")) {
Don Gagne's avatar
Don Gagne committed
467
            _orientationCalTailDownSideInProgress = true;
468
            if (_calTypeInProgress == CalTypeOffboardCompass) {
Don Gagne's avatar
Don Gagne committed
469 470 471 472
                _orientationCalTailDownSideRotate = true;
            }
        }
        
473
        if (_calTypeInProgress == CalTypeOffboardCompass) {
474
            _orientationCalAreaHelpText->setProperty("text", tr("Rotate the vehicle continuously as shown in the diagram until marked as Completed"));
Don Gagne's avatar
Don Gagne committed
475
        } else {
476
            _orientationCalAreaHelpText->setProperty("text", tr("Hold still in the current orientation"));
Don Gagne's avatar
Don Gagne committed
477 478 479 480 481 482 483
        }
        
        emit orientationCalSidesInProgressChanged();
        emit orientationCalSidesRotateChanged();
        return;
    }
    
484
    if (text.endsWith(QLatin1Literal("side done, rotate to a different side"))) {
Don Gagne's avatar
Don Gagne committed
485 486 487
        QString side = text.section(" ", 0, 0);
        qDebug() << "Side finished" << side;
        
488
        if (side == QLatin1Literal("down")) {
Don Gagne's avatar
Don Gagne committed
489 490 491
            _orientationCalDownSideInProgress = false;
            _orientationCalDownSideDone = true;
            _orientationCalDownSideRotate = false;
492
        } else if (side == QLatin1Literal("up")) {
Don Gagne's avatar
Don Gagne committed
493 494
            _orientationCalUpsideDownSideInProgress = false;
            _orientationCalUpsideDownSideDone = true;
495
            _orientationCalUpsideDownSideRotate = false;
496
        } else if (side == QLatin1Literal("left")) {
Don Gagne's avatar
Don Gagne committed
497 498 499
            _orientationCalLeftSideInProgress = false;
            _orientationCalLeftSideDone = true;
            _orientationCalLeftSideRotate = false;
500
        } else if (side == QLatin1Literal("right")) {
Don Gagne's avatar
Don Gagne committed
501 502
            _orientationCalRightSideInProgress = false;
            _orientationCalRightSideDone = true;
503
            _orientationCalRightSideRotate = false;
504
        } else if (side == QLatin1Literal("front")) {
Don Gagne's avatar
Don Gagne committed
505 506 507
            _orientationCalNoseDownSideInProgress = false;
            _orientationCalNoseDownSideDone = true;
            _orientationCalNoseDownSideRotate = false;
508
        } else if (side == QLatin1Literal("back")) {
Don Gagne's avatar
Don Gagne committed
509 510
            _orientationCalTailDownSideInProgress = false;
            _orientationCalTailDownSideDone = true;
511
            _orientationCalTailDownSideRotate = false;
Don Gagne's avatar
Don Gagne committed
512 513
        }
        
514
        _orientationCalAreaHelpText->setProperty("text", tr("Place you vehicle into one of the orientations shown below and hold it still"));
Don Gagne's avatar
Don Gagne committed
515 516 517 518 519 520 521

        emit orientationCalSidesInProgressChanged();
        emit orientationCalSidesDoneChanged();
        emit orientationCalSidesRotateChanged();
        return;
    }
    
522
    if (text.startsWith(QLatin1Literal("calibration done:"))) {
523 524 525 526
        _stopCalibration(StopCalibrationSuccess);
        return;
    }

527
    if (text.startsWith(QLatin1Literal("calibration cancelled"))) {
Don Gagne's avatar
Don Gagne committed
528 529 530 531
        _stopCalibration(_waitingForCancel ? StopCalibrationCancelled : StopCalibrationFailed);
        return;
    }

532
    if (text.startsWith(QLatin1Literal("calibration failed"))) {
533 534 535
        _stopCalibration(StopCalibrationFailed);
        return;
    }
Don Gagne's avatar
Don Gagne committed
536 537 538 539 540 541
}

void APMSensorsComponentController::_refreshParams(void)
{
    QStringList fastRefreshList;
    
542 543
    fastRefreshList << QStringLiteral("COMPASS_OFS_X") << QStringLiteral("COMPASS_OFS_X") << QStringLiteral("COMPASS_OFS_X")
                    << QStringLiteral("INS_ACCOFFS_X") << QStringLiteral("INS_ACCOFFS_Y") << QStringLiteral("INS_ACCOFFS_Z");
544
    foreach (const QString &paramName, fastRefreshList) {
545
        _vehicle->parameterManager()->refreshParameter(FactSystem::defaultComponentId, paramName);
Don Gagne's avatar
Don Gagne committed
546 547 548
    }
    
    // Now ask for all to refresh
549 550
    _vehicle->parameterManager()->refreshParametersPrefix(FactSystem::defaultComponentId, QStringLiteral("COMPASS_"));
    _vehicle->parameterManager()->refreshParametersPrefix(FactSystem::defaultComponentId, QStringLiteral("INS_"));
Don Gagne's avatar
Don Gagne committed
551 552 553 554 555 556 557 558 559 560 561 562 563 564 565 566
}

void APMSensorsComponentController::_updateAndEmitShowOrientationCalArea(bool show)
{
    _showOrientationCalArea = show;
    emit showOrientationCalAreaChanged();
}

void APMSensorsComponentController::_hideAllCalAreas(void)
{
    _updateAndEmitShowOrientationCalArea(false);
}

void APMSensorsComponentController::cancelCalibration(void)
{
    _cancelButton->setEnabled(false);
567

568 569 570
    if (_calTypeInProgress == CalTypeOffboardCompass) {
        _waitingForCancel = true;
        emit waitingForCancelChanged();
571
        _compassCal.cancelCalibration();
572 573 574
    } else if (_calTypeInProgress == CalTypeOnboardCompass) {
        _vehicle->sendMavCommand(_vehicle->defaultComponentId(), MAV_CMD_DO_CANCEL_MAG_CAL, true /* showError */);
        _stopCalibration(StopCalibrationCancelled);
575
    } else {
576 577
        _waitingForCancel = true;
        emit waitingForCancelChanged();
578 579 580 581
        // The firmware doesn't always allow us to cancel calibration. The best we can do is wait
        // for it to timeout.
        _uas->stopCalibration();
    }
582

Don Gagne's avatar
Don Gagne committed
583 584 585 586 587
}

void APMSensorsComponentController::nextClicked(void)
{
    mavlink_message_t       msg;
Don Gagne's avatar
Don Gagne committed
588 589 590 591 592
    mavlink_msg_command_ack_pack_chan(qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(),
                                      qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(),
                                      _vehicle->priorityLink()->mavlinkChannel(),
                                      &msg,
                                      0,    // command
DonLakeFlyer's avatar
DonLakeFlyer committed
593
                                      1,    // result
Gus Grubba's avatar
Gus Grubba committed
594 595 596 597
                                      0,    // progress
                                      0,    // result_param2
                                      0,    // target_system
                                      0);   // target_component
Don Gagne's avatar
Don Gagne committed
598

599
    _vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg);
600

601
    if (_calTypeInProgress == CalTypeCompassMot) {
602 603
        _stopCalibration(StopCalibrationSuccess);
    }
Don Gagne's avatar
Don Gagne committed
604
}
605 606 607 608 609 610 611 612 613 614

bool APMSensorsComponentController::compassSetupNeeded(void) const
{
    return _sensorsComponent->compassSetupNeeded();
}

bool APMSensorsComponentController::accelSetupNeeded(void) const
{
    return _sensorsComponent->accelSetupNeeded();
}
Don Gagne's avatar
Don Gagne committed
615 616 617 618 619

bool APMSensorsComponentController::usingUDPLink(void)
{
    return _vehicle->priorityLink()->getLinkConfiguration()->type() == LinkConfiguration::TypeUdp;
}
Don Gagne's avatar
Don Gagne committed
620

621
void APMSensorsComponentController::_handleCommandAck(mavlink_message_t& message)
Don Gagne's avatar
Don Gagne committed
622
{
623
    if (_calTypeInProgress == CalTypeLevelHorizon) {
Don Gagne's avatar
Don Gagne committed
624 625 626 627 628 629 630 631 632 633 634 635 636 637 638 639
        mavlink_command_ack_t commandAck;
        mavlink_msg_command_ack_decode(&message, &commandAck);

        if (commandAck.command == MAV_CMD_PREFLIGHT_CALIBRATION) {
            switch (commandAck.result) {
            case MAV_RESULT_ACCEPTED:
                _appendStatusLog(tr("Level horizon complete"));
                _stopCalibration(StopCalibrationSuccessShowLog);
                break;
            default:
                _appendStatusLog(tr("Level horizon failed"));
                _stopCalibration(StopCalibrationFailed);
                break;
            }
        }
    }
640 641 642 643 644 645 646 647 648 649 650 651 652 653 654 655 656 657

    if (_calTypeInProgress == CalTypePressure) {
        mavlink_command_ack_t commandAck;
        mavlink_msg_command_ack_decode(&message, &commandAck);

        if (commandAck.command == MAV_CMD_PREFLIGHT_CALIBRATION) {
            switch (commandAck.result) {
            case MAV_RESULT_ACCEPTED:
                _appendStatusLog(tr("Pressure calibration success"));
                _stopCalibration(StopCalibrationSuccessShowLog);
                break;
            default:
                _appendStatusLog(tr("Pressure calibration fail"));
                _stopCalibration(StopCalibrationFailed);
                break;
            }
        }
    }
Don Gagne's avatar
Don Gagne committed
658
}
659 660 661 662 663 664 665 666

void APMSensorsComponentController::_handleMagCalProgress(mavlink_message_t& message)
{
    if (_calTypeInProgress == CalTypeOnboardCompass) {
        mavlink_mag_cal_progress_t magCalProgress;
        mavlink_msg_mag_cal_progress_decode(&message, &magCalProgress);

        qCDebug(APMSensorsComponentControllerVerboseLog) << "_handleMagCalProgress id:mask:pct"
DonLakeFlyer's avatar
DonLakeFlyer committed
667
                                                         << magCalProgress.compass_id << magCalProgress.cal_mask << magCalProgress.completion_pct;
668 669 670 671 672 673 674 675 676 677 678 679 680 681 682 683 684 685 686 687 688 689 690 691 692 693 694

        // How many compasses are we calibrating?
        int compassCalCount = 0;
        for (int i=0; i<3; i++) {
            if (magCalProgress.cal_mask & (1 << i)) {
                compassCalCount++;
            }
        }

        if (magCalProgress.compass_id < 3) {
            // Each compass gets a portion of the overall progress
            _rgCompassCalProgress[magCalProgress.compass_id] = magCalProgress.completion_pct / compassCalCount;
        }

        if (_progressBar) {
            _progressBar->setProperty("value", (float)(_rgCompassCalProgress[0] + _rgCompassCalProgress[1] + _rgCompassCalProgress[2]) / 100.0);
        }
    }
}

void APMSensorsComponentController::_handleMagCalReport(mavlink_message_t& message)
{
    if (_calTypeInProgress == CalTypeOnboardCompass) {
        mavlink_mag_cal_report_t magCalReport;
        mavlink_msg_mag_cal_report_decode(&message, &magCalReport);

        qCDebug(APMSensorsComponentControllerVerboseLog) << "_handleMagCalReport id:mask:status:fitness"
DonLakeFlyer's avatar
DonLakeFlyer committed
695
                                                         << magCalReport.compass_id << magCalReport.cal_mask << magCalReport.cal_status << magCalReport.fitness;
696 697 698 699 700 701 702 703 704 705 706 707 708 709 710 711 712 713 714 715 716 717 718 719 720 721 722 723 724 725 726 727 728 729 730 731 732 733 734 735 736 737 738 739 740 741 742 743 744 745 746 747 748 749 750 751 752 753 754 755 756 757 758 759 760 761 762 763

        bool additionalCompassCompleted = false;
        if (magCalReport.compass_id < 3 && !_rgCompassCalComplete[magCalReport.compass_id]) {
            if (magCalReport.cal_status == MAG_CAL_SUCCESS) {
                _appendStatusLog(tr("Compass %1 calibration complete").arg(magCalReport.compass_id));
            } else {
                _appendStatusLog(tr("Compass %1 calibration below quality threshold").arg(magCalReport.compass_id));
            }
            _rgCompassCalComplete[magCalReport.compass_id] = true;
            _rgCompassCalSucceeded[magCalReport.compass_id] = magCalReport.cal_status == MAG_CAL_SUCCESS;
            _rgCompassCalFitness[magCalReport.compass_id] = magCalReport.fitness;
            additionalCompassCompleted = true;
        }

        if (_rgCompassCalComplete[0] && _rgCompassCalComplete[1] &&_rgCompassCalComplete[2]) {
            for (int i=0; i<3; i++) {
                qCDebug(APMSensorsComponentControllerLog) << QString("Onboard compass call report #%1: succeed:fitness %2:%3").arg(i).arg(_rgCompassCalSucceeded[i]).arg(_rgCompassCalFitness[i]);
            }
            emit compass1CalFitnessChanged(_rgCompassCalFitness[0]);
            emit compass2CalFitnessChanged(_rgCompassCalFitness[1]);
            emit compass3CalFitnessChanged(_rgCompassCalFitness[2]);
            emit compass1CalSucceededChanged(_rgCompassCalSucceeded[0]);
            emit compass2CalSucceededChanged(_rgCompassCalSucceeded[1]);
            emit compass3CalSucceededChanged(_rgCompassCalSucceeded[2]);
            if (_rgCompassCalSucceeded[0] && _rgCompassCalSucceeded[1] && _rgCompassCalSucceeded[2]) {
                _appendStatusLog(tr("All compasses calibrated successfully"));
                _appendStatusLog(tr("YOU MUST REBOOT YOUR VEHICLE NOW FOR NEW SETTINGS TO TAKE AFFECT"));
                _stopCalibration(StopCalibrationSuccessShowLog);
            } else {
                _appendStatusLog(tr("Compass calibration failed"));
                _appendStatusLog(tr("YOU MUST REBOOT YOUR VEHICLE NOW AND RETRY COMPASS CALIBRATION PRIOR TO FLIGHT"));
                _stopCalibration(StopCalibrationFailed);
            }
        } else if (additionalCompassCompleted) {
            _appendStatusLog(tr("Continue rotating..."));
        }

    }
}

void APMSensorsComponentController::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t message)
{
    Q_UNUSED(link);

    if (message.sysid != _vehicle->id()) {
        return;
    }

    switch (message.msgid) {
    case MAVLINK_MSG_ID_COMMAND_ACK:
        _handleCommandAck(message);
        break;
    case MAVLINK_MSG_ID_MAG_CAL_PROGRESS:
        _handleMagCalProgress(message);
        break;
    case MAVLINK_MSG_ID_MAG_CAL_REPORT:
        _handleMagCalReport(message);
        break;
    }
}

void APMSensorsComponentController::_restorePreviousCompassCalFitness(void)
{
    if (_restoreCompassCalFitness) {
        _restoreCompassCalFitness = false;
        getParameterFact(FactSystem::defaultComponentId, _compassCalFitnessParam)->setRawValue(_previousCompassCalFitness);
    }
}