SensorsComponentController.cc 18.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
#include "SensorsComponentController.h"
#include "QGCMAVLink.h"
12
#include "UAS.h"
13
#include "QGCApplication.h"
14
#include "ParameterManager.h"
Don Gagne's avatar
Don Gagne committed
15 16 17 18

#include <QVariant>
#include <QQmlProperty>

19 20
QGC_LOGGING_CATEGORY(SensorsComponentControllerLog, "SensorsComponentControllerLog")

21
SensorsComponentController::SensorsComponentController(void)
22 23 24 25 26 27 28 29 30
    : _statusLog                                (nullptr)
    , _progressBar                              (nullptr)
    , _compassButton                            (nullptr)
    , _gyroButton                               (nullptr)
    , _accelButton                              (nullptr)
    , _airspeedButton                           (nullptr)
    , _levelButton                              (nullptr)
    , _cancelButton                             (nullptr)
    , _setOrientationsButton                    (nullptr)
31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62
    , _showOrientationCalArea                   (false)
    , _gyroCalInProgress                        (false)
    , _magCalInProgress                         (false)
    , _accelCalInProgress                       (false)
    , _airspeedCalInProgress                    (false)
    , _levelCalInProgress                       (false)
    , _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)
    , _unknownFirmwareVersion                   (false)
    , _waitingForCancel                         (false)
Don Gagne's avatar
Don Gagne committed
63
{
64
}
Don Gagne's avatar
Don Gagne committed
65

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

/// Appends the specified text to the status log area in the ui
void SensorsComponentController::_appendStatusLog(const QString& text)
{
DonLakeFlyer's avatar
DonLakeFlyer committed
74 75 76 77
    if (!_statusLog) {
        qWarning() << "Internal error";
        return;
    }
Don Gagne's avatar
Don Gagne committed
78 79 80 81 82 83 84 85 86
    
    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
87
void SensorsComponentController::_startLogCalibration(void)
Don Gagne's avatar
Don Gagne committed
88
{
Don Gagne's avatar
Don Gagne committed
89
    _unknownFirmwareVersion = false;
90 91
    _hideAllCalAreas();
    
92
    connect(_vehicle, &Vehicle::textMessageReceived, this, &SensorsComponentController::_handleUASTextMessage);
Don Gagne's avatar
Don Gagne committed
93 94 95 96 97 98
    
    _cancelButton->setEnabled(false);
}

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

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

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

161 162
    _refreshParams();
    
Don Gagne's avatar
Don Gagne committed
163 164
    switch (code) {
        case StopCalibrationSuccess:
165
            _orientationCalAreaHelpText->setProperty("text", tr("Calibration complete"));
166 167 168
            if (!_airspeedCalInProgress && !_levelCalInProgress) {
                emit resetStatusTextArea();
            }
Don Gagne's avatar
Don Gagne committed
169
            if (_magCalInProgress) {
170
                emit magCalComplete();
Don Gagne's avatar
Don Gagne committed
171 172 173 174 175 176 177 178 179 180 181
            }
            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
    _airspeedCalInProgress = false;
190 191 192 193
}

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

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

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

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

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

void SensorsComponentController::_handleUASTextMessage(int uasId, int compId, int severity, QString text)
{
    Q_UNUSED(compId);
    Q_UNUSED(severity);
    
227
    if (uasId != _vehicle->id()) {
Don Gagne's avatar
Don Gagne committed
228 229 230
        return;
    }
    
231 232 233 234 235
    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
236 237 238 239 240
            if (_progressBar) {
                _progressBar->setProperty("value", (float)(p / 100.0));
            } else {
                qWarning() << "Internal error";
            }
241 242 243 244
        }
        return;
    }

Don Gagne's avatar
Don Gagne committed
245
    _appendStatusLog(text);
246
    qCDebug(SensorsComponentControllerLog) << text;
Don Gagne's avatar
Don Gagne committed
247 248 249 250 251 252 253 254 255 256 257 258
    
    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());
259

Don Gagne's avatar
Don Gagne committed
260 261 262 263 264 265
    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(" ");
266
        if (parts.count() != 2 && parts[0].toInt() != _supportedFirmwareCalVersion) {
Don Gagne's avatar
Don Gagne committed
267
            _unknownFirmwareVersion = true;
268
            QString msg = tr("Unsupported calibration firmware version, using log");
269 270
            _appendStatusLog(msg);
            qDebug() << msg;
Don Gagne's avatar
Don Gagne committed
271
            return;
Don Gagne's avatar
Don Gagne committed
272
        }
Don Gagne's avatar
Don Gagne committed
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 299
        
        _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;
            
300
            _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
301 302 303 304 305 306 307 308 309 310
            
            if (text == "accel") {
                _accelCalInProgress = true;
                _orientationCalDownSideVisible = true;
                _orientationCalUpsideDownSideVisible = true;
                _orientationCalLeftSideVisible = true;
                _orientationCalRightSideVisible = true;
                _orientationCalTailDownSideVisible = true;
                _orientationCalNoseDownSideVisible = true;
            } else if (text == "mag") {
311 312 313 314

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

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

Don Gagne's avatar
Don Gagne committed
427 428
        emit orientationCalSidesInProgressChanged();
        emit orientationCalSidesDoneChanged();
Don Gagne's avatar
Don Gagne committed
429 430
        emit orientationCalSidesRotateChanged();
        return;
431
    }
432 433

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

void SensorsComponentController::_refreshParams(void)
{
Don Gagne's avatar
Don Gagne committed
457 458 459 460
    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";
461
    foreach (const QString &paramName, fastRefreshList) {
462
        _vehicle->parameterManager()->refreshParameter(FactSystem::defaultComponentId, paramName);
Don Gagne's avatar
Don Gagne committed
463 464 465
    }
    
    // Now ask for all to refresh
466 467
    _vehicle->parameterManager()->refreshParametersPrefix(FactSystem::defaultComponentId, "CAL_");
    _vehicle->parameterManager()->refreshParametersPrefix(FactSystem::defaultComponentId, "SENS_");
Don Gagne's avatar
Don Gagne committed
468 469
}

Don Gagne's avatar
Don Gagne committed
470
void SensorsComponentController::_updateAndEmitShowOrientationCalArea(bool show)
471
{
Don Gagne's avatar
Don Gagne committed
472 473
    _showOrientationCalArea = show;
    emit showOrientationCalAreaChanged();
474 475 476 477
}

void SensorsComponentController::_hideAllCalAreas(void)
{
Don Gagne's avatar
Don Gagne committed
478 479 480
    _updateAndEmitShowOrientationCalArea(false);
}

Don Gagne's avatar
Don Gagne committed
481
void SensorsComponentController::cancelCalibration(void)
Don Gagne's avatar
Don Gagne committed
482
{
Don Gagne's avatar
Don Gagne committed
483 484 485 486 487 488
    // 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();
489
}