SensorsComponentController.cc 16.6 KB
Newer Older
Don Gagne's avatar
Don Gagne committed
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28
/*=====================================================================
 
 QGroundControl Open Source Ground Control Station
 
 (c) 2009, 2015 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
 
 This file is part of the QGROUNDCONTROL project
 
 QGROUNDCONTROL is free software: you can redistribute it and/or modify
 it under the terms of the GNU General Public License as published by
 the Free Software Foundation, either version 3 of the License, or
 (at your option) any later version.
 
 QGROUNDCONTROL is distributed in the hope that it will be useful,
 but WITHOUT ANY WARRANTY; without even the implied warranty of
 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 GNU General Public License for more details.
 
 You should have received a copy of the GNU General Public License
 along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.
 
 ======================================================================*/

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

#include "SensorsComponentController.h"
#include "QGCMAVLink.h"
29
#include "UAS.h"
30
#include "QGCApplication.h"
Don Gagne's avatar
Don Gagne committed
31 32 33 34

#include <QVariant>
#include <QQmlProperty>

35 36
QGC_LOGGING_CATEGORY(SensorsComponentControllerLog, "SensorsComponentControllerLog")

Don Gagne's avatar
Don Gagne committed
37
SensorsComponentController::SensorsComponentController(void) :
38 39
    _statusLog(NULL),
    _progressBar(NULL),
Don Gagne's avatar
Don Gagne committed
40 41 42 43
    _compassButton(NULL),
    _gyroButton(NULL),
    _accelButton(NULL),
    _airspeedButton(NULL),
44
    _levelButton(NULL),
Don Gagne's avatar
Don Gagne committed
45
    _cancelButton(NULL),
Don Gagne's avatar
Don Gagne committed
46
    _showOrientationCalArea(false),
47
    _gyroCalInProgress(false),
Don Gagne's avatar
Don Gagne committed
48 49 50 51 52 53 54 55
    _magCalInProgress(false),
    _accelCalInProgress(false),
    _orientationCalDownSideDone(false),
    _orientationCalUpsideDownSideDone(false),
    _orientationCalLeftSideDone(false),
    _orientationCalRightSideDone(false),
    _orientationCalNoseDownSideDone(false),
    _orientationCalTailDownSideDone(false),
Don Gagne's avatar
Don Gagne committed
56 57 58 59 60 61
    _orientationCalDownSideVisible(false),
    _orientationCalUpsideDownSideVisible(false),
    _orientationCalLeftSideVisible(false),
    _orientationCalRightSideVisible(false),
    _orientationCalNoseDownSideVisible(false),
    _orientationCalTailDownSideVisible(false),
Don Gagne's avatar
Don Gagne committed
62 63 64 65 66 67
    _orientationCalDownSideInProgress(false),
    _orientationCalUpsideDownSideInProgress(false),
    _orientationCalLeftSideInProgress(false),
    _orientationCalRightSideInProgress(false),
    _orientationCalNoseDownSideInProgress(false),
    _orientationCalTailDownSideInProgress(false),
Don Gagne's avatar
Don Gagne committed
68
    _orientationCalDownSideRotate(false),
69
    _orientationCalUpsideDownSideRotate(false),
Don Gagne's avatar
Don Gagne committed
70
    _orientationCalLeftSideRotate(false),
71
    _orientationCalRightSideRotate(false),
Don Gagne's avatar
Don Gagne committed
72
    _orientationCalNoseDownSideRotate(false),
73
    _orientationCalTailDownSideRotate(false),
Don Gagne's avatar
Don Gagne committed
74
    _unknownFirmwareVersion(false),
Don Gagne's avatar
Don Gagne committed
75
    _waitingForCancel(false)
Don Gagne's avatar
Don Gagne committed
76
{
Don Gagne's avatar
Don Gagne committed
77

Don Gagne's avatar
Don Gagne committed
78 79 80 81 82 83 84 85 86 87 88 89 90 91 92
}

/// Appends the specified text to the status log area in the ui
void SensorsComponentController::_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));
}

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

void SensorsComponentController::_startVisualCalibration(void)
{
105 106 107 108
    _compassButton->setEnabled(false);
    _gyroButton->setEnabled(false);
    _accelButton->setEnabled(false);
    _airspeedButton->setEnabled(false);
109
    _levelButton->setEnabled(false);
Don Gagne's avatar
Don Gagne committed
110
    _cancelButton->setEnabled(true);
111 112

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

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

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

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

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

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

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

void SensorsComponentController::_handleUASTextMessage(int uasId, int compId, int severity, QString text)
{
    Q_UNUSED(compId);
    Q_UNUSED(severity);
    
228
    UASInterface* uas = _autopilot->vehicle()->uas();
Don Gagne's avatar
Don Gagne committed
229 230 231 232 233
    Q_ASSERT(uas);
    if (uasId != uas->getUASID()) {
        return;
    }
    
234 235 236 237 238 239 240 241 242 243 244
    if (text.contains("progress <")) {
        QString percent = text.split("<").last().split(">").first();
        bool ok;
        int p = percent.toInt(&ok);
        if (ok) {
            Q_ASSERT(_progressBar);
            _progressBar->setProperty("value", (float)(p / 100.0));
        }
        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 269 270
            QString msg = "Unsupported calibration firmware version, using log";
            _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 300 301 302 303 304 305 306 307 308 309 310 311 312
        
        _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;
            
            _orientationCalAreaHelpText->setProperty("text", "Place your vehicle into one of the Incomplete orientations shown below and hold it still");
            
            if (text == "accel") {
                _accelCalInProgress = true;
                _orientationCalDownSideVisible = true;
                _orientationCalUpsideDownSideVisible = true;
                _orientationCalLeftSideVisible = true;
                _orientationCalRightSideVisible = true;
                _orientationCalTailDownSideVisible = true;
                _orientationCalNoseDownSideVisible = true;
            } else if (text == "mag") {
                _magCalInProgress = true;
                _orientationCalDownSideVisible = true;
313
                _orientationCalUpsideDownSideVisible = true;
Don Gagne's avatar
Don Gagne committed
314
                _orientationCalLeftSideVisible = true;
315 316
                _orientationCalRightSideVisible = true;
                _orientationCalTailDownSideVisible = true;
Don Gagne's avatar
Don Gagne committed
317 318 319 320 321 322 323 324 325 326 327
                _orientationCalNoseDownSideVisible = true;
            } else if (text == "gyro") {
                _gyroCalInProgress = true;
                _orientationCalDownSideVisible = true;
            } else {
                Q_ASSERT(false);
            }
            emit orientationCalSidesDoneChanged();
            emit orientationCalSidesVisibleChanged();
            emit orientationCalSidesInProgressChanged();
            _updateAndEmitShowOrientationCalArea(true);
Don Gagne's avatar
Don Gagne committed
328
        }
Don Gagne's avatar
Don Gagne committed
329 330 331 332
        return;
    }
    
    if (text.endsWith("orientation detected")) {
Don Gagne's avatar
Don Gagne committed
333 334
        QString side = text.section(" ", 0, 0);
        qDebug() << "Side started" << side;
Don Gagne's avatar
Don Gagne committed
335
        
Don Gagne's avatar
Don Gagne committed
336 337
        if (side == "down") {
            _orientationCalDownSideInProgress = true;
Don Gagne's avatar
Don Gagne committed
338 339 340
            if (_magCalInProgress) {
                _orientationCalDownSideRotate = true;
            }
341
        } else if (side == "up") {
Don Gagne's avatar
Don Gagne committed
342
            _orientationCalUpsideDownSideInProgress = true;
343 344 345
            if (_magCalInProgress) {
                _orientationCalUpsideDownSideRotate = true;
            }
346
        } else if (side == "left") {
Don Gagne's avatar
Don Gagne committed
347
            _orientationCalLeftSideInProgress = true;
Don Gagne's avatar
Don Gagne committed
348 349 350
            if (_magCalInProgress) {
                _orientationCalLeftSideRotate = true;
            }
351
        } else if (side == "right") {
Don Gagne's avatar
Don Gagne committed
352
            _orientationCalRightSideInProgress = true;
353 354 355
            if (_magCalInProgress) {
                _orientationCalRightSideRotate = true;
            }
356
        } else if (side == "front") {
Don Gagne's avatar
Don Gagne committed
357
            _orientationCalNoseDownSideInProgress = true;
Don Gagne's avatar
Don Gagne committed
358 359 360
            if (_magCalInProgress) {
                _orientationCalNoseDownSideRotate = true;
            }
361
        } else if (side == "back") {
Don Gagne's avatar
Don Gagne committed
362
            _orientationCalTailDownSideInProgress = true;
363 364 365
            if (_magCalInProgress) {
                _orientationCalTailDownSideRotate = true;
            }
366
        }
Don Gagne's avatar
Don Gagne committed
367 368 369 370 371 372 373
        
        if (_magCalInProgress) {
            _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");
        }
        
Don Gagne's avatar
Don Gagne committed
374
        emit orientationCalSidesInProgressChanged();
Don Gagne's avatar
Don Gagne committed
375 376 377 378 379
        emit orientationCalSidesRotateChanged();
        return;
    }
    
    if (text.endsWith("side done, rotate to a different side")) {
380 381
        QString side = text.section(" ", 0, 0);
        qDebug() << "Side finished" << side;
Don Gagne's avatar
Don Gagne committed
382
        
383
        if (side == "down") {
Don Gagne's avatar
Don Gagne committed
384 385
            _orientationCalDownSideInProgress = false;
            _orientationCalDownSideDone = true;
Don Gagne's avatar
Don Gagne committed
386
            _orientationCalDownSideRotate = false;
387
        } else if (side == "up") {
Don Gagne's avatar
Don Gagne committed
388 389
            _orientationCalUpsideDownSideInProgress = false;
            _orientationCalUpsideDownSideDone = true;
390
        } else if (side == "left") {
Don Gagne's avatar
Don Gagne committed
391 392
            _orientationCalLeftSideInProgress = false;
            _orientationCalLeftSideDone = true;
Don Gagne's avatar
Don Gagne committed
393
            _orientationCalLeftSideRotate = false;
394
        } else if (side == "right") {
Don Gagne's avatar
Don Gagne committed
395 396
            _orientationCalRightSideInProgress = false;
            _orientationCalRightSideDone = true;
397
        } else if (side == "front") {
Don Gagne's avatar
Don Gagne committed
398 399
            _orientationCalNoseDownSideInProgress = false;
            _orientationCalNoseDownSideDone = true;
Don Gagne's avatar
Don Gagne committed
400
            _orientationCalNoseDownSideRotate = false;
401
        } else if (side == "back") {
Don Gagne's avatar
Don Gagne committed
402 403
            _orientationCalTailDownSideInProgress = false;
            _orientationCalTailDownSideDone = true;
404
        }
Don Gagne's avatar
Don Gagne committed
405 406 407
        
        _orientationCalAreaHelpText->setProperty("text", "Place you vehicle into one of the orientations shown below and hold it still");

Don Gagne's avatar
Don Gagne committed
408 409
        emit orientationCalSidesInProgressChanged();
        emit orientationCalSidesDoneChanged();
Don Gagne's avatar
Don Gagne committed
410 411
        emit orientationCalSidesRotateChanged();
        return;
412
    }
Don Gagne's avatar
Don Gagne committed
413
    
Don Gagne's avatar
Don Gagne committed
414 415 416 417
    QString calCompletePrefix("calibration done:");
    if (text.startsWith(calCompletePrefix)) {
        _stopCalibration(StopCalibrationSuccess);
        return;
418
    }
Don Gagne's avatar
Don Gagne committed
419 420 421 422
    
    if (text.startsWith("calibration cancelled")) {
        _stopCalibration(_waitingForCancel ? StopCalibrationCancelled : StopCalibrationFailed);
        return;
423 424
    }
    
Don Gagne's avatar
Don Gagne committed
425 426 427 428 429 430 431 432
    if (text.startsWith("calibration failed")) {
        _stopCalibration(StopCalibrationFailed);
        return;
    }
}

void SensorsComponentController::_refreshParams(void)
{
Don Gagne's avatar
Don Gagne committed
433 434 435 436 437 438 439 440 441
    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";
    foreach (QString paramName, fastRefreshList) {
        _autopilot->refreshParameter(FactSystem::defaultComponentId, paramName);
    }
    
    // Now ask for all to refresh
442 443
    _autopilot->refreshParametersPrefix(FactSystem::defaultComponentId, "CAL_");
    _autopilot->refreshParametersPrefix(FactSystem::defaultComponentId, "SENS_");
Don Gagne's avatar
Don Gagne committed
444 445 446 447
}

bool SensorsComponentController::fixedWing(void)
{
448 449 450 451 452
    switch (_vehicle->vehicleType()) {
        case MAV_TYPE_FIXED_WING:
        case MAV_TYPE_VTOL_DUOROTOR:
        case MAV_TYPE_VTOL_QUADROTOR:
        case MAV_TYPE_VTOL_TILTROTOR:
453 454 455 456
        case MAV_TYPE_VTOL_RESERVED2:
        case MAV_TYPE_VTOL_RESERVED3:
        case MAV_TYPE_VTOL_RESERVED4:
        case MAV_TYPE_VTOL_RESERVED5:
457 458 459 460
            return true;
        default:
            return false;
    }
Don Gagne's avatar
Don Gagne committed
461
}
462

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

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

Don Gagne's avatar
Don Gagne committed
474
void SensorsComponentController::cancelCalibration(void)
Don Gagne's avatar
Don Gagne committed
475
{
Don Gagne's avatar
Don Gagne committed
476 477 478 479 480 481
    // 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();
482
}