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

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

/// @file
///     @author Don Gagne <don@thegagnes.com>

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

#include <QVariant>
#include <QQmlProperty>

23 24
QGC_LOGGING_CATEGORY(SensorsComponentControllerLog, "SensorsComponentControllerLog")

Don Gagne's avatar
Don Gagne committed
25
SensorsComponentController::SensorsComponentController(void) :
26 27
    _statusLog(NULL),
    _progressBar(NULL),
Don Gagne's avatar
Don Gagne committed
28 29 30 31
    _compassButton(NULL),
    _gyroButton(NULL),
    _accelButton(NULL),
    _airspeedButton(NULL),
32
    _levelButton(NULL),
Don Gagne's avatar
Don Gagne committed
33
    _cancelButton(NULL),
Don Gagne's avatar
Don Gagne committed
34
    _setOrientationsButton(NULL),
Don Gagne's avatar
Don Gagne committed
35
    _showOrientationCalArea(false),
36
    _gyroCalInProgress(false),
Don Gagne's avatar
Don Gagne committed
37 38 39 40 41 42 43 44
    _magCalInProgress(false),
    _accelCalInProgress(false),
    _orientationCalDownSideDone(false),
    _orientationCalUpsideDownSideDone(false),
    _orientationCalLeftSideDone(false),
    _orientationCalRightSideDone(false),
    _orientationCalNoseDownSideDone(false),
    _orientationCalTailDownSideDone(false),
Don Gagne's avatar
Don Gagne committed
45 46 47 48 49 50
    _orientationCalDownSideVisible(false),
    _orientationCalUpsideDownSideVisible(false),
    _orientationCalLeftSideVisible(false),
    _orientationCalRightSideVisible(false),
    _orientationCalNoseDownSideVisible(false),
    _orientationCalTailDownSideVisible(false),
Don Gagne's avatar
Don Gagne committed
51 52 53 54 55 56
    _orientationCalDownSideInProgress(false),
    _orientationCalUpsideDownSideInProgress(false),
    _orientationCalLeftSideInProgress(false),
    _orientationCalRightSideInProgress(false),
    _orientationCalNoseDownSideInProgress(false),
    _orientationCalTailDownSideInProgress(false),
Don Gagne's avatar
Don Gagne committed
57
    _orientationCalDownSideRotate(false),
58
    _orientationCalUpsideDownSideRotate(false),
Don Gagne's avatar
Don Gagne committed
59
    _orientationCalLeftSideRotate(false),
60
    _orientationCalRightSideRotate(false),
Don Gagne's avatar
Don Gagne committed
61
    _orientationCalNoseDownSideRotate(false),
62
    _orientationCalTailDownSideRotate(false),
Don Gagne's avatar
Don Gagne committed
63
    _unknownFirmwareVersion(false),
Don Gagne's avatar
Don Gagne committed
64
    _waitingForCancel(false)
Don Gagne's avatar
Don Gagne committed
65
{
66
}
Don Gagne's avatar
Don Gagne committed
67

68 69 70
bool SensorsComponentController::usingUDPLink(void)
{
    return _vehicle->priorityLink()->getLinkConfiguration()->type() == LinkConfiguration::TypeUdp;
Don Gagne's avatar
Don Gagne committed
71 72 73 74 75
}

/// Appends the specified text to the status log area in the ui
void SensorsComponentController::_appendStatusLog(const QString& text)
{
DonLakeFlyer's avatar
DonLakeFlyer committed
76 77 78 79
    if (!_statusLog) {
        qWarning() << "Internal error";
        return;
    }
Don Gagne's avatar
Don Gagne committed
80 81 82 83 84 85 86 87 88
    
    QVariant returnedValue;
    QVariant varText = text;
    QMetaObject::invokeMethod(_statusLog,
                              "append",
                              Q_RETURN_ARG(QVariant, returnedValue),
                              Q_ARG(QVariant, varText));
}

Don Gagne's avatar
Don Gagne committed
89
void SensorsComponentController::_startLogCalibration(void)
Don Gagne's avatar
Don Gagne committed
90
{
Don Gagne's avatar
Don Gagne committed
91
    _unknownFirmwareVersion = false;
92 93
    _hideAllCalAreas();
    
Don Gagne's avatar
Don Gagne committed
94 95 96 97 98 99 100
    connect(_uas, &UASInterface::textMessageReceived, this, &SensorsComponentController::_handleUASTextMessage);
    
    _cancelButton->setEnabled(false);
}

void SensorsComponentController::_startVisualCalibration(void)
{
101 102 103 104
    _compassButton->setEnabled(false);
    _gyroButton->setEnabled(false);
    _accelButton->setEnabled(false);
    _airspeedButton->setEnabled(false);
105
    _levelButton->setEnabled(false);
Don Gagne's avatar
Don Gagne committed
106
    _setOrientationsButton->setEnabled(false);
Don Gagne's avatar
Don Gagne committed
107
    _cancelButton->setEnabled(true);
108 109

    _resetInternalState();
Don Gagne's avatar
Don Gagne committed
110 111
    
    _progressBar->setProperty("value", 0);
112 113
}

114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139
void SensorsComponentController::_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();
}

Don Gagne's avatar
Don Gagne committed
140
void SensorsComponentController::_stopCalibration(SensorsComponentController::StopCalibrationCode code)
141
{
Don Gagne's avatar
Don Gagne committed
142
    disconnect(_uas, &UASInterface::textMessageReceived, this, &SensorsComponentController::_handleUASTextMessage);
Don Gagne's avatar
Don Gagne committed
143
    
144 145 146 147
    _compassButton->setEnabled(true);
    _gyroButton->setEnabled(true);
    _accelButton->setEnabled(true);
    _airspeedButton->setEnabled(true);
148
    _levelButton->setEnabled(true);
Don Gagne's avatar
Don Gagne committed
149
    _setOrientationsButton->setEnabled(true);
Don Gagne's avatar
Don Gagne committed
150 151 152
    _cancelButton->setEnabled(false);
    
    if (code == StopCalibrationSuccess) {
153
        _resetInternalState();
Don Gagne's avatar
Don Gagne committed
154 155 156 157 158
        
        _progressBar->setProperty("value", 1);
    } else {
        _progressBar->setProperty("value", 0);
    }
159
    
Don Gagne's avatar
Don Gagne committed
160 161 162
    _waitingForCancel = false;
    emit waitingForCancelChanged();

163 164
    _refreshParams();
    
Don Gagne's avatar
Don Gagne committed
165 166
    switch (code) {
        case StopCalibrationSuccess:
167
            _orientationCalAreaHelpText->setProperty("text", tr("Calibration complete"));
Don Gagne's avatar
Don Gagne committed
168 169 170 171 172 173 174 175 176 177 178 179 180 181
            emit resetStatusTextArea();
            if (_magCalInProgress) {
                emit setCompassRotations();
            }
            break;
            
        case StopCalibrationCancelled:
            emit resetStatusTextArea();
            _hideAllCalAreas();
            break;
            
        default:
            // Assume failed
            _hideAllCalAreas();
182
            qgcApp()->showMessage(tr("Calibration failed. Calibration log will be displayed."));
Don Gagne's avatar
Don Gagne committed
183
            break;
184
    }
Don Gagne's avatar
Don Gagne committed
185 186 187 188
    
    _magCalInProgress = false;
    _accelCalInProgress = false;
    _gyroCalInProgress = false;
189 190 191 192
}

void SensorsComponentController::calibrateGyro(void)
{
Don Gagne's avatar
Don Gagne committed
193 194
    _startLogCalibration();
    _uas->startCalibration(UASInterface::StartCalibrationGyro);
Don Gagne's avatar
Don Gagne committed
195 196 197 198
}

void SensorsComponentController::calibrateCompass(void)
{
Don Gagne's avatar
Don Gagne committed
199 200
    _startLogCalibration();
    _uas->startCalibration(UASInterface::StartCalibrationMag);
Don Gagne's avatar
Don Gagne committed
201 202 203 204
}

void SensorsComponentController::calibrateAccel(void)
{
Don Gagne's avatar
Don Gagne committed
205 206
    _startLogCalibration();
    _uas->startCalibration(UASInterface::StartCalibrationAccel);
Don Gagne's avatar
Don Gagne committed
207 208
}

209 210 211 212 213 214
void SensorsComponentController::calibrateLevel(void)
{
    _startLogCalibration();
    _uas->startCalibration(UASInterface::StartCalibrationLevel);
}

Don Gagne's avatar
Don Gagne committed
215 216
void SensorsComponentController::calibrateAirspeed(void)
{
Don Gagne's avatar
Don Gagne committed
217 218
    _startLogCalibration();
    _uas->startCalibration(UASInterface::StartCalibrationAirspeed);
Don Gagne's avatar
Don Gagne committed
219 220 221 222 223 224 225
}

void SensorsComponentController::_handleUASTextMessage(int uasId, int compId, int severity, QString text)
{
    Q_UNUSED(compId);
    Q_UNUSED(severity);
    
226
    if (uasId != _vehicle->id()) {
Don Gagne's avatar
Don Gagne committed
227 228 229
        return;
    }
    
230 231 232 233 234
    if (text.contains("progress <")) {
        QString percent = text.split("<").last().split(">").first();
        bool ok;
        int p = percent.toInt(&ok);
        if (ok) {
DonLakeFlyer's avatar
DonLakeFlyer committed
235 236 237 238 239
            if (_progressBar) {
                _progressBar->setProperty("value", (float)(p / 100.0));
            } else {
                qWarning() << "Internal error";
            }
240 241 242 243
        }
        return;
    }

Don Gagne's avatar
Don Gagne committed
244
    _appendStatusLog(text);
245
    qCDebug(SensorsComponentControllerLog) << text;
Don Gagne's avatar
Don Gagne committed
246 247 248 249 250 251 252 253 254 255 256 257
    
    if (_unknownFirmwareVersion) {
        // We don't know how to do visual cal with the version of firwmare
        return;
    }
    
    // All calibration messages start with [cal]
    QString calPrefix("[cal] ");
    if (!text.startsWith(calPrefix)) {
        return;
    }
    text = text.right(text.length() - calPrefix.length());
258

Don Gagne's avatar
Don Gagne committed
259 260 261 262 263 264
    QString calStartPrefix("calibration started: ");
    if (text.startsWith(calStartPrefix)) {
        text = text.right(text.length() - calStartPrefix.length());
        
        // Split version number and cal type
        QStringList parts = text.split(" ");
265
        if (parts.count() != 2 && parts[0].toInt() != _supportedFirmwareCalVersion) {
Don Gagne's avatar
Don Gagne committed
266
            _unknownFirmwareVersion = true;
267
            QString msg = tr("Unsupported calibration firmware version, using log");
268 269
            _appendStatusLog(msg);
            qDebug() << msg;
Don Gagne's avatar
Don Gagne committed
270
            return;
Don Gagne's avatar
Don Gagne committed
271
        }
Don Gagne's avatar
Don Gagne committed
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 297 298
        
        _startVisualCalibration();
        
        text = parts[1];
        if (text == "accel" || text == "mag" || text == "gyro") {
            // 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;
            
299
            _orientationCalAreaHelpText->setProperty("text", tr("Place your vehicle into one of the Incomplete orientations shown below and hold it still"));
Don Gagne's avatar
Don Gagne committed
300 301 302 303 304 305 306 307 308 309
            
            if (text == "accel") {
                _accelCalInProgress = true;
                _orientationCalDownSideVisible = true;
                _orientationCalUpsideDownSideVisible = true;
                _orientationCalLeftSideVisible = true;
                _orientationCalRightSideVisible = true;
                _orientationCalTailDownSideVisible = true;
                _orientationCalNoseDownSideVisible = true;
            } else if (text == "mag") {
310 311 312 313

                // Work out what the autopilot is configured to
                int sides = 0;

314
                if (_vehicle->parameterManager()->parameterExists(FactSystem::defaultComponentId, "CAL_MAG_SIDES")) {
315
                    // Read the requested calibration directions off the system
316
                    sides = _vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, "CAL_MAG_SIDES")->rawValue().toFloat();
317 318 319 320 321
                } else {
                    // There is no valid setting, default to all six sides
                    sides = (1 << 5) | (1 << 4) | (1 << 3) | (1 << 2) | (1 << 1) | (1 << 0);
                }

Don Gagne's avatar
Don Gagne committed
322
                _magCalInProgress = true;
323 324 325 326 327 328
                _orientationCalTailDownSideVisible =   ((sides & (1 << 0)) > 0);
                _orientationCalNoseDownSideVisible =   ((sides & (1 << 1)) > 0);
                _orientationCalLeftSideVisible =       ((sides & (1 << 2)) > 0);
                _orientationCalRightSideVisible =      ((sides & (1 << 3)) > 0);
                _orientationCalUpsideDownSideVisible = ((sides & (1 << 4)) > 0);
                _orientationCalDownSideVisible =       ((sides & (1 << 5)) > 0);
Don Gagne's avatar
Don Gagne committed
329 330 331 332
            } else if (text == "gyro") {
                _gyroCalInProgress = true;
                _orientationCalDownSideVisible = true;
            } else {
DonLakeFlyer's avatar
DonLakeFlyer committed
333
                qWarning() << "Unknown calibration message type" << text;
Don Gagne's avatar
Don Gagne committed
334 335 336 337 338
            }
            emit orientationCalSidesDoneChanged();
            emit orientationCalSidesVisibleChanged();
            emit orientationCalSidesInProgressChanged();
            _updateAndEmitShowOrientationCalArea(true);
Don Gagne's avatar
Don Gagne committed
339
        }
Don Gagne's avatar
Don Gagne committed
340 341 342 343
        return;
    }
    
    if (text.endsWith("orientation detected")) {
Don Gagne's avatar
Don Gagne committed
344 345
        QString side = text.section(" ", 0, 0);
        qDebug() << "Side started" << side;
Don Gagne's avatar
Don Gagne committed
346
        
Don Gagne's avatar
Don Gagne committed
347 348
        if (side == "down") {
            _orientationCalDownSideInProgress = true;
Don Gagne's avatar
Don Gagne committed
349 350 351
            if (_magCalInProgress) {
                _orientationCalDownSideRotate = true;
            }
352
        } else if (side == "up") {
Don Gagne's avatar
Don Gagne committed
353
            _orientationCalUpsideDownSideInProgress = true;
354 355 356
            if (_magCalInProgress) {
                _orientationCalUpsideDownSideRotate = true;
            }
357
        } else if (side == "left") {
Don Gagne's avatar
Don Gagne committed
358
            _orientationCalLeftSideInProgress = true;
Don Gagne's avatar
Don Gagne committed
359 360 361
            if (_magCalInProgress) {
                _orientationCalLeftSideRotate = true;
            }
362
        } else if (side == "right") {
Don Gagne's avatar
Don Gagne committed
363
            _orientationCalRightSideInProgress = true;
364 365 366
            if (_magCalInProgress) {
                _orientationCalRightSideRotate = true;
            }
367
        } else if (side == "front") {
Don Gagne's avatar
Don Gagne committed
368
            _orientationCalNoseDownSideInProgress = true;
Don Gagne's avatar
Don Gagne committed
369 370 371
            if (_magCalInProgress) {
                _orientationCalNoseDownSideRotate = true;
            }
372
        } else if (side == "back") {
Don Gagne's avatar
Don Gagne committed
373
            _orientationCalTailDownSideInProgress = true;
374 375 376
            if (_magCalInProgress) {
                _orientationCalTailDownSideRotate = true;
            }
377
        }
Don Gagne's avatar
Don Gagne committed
378 379
        
        if (_magCalInProgress) {
380
            _orientationCalAreaHelpText->setProperty("text", tr("Rotate the vehicle continuously as shown in the diagram until marked as Completed"));
Don Gagne's avatar
Don Gagne committed
381
        } else {
382
            _orientationCalAreaHelpText->setProperty("text", tr("Hold still in the current orientation"));
Don Gagne's avatar
Don Gagne committed
383 384
        }
        
Don Gagne's avatar
Don Gagne committed
385
        emit orientationCalSidesInProgressChanged();
Don Gagne's avatar
Don Gagne committed
386 387 388 389 390
        emit orientationCalSidesRotateChanged();
        return;
    }
    
    if (text.endsWith("side done, rotate to a different side")) {
391 392
        QString side = text.section(" ", 0, 0);
        qDebug() << "Side finished" << side;
Don Gagne's avatar
Don Gagne committed
393
        
394
        if (side == "down") {
Don Gagne's avatar
Don Gagne committed
395 396
            _orientationCalDownSideInProgress = false;
            _orientationCalDownSideDone = true;
Don Gagne's avatar
Don Gagne committed
397
            _orientationCalDownSideRotate = false;
398
        } else if (side == "up") {
Don Gagne's avatar
Don Gagne committed
399 400
            _orientationCalUpsideDownSideInProgress = false;
            _orientationCalUpsideDownSideDone = true;
401
            _orientationCalUpsideDownSideRotate = false;
402
        } else if (side == "left") {
Don Gagne's avatar
Don Gagne committed
403 404
            _orientationCalLeftSideInProgress = false;
            _orientationCalLeftSideDone = true;
Don Gagne's avatar
Don Gagne committed
405
            _orientationCalLeftSideRotate = false;
406
        } else if (side == "right") {
Don Gagne's avatar
Don Gagne committed
407 408
            _orientationCalRightSideInProgress = false;
            _orientationCalRightSideDone = true;
409
            _orientationCalRightSideRotate = false;
410
        } else if (side == "front") {
Don Gagne's avatar
Don Gagne committed
411 412
            _orientationCalNoseDownSideInProgress = false;
            _orientationCalNoseDownSideDone = true;
Don Gagne's avatar
Don Gagne committed
413
            _orientationCalNoseDownSideRotate = false;
414
        } else if (side == "back") {
Don Gagne's avatar
Don Gagne committed
415 416
            _orientationCalTailDownSideInProgress = false;
            _orientationCalTailDownSideDone = true;
417
            _orientationCalTailDownSideRotate = false;
418
        }
Don Gagne's avatar
Don Gagne committed
419
        
420
        _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
421

Don Gagne's avatar
Don Gagne committed
422 423
        emit orientationCalSidesInProgressChanged();
        emit orientationCalSidesDoneChanged();
Don Gagne's avatar
Don Gagne committed
424 425
        emit orientationCalSidesRotateChanged();
        return;
426
    }
427 428

    if (text.endsWith("side already completed")) {
429
        _orientationCalAreaHelpText->setProperty("text", tr("Orientation already completed, place you vehicle into one of the incomplete orientations shown below and hold it still"));
430 431
        return;
    }
Don Gagne's avatar
Don Gagne committed
432
    
Don Gagne's avatar
Don Gagne committed
433 434 435 436
    QString calCompletePrefix("calibration done:");
    if (text.startsWith(calCompletePrefix)) {
        _stopCalibration(StopCalibrationSuccess);
        return;
437
    }
Don Gagne's avatar
Don Gagne committed
438 439 440 441
    
    if (text.startsWith("calibration cancelled")) {
        _stopCalibration(_waitingForCancel ? StopCalibrationCancelled : StopCalibrationFailed);
        return;
442 443
    }
    
Don Gagne's avatar
Don Gagne committed
444 445 446 447 448 449 450 451
    if (text.startsWith("calibration failed")) {
        _stopCalibration(StopCalibrationFailed);
        return;
    }
}

void SensorsComponentController::_refreshParams(void)
{
Don Gagne's avatar
Don Gagne committed
452 453 454 455
    QStringList fastRefreshList;
    
    // We ask for a refresh on these first so that the rotation combo show up as fast as possible
    fastRefreshList << "CAL_MAG0_ID" << "CAL_MAG1_ID" << "CAL_MAG2_ID" << "CAL_MAG0_ROT" << "CAL_MAG1_ROT" << "CAL_MAG2_ROT";
456
    foreach (const QString &paramName, fastRefreshList) {
457
        _vehicle->parameterManager()->refreshParameter(FactSystem::defaultComponentId, paramName);
Don Gagne's avatar
Don Gagne committed
458 459 460
    }
    
    // Now ask for all to refresh
461 462
    _vehicle->parameterManager()->refreshParametersPrefix(FactSystem::defaultComponentId, "CAL_");
    _vehicle->parameterManager()->refreshParametersPrefix(FactSystem::defaultComponentId, "SENS_");
Don Gagne's avatar
Don Gagne committed
463 464
}

Don Gagne's avatar
Don Gagne committed
465
void SensorsComponentController::_updateAndEmitShowOrientationCalArea(bool show)
466
{
Don Gagne's avatar
Don Gagne committed
467 468
    _showOrientationCalArea = show;
    emit showOrientationCalAreaChanged();
469 470 471 472
}

void SensorsComponentController::_hideAllCalAreas(void)
{
Don Gagne's avatar
Don Gagne committed
473 474 475
    _updateAndEmitShowOrientationCalArea(false);
}

Don Gagne's avatar
Don Gagne committed
476
void SensorsComponentController::cancelCalibration(void)
Don Gagne's avatar
Don Gagne committed
477
{
Don Gagne's avatar
Don Gagne committed
478 479 480 481 482 483
    // The firmware doesn't allow us to cancel calibration. The best we can do is wait
    // for it to timeout.
    _waitingForCancel = true;
    emit waitingForCancelChanged();
    _cancelButton->setEnabled(false);
    _uas->stopCalibration();
484
}