PowerComponentController.cc 5.06 KB
Newer Older
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 "PowerComponentController.h"
#include "QGCMAVLink.h"
29
#include "UAS.h"
30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45

#include <QVariant>
#include <QQmlProperty>

PowerComponentController::PowerComponentController(void)
{

}

void PowerComponentController::calibrateEsc(void)
{
    _warningMessages.clear();
    connect(_uas, &UASInterface::textMessageReceived, this, &PowerComponentController::_handleUASTextMessage);
    _uas->startCalibration(UASInterface::StartCalibrationEsc);
}

46 47 48 49 50 51 52
void PowerComponentController::busConfigureActuators(void)
{
    _warningMessages.clear();
    connect(_uas, &UASInterface::textMessageReceived, this, &PowerComponentController::_handleUASTextMessage);
    _uas->startBusConfig(UASInterface::StartBusConfigActuators);
}

53 54 55 56 57 58
void PowerComponentController::stopBusConfigureActuators(void)
{
    disconnect(_uas, &UASInterface::textMessageReceived, this, &PowerComponentController::_handleUASTextMessage);
    _uas->startBusConfig(UASInterface::EndBusConfigActuators);
}

59 60 61 62 63
void PowerComponentController::_stopCalibration(void)
{
    disconnect(_uas, &UASInterface::textMessageReceived, this, &PowerComponentController::_handleUASTextMessage);
}

64 65 66 67 68
void PowerComponentController::_stopBusConfig(void)
{
    _stopCalibration();
}

69 70 71 72 73
void PowerComponentController::_handleUASTextMessage(int uasId, int compId, int severity, QString text)
{
    Q_UNUSED(compId);
    Q_UNUSED(severity);
    
74
    UASInterface* uas = _autopilot->vehicle()->uas();
75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98
    Q_ASSERT(uas);
    if (uasId != uas->getUASID()) {
        return;
    }
    
    // All calibration messages start with [cal]
    QString calPrefix("[cal] ");
    if (!text.startsWith(calPrefix)) {
        return;
    }
    text = text.right(text.length() - calPrefix.length());

    // Make sure we can understand this firmware rev
    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) {
            emit incorrectFirmwareRevReporting();
            return;
        }
        
Don Gagne's avatar
Don Gagne committed
99 100 101
#if 0
        // FIXME: Cal version check is not working. Needs to be able to cancel, calibration
        
102 103 104 105 106 107 108 109 110
        int firmwareRev = parts[0].toInt();
        if (firmwareRev < _neededFirmwareRev) {
            emit oldFirmware();
            return;
        }
        if (firmwareRev > _neededFirmwareRev) {
            emit newerFirmware();
            return;
        }
Don Gagne's avatar
Don Gagne committed
111
#endif
112 113 114 115 116 117 118 119 120 121 122 123
    }

    if (text == "Connect battery now") {
        emit connectBattery();
        return;
    }
    
    if (text == "Battery connected") {
        emit batteryConnected();
        return;
    }

124
    
125 126
    QString failedPrefix("calibration failed: ");
    if (text.startsWith(failedPrefix)) {
127 128 129 130 131 132
        QString failureText = text.right(text.length() - failedPrefix.length());
        if (failureText.startsWith("Disconnect battery")) {
            emit disconnectBattery();
            return;
        }
        
133 134 135 136 137 138 139 140 141 142 143 144
        _stopCalibration();
        emit calibrationFailed(text.right(text.length() - failedPrefix.length()));
        return;
    }
    
    QString calCompletePrefix("calibration done:");
    if (text.startsWith(calCompletePrefix)) {
        _stopCalibration();
        emit calibrationSuccess(_warningMessages);
        return;
    }
    
145
    QString warningPrefix("config warning: ");
146 147 148
    if (text.startsWith(warningPrefix)) {
        _warningMessages << text.right(text.length() - warningPrefix.length());
    }
149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168

    QString busFailedPrefix("bus conf fail:");
    if (text.startsWith(busFailedPrefix)) {

        _stopBusConfig();
        emit calibrationFailed(text.right(text.length() - failedPrefix.length()));
        return;
    }

    QString busCompletePrefix("bus conf done:");
    if (text.startsWith(calCompletePrefix)) {
        _stopBusConfig();
        emit calibrationSuccess(_warningMessages);
        return;
    }

    QString busWarningPrefix("bus conf warn: ");
    if (text.startsWith(busWarningPrefix)) {
        _warningMessages << text.right(text.length() - warningPrefix.length());
    }
169
}