APMSensorsComponentController.cc 35.4 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 29 30 31
    : _sensorsComponent(nullptr)
    , _statusLog(nullptr)
    , _progressBar(nullptr)
    , _nextButton(nullptr)
    , _cancelButton(nullptr)
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
    , _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 &&
DonLakeFlyer's avatar
DonLakeFlyer committed
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 &&
DonLakeFlyer's avatar
DonLakeFlyer committed
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 &&
DonLakeFlyer's avatar
DonLakeFlyer committed
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;
    }

DonLakeFlyer's avatar
DonLakeFlyer committed
338 339 340 341 342 343 344 345 346 347 348 349 350 351 352 353 354 355 356 357 358 359 360 361 362 363 364 365 366 367 368 369 370 371 372 373 374 375 376 377 378 379 380 381 382 383 384 385 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 411 412 413 414 415 416 417 418 419 420 421 422 423 424 425 426 427 428 429 430 431 432 433 434
    QString originalMessageText = text;
    text = text.toLower();

    QStringList hidePrefixList = { QStringLiteral("prearm:"), QStringLiteral("ekf"), QStringLiteral("arm"), QStringLiteral("initialising") };
    for (const QString& hidePrefix: hidePrefixList) {
        if (text.startsWith(hidePrefix)) {
            return;
        }
    }

    if (_calTypeInProgress == CalTypeAccel) {
        if (text == QStringLiteral("place vehicle level and press any key.")) {
            _startVisualCalibration();
            _cancelButton->setEnabled(false);

            // 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;

            _calTypeInProgress = CalTypeAccel;
            _orientationCalDownSideVisible = true;
            _orientationCalUpsideDownSideVisible = true;
            _orientationCalLeftSideVisible = true;
            _orientationCalRightSideVisible = true;
            _orientationCalTailDownSideVisible = true;
            _orientationCalNoseDownSideVisible = true;

            emit orientationCalSidesDoneChanged();
            emit orientationCalSidesVisibleChanged();
            emit orientationCalSidesInProgressChanged();
            _updateAndEmitShowOrientationCalArea(true);
        }

        QString placeVehicle("place vehicle ");
        if (_calTypeInProgress == CalTypeAccel && text.startsWith(placeVehicle)) {
            text = text.right(text.length() - placeVehicle.length());
            if (text.startsWith("level")) {
                _orientationCalDownSideInProgress = true;
                _nextButton->setEnabled(true);
            } else if (text.startsWith("on its left")) {
                _orientationCalDownSideDone =       true;
                _orientationCalDownSideInProgress = false;
                _orientationCalLeftSideInProgress = true;
                _progressBar->setProperty("value", (qreal)(17 / 100.0));
            } else if (text.startsWith("on its right")) {
                _orientationCalLeftSideDone =       true;
                _orientationCalLeftSideInProgress = false;
                _orientationCalRightSideInProgress = true;
                _progressBar->setProperty("value", (qreal)(34 / 100.0));
            } else if (text.startsWith("nose down")) {
                _orientationCalRightSideDone =       true;
                _orientationCalRightSideInProgress = false;
                _orientationCalNoseDownSideInProgress = true;
                _progressBar->setProperty("value", (qreal)(51 / 100.0));
            } else if (text.startsWith("nose up")) {
                _orientationCalNoseDownSideDone =       true;
                _orientationCalNoseDownSideInProgress = false;
                _orientationCalTailDownSideInProgress = true;
                _progressBar->setProperty("value", (qreal)(68 / 100.0));
            } else if (text.startsWith("on its back")) {
                _orientationCalTailDownSideDone =       true;
                _orientationCalTailDownSideInProgress = false;
                _orientationCalUpsideDownSideInProgress = true;
                _progressBar->setProperty("value", (qreal)(85 / 100.0));
            }

            _orientationCalAreaHelpText->setProperty("text", tr("Hold still in the current orientation and press Next when ready"));

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

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

    if (text.contains(QLatin1String("calibration successful"))) {
        _stopCalibration(StopCalibrationSuccess);
Don Gagne's avatar
Don Gagne committed
435 436 437
        return;
    }

DonLakeFlyer's avatar
DonLakeFlyer committed
438 439 440 441 442 443 444 445 446 447 448 449
    if (text.startsWith(QStringLiteral("calibration cancelled"))) {
        _stopCalibration(_waitingForCancel ? StopCalibrationCancelled : StopCalibrationFailed);
        return;
    }

    if (text.startsWith(QStringLiteral("calibration failed"))) {
        _stopCalibration(StopCalibrationFailed);
        return;
    }

#if 0

450
    if (text.contains(QLatin1Literal("progress <"))) {
451 452 453
        QString percent = text.split("<").last().split(">").first();
        bool ok;
        int p = percent.toInt(&ok);
454
        if (ok && _progressBar) {
455 456 457 458 459
            _progressBar->setProperty("value", (float)(p / 100.0));
        }
        return;
    }

460
    QString anyKey(QStringLiteral("and press any"));
Don Gagne's avatar
Don Gagne committed
461
    if (text.contains(anyKey)) {
462
        text = text.left(text.indexOf(anyKey)) + QStringLiteral("and click Next to continue.");
463
        _nextButton->setEnabled(true);
Don Gagne's avatar
Don Gagne committed
464 465 466 467 468
    }

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

469
    if (text.contains(QLatin1String("Calibration successful"))) {
Don Gagne's avatar
Don Gagne committed
470 471 472 473
        _stopCalibration(StopCalibrationSuccess);
        return;
    }

474
    if (text.contains(QLatin1String("FAILED"))) {
Don Gagne's avatar
Don Gagne committed
475 476 477 478 479
        _stopCalibration(StopCalibrationFailed);
        return;
    }

    // All calibration messages start with [cal]
480
    QString calPrefix(QStringLiteral("[cal] "));
Don Gagne's avatar
Don Gagne committed
481 482 483 484 485
    if (!text.startsWith(calPrefix)) {
        return;
    }
    text = text.right(text.length() - calPrefix.length());

486
    QString calStartPrefix(QStringLiteral("calibration started: "));
Don Gagne's avatar
Don Gagne committed
487 488 489 490 491
    if (text.startsWith(calStartPrefix)) {
        text = text.right(text.length() - calStartPrefix.length());
        
        _startVisualCalibration();
        
492
        if (text == QLatin1Literal("accel") || text == QLatin1Literal("mag") || text == QLatin1Literal("gyro")) {
Don Gagne's avatar
Don Gagne committed
493 494 495 496 497 498 499 500 501 502 503 504 505 506 507 508 509 510 511 512 513 514 515 516 517
            // 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") {
518
                _calTypeInProgress = CalTypeAccel;
Don Gagne's avatar
Don Gagne committed
519 520 521 522 523 524 525
                _orientationCalDownSideVisible = true;
                _orientationCalUpsideDownSideVisible = true;
                _orientationCalLeftSideVisible = true;
                _orientationCalRightSideVisible = true;
                _orientationCalTailDownSideVisible = true;
                _orientationCalNoseDownSideVisible = true;
            } else if (text == "mag") {
526
                _calTypeInProgress = CalTypeOffboardCompass;
Don Gagne's avatar
Don Gagne committed
527 528 529 530 531 532 533 534 535 536 537 538 539 540 541 542 543
                _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;
    }
    
544
    if (text.endsWith(QLatin1Literal("orientation detected"))) {
Don Gagne's avatar
Don Gagne committed
545 546 547
        QString side = text.section(" ", 0, 0);
        qDebug() << "Side started" << side;
        
548
        if (side == QLatin1Literal("down")) {
Don Gagne's avatar
Don Gagne committed
549
            _orientationCalDownSideInProgress = true;
550
            if (_calTypeInProgress == CalTypeOffboardCompass) {
Don Gagne's avatar
Don Gagne committed
551 552
                _orientationCalDownSideRotate = true;
            }
553
        } else if (side == QLatin1Literal("up")) {
Don Gagne's avatar
Don Gagne committed
554
            _orientationCalUpsideDownSideInProgress = true;
555
            if (_calTypeInProgress == CalTypeOffboardCompass) {
Don Gagne's avatar
Don Gagne committed
556 557
                _orientationCalUpsideDownSideRotate = true;
            }
558
        } else if (side == QLatin1Literal("left")) {
Don Gagne's avatar
Don Gagne committed
559
            _orientationCalLeftSideInProgress = true;
560
            if (_calTypeInProgress == CalTypeOffboardCompass) {
Don Gagne's avatar
Don Gagne committed
561 562
                _orientationCalLeftSideRotate = true;
            }
563
        } else if (side == QLatin1Literal("right")) {
Don Gagne's avatar
Don Gagne committed
564
            _orientationCalRightSideInProgress = true;
565
            if (_calTypeInProgress == CalTypeOffboardCompass) {
Don Gagne's avatar
Don Gagne committed
566 567
                _orientationCalRightSideRotate = true;
            }
568
        } else if (side == QLatin1Literal("front")) {
Don Gagne's avatar
Don Gagne committed
569
            _orientationCalNoseDownSideInProgress = true;
570
            if (_calTypeInProgress == CalTypeOffboardCompass) {
Don Gagne's avatar
Don Gagne committed
571 572
                _orientationCalNoseDownSideRotate = true;
            }
573
        } else if (side == QLatin1Literal("back")) {
Don Gagne's avatar
Don Gagne committed
574
            _orientationCalTailDownSideInProgress = true;
575
            if (_calTypeInProgress == CalTypeOffboardCompass) {
Don Gagne's avatar
Don Gagne committed
576 577 578 579
                _orientationCalTailDownSideRotate = true;
            }
        }
        
580
        if (_calTypeInProgress == CalTypeOffboardCompass) {
581
            _orientationCalAreaHelpText->setProperty("text", tr("Rotate the vehicle continuously as shown in the diagram until marked as Completed"));
Don Gagne's avatar
Don Gagne committed
582
        } else {
583
            _orientationCalAreaHelpText->setProperty("text", tr("Hold still in the current orientation"));
Don Gagne's avatar
Don Gagne committed
584 585 586 587 588 589 590
        }
        
        emit orientationCalSidesInProgressChanged();
        emit orientationCalSidesRotateChanged();
        return;
    }
    
591
    if (text.endsWith(QLatin1Literal("side done, rotate to a different side"))) {
Don Gagne's avatar
Don Gagne committed
592 593 594
        QString side = text.section(" ", 0, 0);
        qDebug() << "Side finished" << side;
        
595
        if (side == QLatin1Literal("down")) {
Don Gagne's avatar
Don Gagne committed
596 597 598
            _orientationCalDownSideInProgress = false;
            _orientationCalDownSideDone = true;
            _orientationCalDownSideRotate = false;
599
        } else if (side == QLatin1Literal("up")) {
Don Gagne's avatar
Don Gagne committed
600 601
            _orientationCalUpsideDownSideInProgress = false;
            _orientationCalUpsideDownSideDone = true;
602
            _orientationCalUpsideDownSideRotate = false;
603
        } else if (side == QLatin1Literal("left")) {
Don Gagne's avatar
Don Gagne committed
604 605 606
            _orientationCalLeftSideInProgress = false;
            _orientationCalLeftSideDone = true;
            _orientationCalLeftSideRotate = false;
607
        } else if (side == QLatin1Literal("right")) {
Don Gagne's avatar
Don Gagne committed
608 609
            _orientationCalRightSideInProgress = false;
            _orientationCalRightSideDone = true;
610
            _orientationCalRightSideRotate = false;
611
        } else if (side == QLatin1Literal("front")) {
Don Gagne's avatar
Don Gagne committed
612 613 614
            _orientationCalNoseDownSideInProgress = false;
            _orientationCalNoseDownSideDone = true;
            _orientationCalNoseDownSideRotate = false;
615
        } else if (side == QLatin1Literal("back")) {
Don Gagne's avatar
Don Gagne committed
616 617
            _orientationCalTailDownSideInProgress = false;
            _orientationCalTailDownSideDone = true;
618
            _orientationCalTailDownSideRotate = false;
Don Gagne's avatar
Don Gagne committed
619 620
        }
        
621
        _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
622 623 624 625 626 627 628

        emit orientationCalSidesInProgressChanged();
        emit orientationCalSidesDoneChanged();
        emit orientationCalSidesRotateChanged();
        return;
    }
    
629
    if (text.startsWith(QLatin1Literal("calibration done:"))) {
630 631 632 633
        _stopCalibration(StopCalibrationSuccess);
        return;
    }

634
    if (text.startsWith(QLatin1Literal("calibration cancelled"))) {
Don Gagne's avatar
Don Gagne committed
635 636 637 638
        _stopCalibration(_waitingForCancel ? StopCalibrationCancelled : StopCalibrationFailed);
        return;
    }

639
    if (text.startsWith(QLatin1Literal("calibration failed"))) {
640 641 642
        _stopCalibration(StopCalibrationFailed);
        return;
    }
DonLakeFlyer's avatar
DonLakeFlyer committed
643
#endif
Don Gagne's avatar
Don Gagne committed
644 645 646 647 648 649
}

void APMSensorsComponentController::_refreshParams(void)
{
    QStringList fastRefreshList;
    
650 651
    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");
652
    foreach (const QString &paramName, fastRefreshList) {
653
        _vehicle->parameterManager()->refreshParameter(FactSystem::defaultComponentId, paramName);
Don Gagne's avatar
Don Gagne committed
654 655 656
    }
    
    // Now ask for all to refresh
657 658
    _vehicle->parameterManager()->refreshParametersPrefix(FactSystem::defaultComponentId, QStringLiteral("COMPASS_"));
    _vehicle->parameterManager()->refreshParametersPrefix(FactSystem::defaultComponentId, QStringLiteral("INS_"));
Don Gagne's avatar
Don Gagne committed
659 660 661 662 663 664 665 666 667 668 669 670 671 672 673 674
}

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

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

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

676 677 678
    if (_calTypeInProgress == CalTypeOffboardCompass) {
        _waitingForCancel = true;
        emit waitingForCancelChanged();
679
        _compassCal.cancelCalibration();
680 681 682
    } else if (_calTypeInProgress == CalTypeOnboardCompass) {
        _vehicle->sendMavCommand(_vehicle->defaultComponentId(), MAV_CMD_DO_CANCEL_MAG_CAL, true /* showError */);
        _stopCalibration(StopCalibrationCancelled);
683
    } else {
684 685
        _waitingForCancel = true;
        emit waitingForCancelChanged();
686 687 688 689
        // The firmware doesn't always allow us to cancel calibration. The best we can do is wait
        // for it to timeout.
        _uas->stopCalibration();
    }
690

Don Gagne's avatar
Don Gagne committed
691 692 693 694 695
}

void APMSensorsComponentController::nextClicked(void)
{
    mavlink_message_t       msg;
Don Gagne's avatar
Don Gagne committed
696 697 698 699 700
    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
701
                                      1,    // result
Gus Grubba's avatar
Gus Grubba committed
702 703 704 705
                                      0,    // progress
                                      0,    // result_param2
                                      0,    // target_system
                                      0);   // target_component
Don Gagne's avatar
Don Gagne committed
706

707
    _vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg);
708

709
    if (_calTypeInProgress == CalTypeCompassMot) {
710 711
        _stopCalibration(StopCalibrationSuccess);
    }
Don Gagne's avatar
Don Gagne committed
712
}
713 714 715 716 717 718 719 720 721 722

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

bool APMSensorsComponentController::accelSetupNeeded(void) const
{
    return _sensorsComponent->accelSetupNeeded();
}
Don Gagne's avatar
Don Gagne committed
723 724 725 726 727

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

729
void APMSensorsComponentController::_handleCommandAck(mavlink_message_t& message)
Don Gagne's avatar
Don Gagne committed
730
{
731
    if (_calTypeInProgress == CalTypeLevelHorizon) {
Don Gagne's avatar
Don Gagne committed
732 733 734 735 736 737 738 739 740 741 742 743 744 745 746 747
        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;
            }
        }
    }
748 749 750 751 752 753 754 755 756 757 758 759 760 761 762 763 764 765

    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
766
}
767 768 769 770 771 772 773 774

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
775
                                                         << magCalProgress.compass_id << magCalProgress.cal_mask << magCalProgress.completion_pct;
776 777 778 779 780 781 782 783 784

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

785
        if (magCalProgress.compass_id < 3 && compassCalCount != 0) {
786 787 788 789 790 791 792 793 794 795 796 797 798 799 800 801 802
            // 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
803
                                                         << magCalReport.compass_id << magCalReport.cal_mask << magCalReport.cal_status << magCalReport.fitness;
804 805 806 807 808 809 810 811 812 813 814 815 816 817 818 819 820 821 822 823 824 825 826 827 828 829 830 831 832 833 834 835 836 837 838 839 840 841 842 843 844 845 846 847 848 849 850 851 852 853 854 855 856 857 858 859 860 861 862 863 864 865 866 867 868 869 870 871

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