APMSensorsComponentController.cc 30.8 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
    , _progressBar(NULL)
    , _compassButton(NULL)
    , _accelButton(NULL)
    , _compassMotButton(NULL)
    , _levelButton(NULL)
34
    , _calibratePressureButton(NULL)
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
    , _nextButton(NULL)
    , _cancelButton(NULL)
    , _setOrientationsButton(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
66
{
67 68 69
    _compassCal.setVehicle(_vehicle);
    connect(&_compassCal, &APMCompassCal::vehicleTextMessage, this, &APMSensorsComponentController::_handleUASTextMessage);

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

72 73 74 75 76 77 78 79 80 81 82 83 84
    // 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
85 86

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

89 90 91 92 93
APMSensorsComponentController::~APMSensorsComponentController()
{
    _restorePreviousCompassCalFitness();
}

Don Gagne's avatar
Don Gagne committed
94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114
/// 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();
    
    connect(_uas, &UASInterface::textMessageReceived, this, &APMSensorsComponentController::_handleUASTextMessage);
    
    _compassButton->setEnabled(false);
    _accelButton->setEnabled(false);
115
    _compassMotButton->setEnabled(false);
Don Gagne's avatar
Don Gagne committed
116
    _levelButton->setEnabled(false);
117
    _calibratePressureButton->setEnabled(false);
Don Gagne's avatar
Don Gagne committed
118
    _setOrientationsButton->setEnabled(false);
119
    if (_calTypeInProgress == CalTypeAccel || _calTypeInProgress == CalTypeCompassMot) {
120 121
        _nextButton->setEnabled(true);
    }
122
    _cancelButton->setEnabled(_calTypeInProgress == CalTypeOnboardCompass);
Don Gagne's avatar
Don Gagne committed
123 124 125 126 127 128
}

void APMSensorsComponentController::_startVisualCalibration(void)
{
    _compassButton->setEnabled(false);
    _accelButton->setEnabled(false);
129
    _compassMotButton->setEnabled(false);
Don Gagne's avatar
Don Gagne committed
130
    _levelButton->setEnabled(false);
131
    _calibratePressureButton->setEnabled(false);
Don Gagne's avatar
Don Gagne committed
132
    _setOrientationsButton->setEnabled(false);
Don Gagne's avatar
Don Gagne committed
133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167
    _cancelButton->setEnabled(true);

    _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)
{
168
    _vehicle->setConnectionLostEnabled(true);
169

Don Gagne's avatar
Don Gagne committed
170 171 172 173
    disconnect(_uas, &UASInterface::textMessageReceived, this, &APMSensorsComponentController::_handleUASTextMessage);
    
    _compassButton->setEnabled(true);
    _accelButton->setEnabled(true);
174
    _compassMotButton->setEnabled(true);
Don Gagne's avatar
Don Gagne committed
175
    _levelButton->setEnabled(true);
176
    _calibratePressureButton->setEnabled(true);
Don Gagne's avatar
Don Gagne committed
177
    _setOrientationsButton->setEnabled(true);
Don Gagne's avatar
Don Gagne committed
178 179
    _nextButton->setEnabled(false);
    _cancelButton->setEnabled(false);
180

181 182 183 184
    if (_calTypeInProgress == CalTypeOnboardCompass) {
        _restorePreviousCompassCalFitness();
    }

Don Gagne's avatar
Don Gagne committed
185 186 187
    if (code == StopCalibrationSuccess) {
        _resetInternalState();
        _progressBar->setProperty("value", 1);
188 189
        if (parameterExists(FactSystem::defaultComponentId, QStringLiteral("COMPASS_LEARN"))) {
            getParameterFact(FactSystem::defaultComponentId, QStringLiteral("COMPASS_LEARN"))->setRawValue(0);
190
        }
Don Gagne's avatar
Don Gagne committed
191 192 193 194 195 196 197 198 199 200
    } else {
        _progressBar->setProperty("value", 0);
    }
    
    _waitingForCancel = false;
    emit waitingForCancelChanged();

    _refreshParams();
    
    switch (code) {
201 202 203
    case StopCalibrationSuccess:
        _orientationCalAreaHelpText->setProperty("text", "Calibration complete");
        emit resetStatusTextArea();
204
        emit calibrationComplete(_calTypeInProgress);
205 206
        break;

Don Gagne's avatar
Don Gagne committed
207
    case StopCalibrationSuccessShowLog:
208
        emit calibrationComplete(_calTypeInProgress);
Don Gagne's avatar
Don Gagne committed
209 210
        break;

211 212 213 214 215 216 217 218
    case StopCalibrationCancelled:
        emit resetStatusTextArea();
        _hideAllCalAreas();
        break;

    default:
        // Assume failed
        _hideAllCalAreas();
219
        qgcApp()->showMessage(QStringLiteral("Calibration failed. Calibration log will be displayed."));
220
        break;
Don Gagne's avatar
Don Gagne committed
221 222
    }
    
223 224 225 226 227 228 229 230 231 232 233 234 235 236 237 238 239 240 241 242 243 244 245 246 247 248 249 250 251 252 253 254 255 256 257 258 259 260 261
    _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;
            if (getParameterFact(FactSystem::defaultComponentId, "COMPASS_DEV_ID")->rawValue().toInt() > 0) {
                compassBits |= 1 << 0;
                qCDebug(APMSensorsComponentControllerLog) << "Performing onboard compass cal for compass 1";
            } else {
                _rgCompassCalComplete[0] = true;
                _rgCompassCalSucceeded[0] = true;
                _rgCompassCalFitness[0] = 0;
            }
            if (getParameterFact(FactSystem::defaultComponentId, "COMPASS_DEV_ID2")->rawValue().toInt() > 0) {
                compassBits |= 1 << 1;
                qCDebug(APMSensorsComponentControllerLog) << "Performing onboard compass cal for compass 2";
            } else {
                _rgCompassCalComplete[1] = true;
Don Gagne's avatar
Don Gagne committed
262
                _rgCompassCalSucceeded[1] = true;
263 264 265 266 267 268 269
                _rgCompassCalFitness[1] = 0;
            }
            if (getParameterFact(FactSystem::defaultComponentId, "COMPASS_DEV_ID3")->rawValue().toInt() > 0) {
                compassBits |= 1 << 2;
                qCDebug(APMSensorsComponentControllerLog) << "Performing onboard compass cal for compass 3";
            } else {
                _rgCompassCalComplete[2] = true;
Don Gagne's avatar
Don Gagne committed
270
                _rgCompassCalSucceeded[2] = true;
271 272 273 274 275 276 277 278 279 280 281 282 283 284 285 286 287 288 289 290 291 292 293 294 295 296
                _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
297 298 299 300
}

void APMSensorsComponentController::calibrateCompass(void)
{
301 302 303 304 305 306
    // 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
307 308 309 310
}

void APMSensorsComponentController::calibrateAccel(void)
{
311
    _calTypeInProgress = CalTypeAccel;
312
    _vehicle->setConnectionLostEnabled(false);
Don Gagne's avatar
Don Gagne committed
313 314 315 316
    _startLogCalibration();
    _uas->startCalibration(UASInterface::StartCalibrationAccel);
}

317 318
void APMSensorsComponentController::calibrateMotorInterference(void)
{
319
    _calTypeInProgress = CalTypeCompassMot;
320 321 322 323 324 325 326 327
    _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
328 329
void APMSensorsComponentController::levelHorizon(void)
{
330
    _calTypeInProgress = CalTypeLevelHorizon;
Don Gagne's avatar
Don Gagne committed
331 332 333 334 335 336
    _vehicle->setConnectionLostEnabled(false);
    _startLogCalibration();
    _appendStatusLog(tr("Hold the vehicle in its level flight position."));
    _uas->startCalibration(UASInterface::StartCalibrationLevel);
}

337 338 339 340 341 342 343 344 345
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
346 347 348 349 350
void APMSensorsComponentController::_handleUASTextMessage(int uasId, int compId, int severity, QString text)
{
    Q_UNUSED(compId);
    Q_UNUSED(severity);
    
351
    if (uasId != _vehicle->id()) {
Don Gagne's avatar
Don Gagne committed
352 353 354
        return;
    }

355 356
    if (text.startsWith(QLatin1Literal("PreArm:")) || text.startsWith(QLatin1Literal("EKF"))
            || text.startsWith(QLatin1Literal("Arm")) || text.startsWith(QLatin1Literal("Initialising"))) {
Don Gagne's avatar
Don Gagne committed
357 358 359
        return;
    }

360
    if (text.contains(QLatin1Literal("progress <"))) {
361 362 363
        QString percent = text.split("<").last().split(">").first();
        bool ok;
        int p = percent.toInt(&ok);
364
        if (ok && _progressBar) {
365 366 367 368 369
            _progressBar->setProperty("value", (float)(p / 100.0));
        }
        return;
    }

370
    QString anyKey(QStringLiteral("and press any"));
Don Gagne's avatar
Don Gagne committed
371
    if (text.contains(anyKey)) {
372
        text = text.left(text.indexOf(anyKey)) + QStringLiteral("and click Next to continue.");
373
        _nextButton->setEnabled(true);
Don Gagne's avatar
Don Gagne committed
374 375 376 377 378
    }

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

379
    if (text.contains(QLatin1String("Calibration successful"))) {
Don Gagne's avatar
Don Gagne committed
380 381 382 383
        _stopCalibration(StopCalibrationSuccess);
        return;
    }

384
    if (text.contains(QLatin1String("FAILED"))) {
Don Gagne's avatar
Don Gagne committed
385 386 387 388 389
        _stopCalibration(StopCalibrationFailed);
        return;
    }

    // All calibration messages start with [cal]
390
    QString calPrefix(QStringLiteral("[cal] "));
Don Gagne's avatar
Don Gagne committed
391 392 393 394 395
    if (!text.startsWith(calPrefix)) {
        return;
    }
    text = text.right(text.length() - calPrefix.length());

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

        emit orientationCalSidesInProgressChanged();
        emit orientationCalSidesDoneChanged();
        emit orientationCalSidesRotateChanged();
        return;
    }
    
539
    if (text.startsWith(QLatin1Literal("calibration done:"))) {
540 541 542 543
        _stopCalibration(StopCalibrationSuccess);
        return;
    }

544
    if (text.startsWith(QLatin1Literal("calibration cancelled"))) {
Don Gagne's avatar
Don Gagne committed
545 546 547 548
        _stopCalibration(_waitingForCancel ? StopCalibrationCancelled : StopCalibrationFailed);
        return;
    }

549
    if (text.startsWith(QLatin1Literal("calibration failed"))) {
550 551 552
        _stopCalibration(StopCalibrationFailed);
        return;
    }
Don Gagne's avatar
Don Gagne committed
553 554 555 556 557 558
}

void APMSensorsComponentController::_refreshParams(void)
{
    QStringList fastRefreshList;
    
559 560
    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");
561
    foreach (const QString &paramName, fastRefreshList) {
562
        _vehicle->parameterManager()->refreshParameter(FactSystem::defaultComponentId, paramName);
Don Gagne's avatar
Don Gagne committed
563 564 565
    }
    
    // Now ask for all to refresh
566 567
    _vehicle->parameterManager()->refreshParametersPrefix(FactSystem::defaultComponentId, QStringLiteral("COMPASS_"));
    _vehicle->parameterManager()->refreshParametersPrefix(FactSystem::defaultComponentId, QStringLiteral("INS_"));
Don Gagne's avatar
Don Gagne committed
568 569 570 571 572 573 574 575 576 577 578 579 580 581 582 583
}

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

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

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

585 586 587
    if (_calTypeInProgress == CalTypeOffboardCompass) {
        _waitingForCancel = true;
        emit waitingForCancelChanged();
588
        _compassCal.cancelCalibration();
589 590 591
    } else if (_calTypeInProgress == CalTypeOnboardCompass) {
        _vehicle->sendMavCommand(_vehicle->defaultComponentId(), MAV_CMD_DO_CANCEL_MAG_CAL, true /* showError */);
        _stopCalibration(StopCalibrationCancelled);
592
    } else {
593 594
        _waitingForCancel = true;
        emit waitingForCancelChanged();
595 596 597 598
        // The firmware doesn't always allow us to cancel calibration. The best we can do is wait
        // for it to timeout.
        _uas->stopCalibration();
    }
599

Don Gagne's avatar
Don Gagne committed
600 601 602 603 604
}

void APMSensorsComponentController::nextClicked(void)
{
    mavlink_message_t       msg;
Don Gagne's avatar
Don Gagne committed
605 606 607 608 609
    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
610 611
                                      1,    // result
                                      0);   // progress
Don Gagne's avatar
Don Gagne committed
612

613
    _vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg);
614

615
    if (_calTypeInProgress == CalTypeCompassMot) {
616 617
        _stopCalibration(StopCalibrationSuccess);
    }
Don Gagne's avatar
Don Gagne committed
618
}
619 620 621 622 623 624 625 626 627 628

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

bool APMSensorsComponentController::accelSetupNeeded(void) const
{
    return _sensorsComponent->accelSetupNeeded();
}
Don Gagne's avatar
Don Gagne committed
629 630 631 632 633

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

635
void APMSensorsComponentController::_handleCommandAck(mavlink_message_t& message)
Don Gagne's avatar
Don Gagne committed
636
{
637
    if (_calTypeInProgress == CalTypeLevelHorizon) {
Don Gagne's avatar
Don Gagne committed
638 639 640 641 642 643 644 645 646 647 648 649 650 651 652 653
        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;
            }
        }
    }
654 655 656 657 658 659 660 661 662 663 664 665 666 667 668 669 670 671

    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
672
}
673 674 675 676 677 678 679 680

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
681
                                                         << magCalProgress.compass_id << magCalProgress.cal_mask << magCalProgress.completion_pct;
682 683 684 685 686 687 688 689 690 691 692 693 694 695 696 697 698 699 700 701 702 703 704 705 706 707 708

        // 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
709
                                                         << magCalReport.compass_id << magCalReport.cal_mask << magCalReport.cal_status << magCalReport.fitness;
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 764 765 766 767 768 769 770 771 772 773 774 775 776 777

        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);
    }
}