SensorsComponentController.cc 15.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 29
/*=====================================================================
 
 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"
#include "UASManager.h"
30
#include "QGCMessageBox.h"
Don Gagne's avatar
Don Gagne committed
31 32 33 34 35 36

#include <QVariant>
#include <QQmlProperty>

SensorsComponentController::SensorsComponentController(AutoPilotPlugin* autopilot, QObject* parent) :
    QObject(parent),
37 38
    _statusLog(NULL),
    _progressBar(NULL),
Don Gagne's avatar
Don Gagne committed
39 40 41 42 43
    _compassButton(NULL),
    _gyroButton(NULL),
    _accelButton(NULL),
    _airspeedButton(NULL),
    _cancelButton(NULL),
Don Gagne's avatar
Don Gagne committed
44
    _showOrientationCalArea(false),
45
    _gyroCalInProgress(false),
Don Gagne's avatar
Don Gagne committed
46 47 48 49 50 51 52 53
    _magCalInProgress(false),
    _accelCalInProgress(false),
    _orientationCalDownSideDone(false),
    _orientationCalUpsideDownSideDone(false),
    _orientationCalLeftSideDone(false),
    _orientationCalRightSideDone(false),
    _orientationCalNoseDownSideDone(false),
    _orientationCalTailDownSideDone(false),
Don Gagne's avatar
Don Gagne committed
54 55 56 57 58 59
    _orientationCalDownSideVisible(false),
    _orientationCalUpsideDownSideVisible(false),
    _orientationCalLeftSideVisible(false),
    _orientationCalRightSideVisible(false),
    _orientationCalNoseDownSideVisible(false),
    _orientationCalTailDownSideVisible(false),
Don Gagne's avatar
Don Gagne committed
60 61 62 63 64 65
    _orientationCalDownSideInProgress(false),
    _orientationCalUpsideDownSideInProgress(false),
    _orientationCalLeftSideInProgress(false),
    _orientationCalRightSideInProgress(false),
    _orientationCalNoseDownSideInProgress(false),
    _orientationCalTailDownSideInProgress(false),
Don Gagne's avatar
Don Gagne committed
66 67 68 69 70
    _orientationCalDownSideRotate(false),
    _orientationCalLeftSideRotate(false),
    _orientationCalNoseDownSideRotate(false),
    _unknownFirmwareVersion(false),
    _waitingForCancel(false),
Don Gagne's avatar
Don Gagne committed
71 72
    _autopilot(autopilot)
{
73 74 75
    Q_ASSERT(_autopilot);
    Q_ASSERT(_autopilot->pluginReady());
    
Don Gagne's avatar
Don Gagne committed
76 77 78
    _uas = _autopilot->uas();
    Q_ASSERT(_uas);
    
79 80 81 82 83 84 85
    // Mag rotation parameters are optional
    _showCompass0 = _autopilot->parameterExists("CAL_MAG0_ROT") &&
                        _autopilot->getParameterFact("CAL_MAG0_ROT")->value().toInt() >= 0;
    _showCompass1 = _autopilot->parameterExists("CAL_MAG1_ROT") &&
                        _autopilot->getParameterFact("CAL_MAG1_ROT")->value().toInt() >= 0;
    _showCompass2 = _autopilot->parameterExists("CAL_MAG2_ROT") &&
                        _autopilot->getParameterFact("CAL_MAG2_ROT")->value().toInt() >= 0;
Don Gagne's avatar
Don Gagne committed
86 87 88 89 90 91 92 93 94 95 96 97 98 99 100
}

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

void SensorsComponentController::_startVisualCalibration(void)
{
113 114 115 116
    _compassButton->setEnabled(false);
    _gyroButton->setEnabled(false);
    _accelButton->setEnabled(false);
    _airspeedButton->setEnabled(false);
Don Gagne's avatar
Don Gagne committed
117 118 119
    _cancelButton->setEnabled(true);
    
    _progressBar->setProperty("value", 0);
120 121
}

Don Gagne's avatar
Don Gagne committed
122
void SensorsComponentController::_stopCalibration(SensorsComponentController::StopCalibrationCode code)
123
{
Don Gagne's avatar
Don Gagne committed
124
    disconnect(_uas, &UASInterface::textMessageReceived, this, &SensorsComponentController::_handleUASTextMessage);
Don Gagne's avatar
Don Gagne committed
125
    
126 127 128 129
    _compassButton->setEnabled(true);
    _gyroButton->setEnabled(true);
    _accelButton->setEnabled(true);
    _airspeedButton->setEnabled(true);
Don Gagne's avatar
Don Gagne committed
130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151
    _cancelButton->setEnabled(false);
    
    if (code == StopCalibrationSuccess) {
        _orientationCalDownSideDone = true;
        _orientationCalUpsideDownSideDone = true;
        _orientationCalLeftSideDone = true;
        _orientationCalRightSideDone = true;
        _orientationCalTailDownSideDone = true;
        _orientationCalNoseDownSideDone = true;
        _orientationCalDownSideInProgress = false;
        _orientationCalUpsideDownSideInProgress = false;
        _orientationCalLeftSideInProgress = false;
        _orientationCalRightSideInProgress = false;
        _orientationCalNoseDownSideInProgress = false;
        _orientationCalTailDownSideInProgress = false;
        emit orientationCalSidesDoneChanged();
        emit orientationCalSidesInProgressChanged();
        
        _progressBar->setProperty("value", 1);
    } else {
        _progressBar->setProperty("value", 0);
    }
152
    
Don Gagne's avatar
Don Gagne committed
153 154 155
    _waitingForCancel = false;
    emit waitingForCancelChanged();

156 157
    _refreshParams();
    
Don Gagne's avatar
Don Gagne committed
158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176
    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();
            QGCMessageBox::warning("Calibration", "Calibration failed. Calibration log will be displayed.");
            break;
177
    }
Don Gagne's avatar
Don Gagne committed
178 179 180 181
    
    _magCalInProgress = false;
    _accelCalInProgress = false;
    _gyroCalInProgress = false;
182 183 184 185
}

void SensorsComponentController::calibrateGyro(void)
{
Don Gagne's avatar
Don Gagne committed
186 187
    _startLogCalibration();
    _uas->startCalibration(UASInterface::StartCalibrationGyro);
Don Gagne's avatar
Don Gagne committed
188 189 190 191
}

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

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

void SensorsComponentController::calibrateAirspeed(void)
{
Don Gagne's avatar
Don Gagne committed
204 205
    _startLogCalibration();
    _uas->startCalibration(UASInterface::StartCalibrationAirspeed);
Don Gagne's avatar
Don Gagne committed
206 207 208 209 210 211 212 213 214 215 216 217 218
}

void SensorsComponentController::_handleUASTextMessage(int uasId, int compId, int severity, QString text)
{
    Q_UNUSED(compId);
    Q_UNUSED(severity);
    
    UASInterface* uas = _autopilot->uas();
    Q_ASSERT(uas);
    if (uasId != uas->getUASID()) {
        return;
    }
    
219 220 221 222 223 224 225 226 227 228 229
    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
230
    _appendStatusLog(text);
Don Gagne's avatar
Don Gagne committed
231 232 233 234 235 236 237 238 239 240 241 242
    
    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());
243

Don Gagne's avatar
Don Gagne committed
244 245 246 247 248 249 250 251 252 253
    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(" ");
        if (parts.count() != 2 && parts[0].toInt() != 1) {
            _unknownFirmwareVersion = true;
            qDebug() << "Unknown cal firmware version, using log";
            return;
Don Gagne's avatar
Don Gagne committed
254
        }
Don Gagne's avatar
Don Gagne committed
255 256 257 258 259 260 261 262 263 264 265 266 267 268 269 270 271 272 273 274 275 276 277 278 279 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
        
        _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;
                _orientationCalLeftSideVisible = true;
                _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
307
        }
Don Gagne's avatar
Don Gagne committed
308 309 310 311
        return;
    }
    
    if (text.endsWith("orientation detected")) {
Don Gagne's avatar
Don Gagne committed
312 313
        QString side = text.section(" ", 0, 0);
        qDebug() << "Side started" << side;
Don Gagne's avatar
Don Gagne committed
314
        
Don Gagne's avatar
Don Gagne committed
315 316
        if (side == "down") {
            _orientationCalDownSideInProgress = true;
Don Gagne's avatar
Don Gagne committed
317 318 319
            if (_magCalInProgress) {
                _orientationCalDownSideRotate = true;
            }
320
        } else if (side == "up") {
Don Gagne's avatar
Don Gagne committed
321
            _orientationCalUpsideDownSideInProgress = true;
322
        } else if (side == "left") {
Don Gagne's avatar
Don Gagne committed
323
            _orientationCalLeftSideInProgress = true;
Don Gagne's avatar
Don Gagne committed
324 325 326
            if (_magCalInProgress) {
                _orientationCalLeftSideRotate = true;
            }
327
        } else if (side == "right") {
Don Gagne's avatar
Don Gagne committed
328
            _orientationCalRightSideInProgress = true;
329
        } else if (side == "front") {
Don Gagne's avatar
Don Gagne committed
330
            _orientationCalNoseDownSideInProgress = true;
Don Gagne's avatar
Don Gagne committed
331 332 333
            if (_magCalInProgress) {
                _orientationCalNoseDownSideRotate = true;
            }
334
        } else if (side == "back") {
Don Gagne's avatar
Don Gagne committed
335
            _orientationCalTailDownSideInProgress = true;
336
        }
Don Gagne's avatar
Don Gagne committed
337 338 339 340 341 342 343
        
        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
344
        emit orientationCalSidesInProgressChanged();
Don Gagne's avatar
Don Gagne committed
345 346 347 348 349
        emit orientationCalSidesRotateChanged();
        return;
    }
    
    if (text.endsWith("side done, rotate to a different side")) {
350 351
        QString side = text.section(" ", 0, 0);
        qDebug() << "Side finished" << side;
Don Gagne's avatar
Don Gagne committed
352
        
353
        if (side == "down") {
Don Gagne's avatar
Don Gagne committed
354 355
            _orientationCalDownSideInProgress = false;
            _orientationCalDownSideDone = true;
Don Gagne's avatar
Don Gagne committed
356
            _orientationCalDownSideRotate = false;
357
        } else if (side == "up") {
Don Gagne's avatar
Don Gagne committed
358 359
            _orientationCalUpsideDownSideInProgress = false;
            _orientationCalUpsideDownSideDone = true;
360
        } else if (side == "left") {
Don Gagne's avatar
Don Gagne committed
361 362
            _orientationCalLeftSideInProgress = false;
            _orientationCalLeftSideDone = true;
Don Gagne's avatar
Don Gagne committed
363
            _orientationCalLeftSideRotate = false;
364
        } else if (side == "right") {
Don Gagne's avatar
Don Gagne committed
365 366
            _orientationCalRightSideInProgress = false;
            _orientationCalRightSideDone = true;
367
        } else if (side == "front") {
Don Gagne's avatar
Don Gagne committed
368 369
            _orientationCalNoseDownSideInProgress = false;
            _orientationCalNoseDownSideDone = true;
Don Gagne's avatar
Don Gagne committed
370
            _orientationCalNoseDownSideRotate = false;
371
        } else if (side == "back") {
Don Gagne's avatar
Don Gagne committed
372 373
            _orientationCalTailDownSideInProgress = false;
            _orientationCalTailDownSideDone = true;
374
        }
Don Gagne's avatar
Don Gagne committed
375 376 377
        
        _orientationCalAreaHelpText->setProperty("text", "Place you vehicle into one of the orientations shown below and hold it still");

Don Gagne's avatar
Don Gagne committed
378 379
        emit orientationCalSidesInProgressChanged();
        emit orientationCalSidesDoneChanged();
Don Gagne's avatar
Don Gagne committed
380 381
        emit orientationCalSidesRotateChanged();
        return;
382
    }
Don Gagne's avatar
Don Gagne committed
383
    
Don Gagne's avatar
Don Gagne committed
384 385 386 387
    QString calCompletePrefix("calibration done:");
    if (text.startsWith(calCompletePrefix)) {
        _stopCalibration(StopCalibrationSuccess);
        return;
388
    }
Don Gagne's avatar
Don Gagne committed
389 390 391 392
    
    if (text.startsWith("calibration cancelled")) {
        _stopCalibration(_waitingForCancel ? StopCalibrationCancelled : StopCalibrationFailed);
        return;
393 394
    }
    
Don Gagne's avatar
Don Gagne committed
395 396 397 398 399 400 401 402
    if (text.startsWith("calibration failed")) {
        _stopCalibration(StopCalibrationFailed);
        return;
    }
}

void SensorsComponentController::_refreshParams(void)
{
403 404
    _autopilot->refreshParametersPrefix(FactSystem::defaultComponentId, "CAL_");
    _autopilot->refreshParametersPrefix(FactSystem::defaultComponentId, "SENS_");
Don Gagne's avatar
Don Gagne committed
405 406 407 408 409 410 411 412
}

bool SensorsComponentController::fixedWing(void)
{
    UASInterface* uas = _autopilot->uas();
    Q_ASSERT(uas);
    return uas->getSystemType() == MAV_TYPE_FIXED_WING;
}
413

Don Gagne's avatar
Don Gagne committed
414
void SensorsComponentController::_updateAndEmitShowOrientationCalArea(bool show)
415
{
Don Gagne's avatar
Don Gagne committed
416 417
    _showOrientationCalArea = show;
    emit showOrientationCalAreaChanged();
418 419 420 421
}

void SensorsComponentController::_hideAllCalAreas(void)
{
Don Gagne's avatar
Don Gagne committed
422 423 424
    _updateAndEmitShowOrientationCalArea(false);
}

Don Gagne's avatar
Don Gagne committed
425
void SensorsComponentController::cancelCalibration(void)
Don Gagne's avatar
Don Gagne committed
426
{
Don Gagne's avatar
Don Gagne committed
427 428 429 430 431 432 433
    // 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();
}