SensorsComponentController.cc 17.2 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
{
77
}
Don Gagne's avatar
Don Gagne committed
78

79 80 81
bool SensorsComponentController::usingUDPLink(void)
{
    return _vehicle->priorityLink()->getLinkConfiguration()->type() == LinkConfiguration::TypeUdp;
Don Gagne's avatar
Don Gagne committed
82 83 84 85 86 87 88 89 90 91 92 93 94 95 96
}

/// 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
97
void SensorsComponentController::_startLogCalibration(void)
Don Gagne's avatar
Don Gagne committed
98
{
Don Gagne's avatar
Don Gagne committed
99
    _unknownFirmwareVersion = false;
100 101
    _hideAllCalAreas();
    
Don Gagne's avatar
Don Gagne committed
102 103 104 105 106 107 108
    connect(_uas, &UASInterface::textMessageReceived, this, &SensorsComponentController::_handleUASTextMessage);
    
    _cancelButton->setEnabled(false);
}

void SensorsComponentController::_startVisualCalibration(void)
{
109 110 111 112
    _compassButton->setEnabled(false);
    _gyroButton->setEnabled(false);
    _accelButton->setEnabled(false);
    _airspeedButton->setEnabled(false);
113
    _levelButton->setEnabled(false);
Don Gagne's avatar
Don Gagne committed
114
    _cancelButton->setEnabled(true);
115 116

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

169 170
    _refreshParams();
    
Don Gagne's avatar
Don Gagne committed
171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187
    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();
188
            qgcApp()->showMessage("Calibration failed. Calibration log will be displayed.");
Don Gagne's avatar
Don Gagne committed
189
            break;
190
    }
Don Gagne's avatar
Don Gagne committed
191 192 193 194
    
    _magCalInProgress = false;
    _accelCalInProgress = false;
    _gyroCalInProgress = false;
195 196 197 198
}

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

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

void SensorsComponentController::calibrateAccel(void)
{
Don Gagne's avatar
Don Gagne committed
211 212
    _startLogCalibration();
    _uas->startCalibration(UASInterface::StartCalibrationAccel);
Don Gagne's avatar
Don Gagne committed
213 214
}

215 216 217 218 219 220
void SensorsComponentController::calibrateLevel(void)
{
    _startLogCalibration();
    _uas->startCalibration(UASInterface::StartCalibrationLevel);
}

Don Gagne's avatar
Don Gagne committed
221 222
void SensorsComponentController::calibrateAirspeed(void)
{
Don Gagne's avatar
Don Gagne committed
223 224
    _startLogCalibration();
    _uas->startCalibration(UASInterface::StartCalibrationAirspeed);
Don Gagne's avatar
Don Gagne committed
225 226 227 228 229 230 231
}

void SensorsComponentController::_handleUASTextMessage(int uasId, int compId, int severity, QString text)
{
    Q_UNUSED(compId);
    Q_UNUSED(severity);
    
232
    UASInterface* uas = _autopilot->vehicle()->uas();
Don Gagne's avatar
Don Gagne committed
233 234 235 236 237
    Q_ASSERT(uas);
    if (uasId != uas->getUASID()) {
        return;
    }
    
238 239 240 241 242 243 244 245 246 247 248
    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
249
    _appendStatusLog(text);
250
    qCDebug(SensorsComponentControllerLog) << text;
Don Gagne's avatar
Don Gagne committed
251 252 253 254 255 256 257 258 259 260 261 262
    
    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());
263

Don Gagne's avatar
Don Gagne committed
264 265 266 267 268 269
    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(" ");
270
        if (parts.count() != 2 && parts[0].toInt() != _supportedFirmwareCalVersion) {
Don Gagne's avatar
Don Gagne committed
271
            _unknownFirmwareVersion = true;
272 273 274
            QString msg = "Unsupported calibration firmware version, using log";
            _appendStatusLog(msg);
            qDebug() << msg;
Don Gagne's avatar
Don Gagne committed
275
            return;
Don Gagne's avatar
Don Gagne committed
276
        }
Don Gagne's avatar
Don Gagne committed
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 313 314 315 316
        
        _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;
317
                _orientationCalUpsideDownSideVisible = true;
Don Gagne's avatar
Don Gagne committed
318
                _orientationCalLeftSideVisible = true;
319 320
                _orientationCalRightSideVisible = true;
                _orientationCalTailDownSideVisible = true;
Don Gagne's avatar
Don Gagne committed
321 322 323 324 325 326 327 328 329 330 331
                _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
332
        }
Don Gagne's avatar
Don Gagne committed
333 334 335 336
        return;
    }
    
    if (text.endsWith("orientation detected")) {
Don Gagne's avatar
Don Gagne committed
337 338
        QString side = text.section(" ", 0, 0);
        qDebug() << "Side started" << side;
Don Gagne's avatar
Don Gagne committed
339
        
Don Gagne's avatar
Don Gagne committed
340 341
        if (side == "down") {
            _orientationCalDownSideInProgress = true;
Don Gagne's avatar
Don Gagne committed
342 343 344
            if (_magCalInProgress) {
                _orientationCalDownSideRotate = true;
            }
345
        } else if (side == "up") {
Don Gagne's avatar
Don Gagne committed
346
            _orientationCalUpsideDownSideInProgress = true;
347 348 349
            if (_magCalInProgress) {
                _orientationCalUpsideDownSideRotate = true;
            }
350
        } else if (side == "left") {
Don Gagne's avatar
Don Gagne committed
351
            _orientationCalLeftSideInProgress = true;
Don Gagne's avatar
Don Gagne committed
352 353 354
            if (_magCalInProgress) {
                _orientationCalLeftSideRotate = true;
            }
355
        } else if (side == "right") {
Don Gagne's avatar
Don Gagne committed
356
            _orientationCalRightSideInProgress = true;
357 358 359
            if (_magCalInProgress) {
                _orientationCalRightSideRotate = true;
            }
360
        } else if (side == "front") {
Don Gagne's avatar
Don Gagne committed
361
            _orientationCalNoseDownSideInProgress = true;
Don Gagne's avatar
Don Gagne committed
362 363 364
            if (_magCalInProgress) {
                _orientationCalNoseDownSideRotate = true;
            }
365
        } else if (side == "back") {
Don Gagne's avatar
Don Gagne committed
366
            _orientationCalTailDownSideInProgress = true;
367 368 369
            if (_magCalInProgress) {
                _orientationCalTailDownSideRotate = true;
            }
370
        }
Don Gagne's avatar
Don Gagne committed
371 372 373 374 375 376 377
        
        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
378
        emit orientationCalSidesInProgressChanged();
Don Gagne's avatar
Don Gagne committed
379 380 381 382 383
        emit orientationCalSidesRotateChanged();
        return;
    }
    
    if (text.endsWith("side done, rotate to a different side")) {
384 385
        QString side = text.section(" ", 0, 0);
        qDebug() << "Side finished" << side;
Don Gagne's avatar
Don Gagne committed
386
        
387
        if (side == "down") {
Don Gagne's avatar
Don Gagne committed
388 389
            _orientationCalDownSideInProgress = false;
            _orientationCalDownSideDone = true;
Don Gagne's avatar
Don Gagne committed
390
            _orientationCalDownSideRotate = false;
391
        } else if (side == "up") {
Don Gagne's avatar
Don Gagne committed
392 393
            _orientationCalUpsideDownSideInProgress = false;
            _orientationCalUpsideDownSideDone = true;
394
            _orientationCalUpsideDownSideRotate = false;
395
        } else if (side == "left") {
Don Gagne's avatar
Don Gagne committed
396 397
            _orientationCalLeftSideInProgress = false;
            _orientationCalLeftSideDone = true;
Don Gagne's avatar
Don Gagne committed
398
            _orientationCalLeftSideRotate = false;
399
        } else if (side == "right") {
Don Gagne's avatar
Don Gagne committed
400 401
            _orientationCalRightSideInProgress = false;
            _orientationCalRightSideDone = true;
402
            _orientationCalRightSideRotate = false;
403
        } else if (side == "front") {
Don Gagne's avatar
Don Gagne committed
404 405
            _orientationCalNoseDownSideInProgress = false;
            _orientationCalNoseDownSideDone = true;
Don Gagne's avatar
Don Gagne committed
406
            _orientationCalNoseDownSideRotate = false;
407
        } else if (side == "back") {
Don Gagne's avatar
Don Gagne committed
408 409
            _orientationCalTailDownSideInProgress = false;
            _orientationCalTailDownSideDone = true;
410
            _orientationCalTailDownSideRotate = false;
411
        }
Don Gagne's avatar
Don Gagne committed
412 413 414
        
        _orientationCalAreaHelpText->setProperty("text", "Place you vehicle into one of the orientations shown below and hold it still");

Don Gagne's avatar
Don Gagne committed
415 416
        emit orientationCalSidesInProgressChanged();
        emit orientationCalSidesDoneChanged();
Don Gagne's avatar
Don Gagne committed
417 418
        emit orientationCalSidesRotateChanged();
        return;
419
    }
420 421 422 423 424

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

void SensorsComponentController::_refreshParams(void)
{
Don Gagne's avatar
Don Gagne committed
445 446 447 448
    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";
449
    foreach (const QString &paramName, fastRefreshList) {
Don Gagne's avatar
Don Gagne committed
450 451 452 453
        _autopilot->refreshParameter(FactSystem::defaultComponentId, paramName);
    }
    
    // Now ask for all to refresh
454 455
    _autopilot->refreshParametersPrefix(FactSystem::defaultComponentId, "CAL_");
    _autopilot->refreshParametersPrefix(FactSystem::defaultComponentId, "SENS_");
Don Gagne's avatar
Don Gagne committed
456 457 458 459
}

bool SensorsComponentController::fixedWing(void)
{
460 461 462 463 464
    switch (_vehicle->vehicleType()) {
        case MAV_TYPE_FIXED_WING:
        case MAV_TYPE_VTOL_DUOROTOR:
        case MAV_TYPE_VTOL_QUADROTOR:
        case MAV_TYPE_VTOL_TILTROTOR:
465 466 467 468
        case MAV_TYPE_VTOL_RESERVED2:
        case MAV_TYPE_VTOL_RESERVED3:
        case MAV_TYPE_VTOL_RESERVED4:
        case MAV_TYPE_VTOL_RESERVED5:
469 470 471 472
            return true;
        default:
            return false;
    }
Don Gagne's avatar
Don Gagne committed
473
}
474

Don Gagne's avatar
Don Gagne committed
475
void SensorsComponentController::_updateAndEmitShowOrientationCalArea(bool show)
476
{
Don Gagne's avatar
Don Gagne committed
477 478
    _showOrientationCalArea = show;
    emit showOrientationCalAreaChanged();
479 480 481 482
}

void SensorsComponentController::_hideAllCalAreas(void)
{
Don Gagne's avatar
Don Gagne committed
483 484 485
    _updateAndEmitShowOrientationCalArea(false);
}

Don Gagne's avatar
Don Gagne committed
486
void SensorsComponentController::cancelCalibration(void)
Don Gagne's avatar
Don Gagne committed
487
{
Don Gagne's avatar
Don Gagne committed
488 489 490 491 492 493
    // 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();
494
}