UASInfoWidget.cc 6.39 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 29 30 31 32 33 34
/*=====================================================================

PIXHAWK Micro Air Vehicle Flying Robotics Toolkit

(c) 2009 PIXHAWK PROJECT  <http://pixhawk.ethz.ch>

This file is part of the PIXHAWK project

    PIXHAWK 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.

    PIXHAWK 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 PIXHAWK. If not, see <http://www.gnu.org/licenses/>.

======================================================================*/

/**
 * @file
 *   @brief Implementation of class UASInfoWidget
 *
 *   @author Lorenz Meier <mavteam@student.ethz.ch>
 *
 */

#include <QtGlobal>
#include <QTimer>
#include <QDir>
35 36 37
#include <QDebug>

#include <float.h>
38 39 40
#include <cstdlib>
#include <cmath>

41 42 43 44
#include "UASInfoWidget.h"
#include "MultiVehicleManager.h"
#include "QGC.h"
#include "UAS.h"
45
#include "QGCApplication.h"
46

47 48
UASInfoWidget::UASInfoWidget(const QString& title, QAction* action, QWidget *parent, QString name)
    : QGCDockWidget(title, action, parent)
49 50 51 52 53
{
    ui.setupUi(this);
    this->name = name;
    activeUAS = NULL;

54 55
    connect(qgcApp()->toolbox()->multiVehicleManager(), &MultiVehicleManager::activeVehicleChanged, this, &UASInfoWidget::_activeVehicleChanged);
    _activeVehicleChanged(qgcApp()->toolbox()->multiVehicleManager()->activeVehicle());
56

57
    startTime = QGC::groundTimeMilliseconds();
58 59 60 61 62 63 64 65 66 67 68 69 70 71 72

    // Set default values
    /** Set two voltage decimals and zero charge level decimals **/
    this->voltageDecimals = 2;
    this->loadDecimals = 2;

    this->voltage = 0;
    this->chargeLevel = 0;
    this->load = 0;
    receiveLoss = 0;
    sendLoss = 0;
    changed = true;
    errors = QMap<QString, int>();

    updateTimer = new QTimer(this);
73
    connect(updateTimer, &QTimer::timeout, this, &UASInfoWidget::refresh);
74 75 76
    updateTimer->start(updateInterval);

    this->setVisible(false);
77 78
    
    loadSettings();
79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101
}

UASInfoWidget::~UASInfoWidget()
{

}

void UASInfoWidget::showEvent(QShowEvent* event)
{
    // React only to internal (pre-display)
    // events
    Q_UNUSED(event);
    updateTimer->start(updateInterval);
}

void UASInfoWidget::hideEvent(QHideEvent* event)
{
    // React only to internal (pre-display)
    // events
    Q_UNUSED(event);
    updateTimer->stop();
}

102
void UASInfoWidget::_activeVehicleChanged(Vehicle* vehicle)
103
{
104
    if (activeUAS) {
105 106 107 108
        disconnect(activeUAS, &UASInterface::batteryChanged, this, &UASInfoWidget::updateBattery);
        disconnect(activeUAS, &UASInterface::dropRateChanged, this, &UASInfoWidget::updateReceiveLoss);
        disconnect(static_cast<UAS*>(activeUAS), &UAS::loadChanged, this, &UASInfoWidget::updateCPULoad);
        disconnect(activeUAS, &UASInterface::errCountChanged, this, &UASInfoWidget::updateErrorCount);
109 110 111 112 113
        activeUAS = NULL;
    }
    
    if (vehicle) {
        activeUAS = vehicle->uas();
114 115 116 117
        connect(activeUAS, &UASInterface::batteryChanged, this, &UASInfoWidget::updateBattery);
        connect(activeUAS, &UASInterface::dropRateChanged, this, &UASInfoWidget::updateReceiveLoss);
        connect(static_cast<UAS*>(activeUAS), &UAS::loadChanged, this, &UASInfoWidget::updateCPULoad);
        connect(activeUAS, &UASInterface::errCountChanged, this, &UASInfoWidget::updateErrorCount);
118 119 120
    }
}

dongfang's avatar
dongfang committed
121
void UASInfoWidget::updateBattery(UASInterface* uas, double voltage, double current, double percent, int seconds)
122
{
dongfang's avatar
dongfang committed
123
    Q_UNUSED(current)
124 125 126 127 128 129 130 131
    setVoltage(uas, voltage);
    setChargeLevel(uas, percent);
    setTimeRemaining(uas, seconds);
}

void UASInfoWidget::updateErrorCount(int uasid, QString component, QString device, int count)
{
    //qDebug() << __FILE__ << __LINE__ << activeUAS->getUASID() << "=" << uasid;
132
    if (activeUAS->getUASID() == uasid) {
133 134 135 136 137 138 139 140 141 142
        errors.remove(component + ":" + device);
        errors.insert(component + ":" + device, count);
    }
}

/**
 *
 */
void UASInfoWidget::updateCPULoad(UASInterface* uas, double load)
{
143
    if (activeUAS == uas) {
144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171
        this->load = load;
    }
}

void UASInfoWidget::updateReceiveLoss(int uasId, float receiveLoss)
{
    Q_UNUSED(uasId);
    this->receiveLoss = this->receiveLoss * 0.8f + receiveLoss * 0.2f;
}

/**
  The send loss is typically calculated on the GCS based on packets
  that were received scrambled from the MAV
 */
void UASInfoWidget::updateSendLoss(int uasId, float sendLoss)
{
    Q_UNUSED(uasId);
    this->sendLoss = this->sendLoss * 0.8f + sendLoss * 0.2f;
}

void UASInfoWidget::setVoltage(UASInterface* uas, double voltage)
{
    Q_UNUSED(uas);
    this->voltage = voltage;
}

void UASInfoWidget::setChargeLevel(UASInterface* uas, double chargeLevel)
{
172
    if (activeUAS == uas) {
173 174 175 176 177 178
        this->chargeLevel = chargeLevel;
    }
}

void UASInfoWidget::setTimeRemaining(UASInterface* uas, double seconds)
{
179
    if (activeUAS == uas) {
180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202
        this->timeRemaining = seconds;
    }
}

void UASInfoWidget::refresh()
{
    ui.voltageLabel->setText(QString::number(this->voltage, 'f', voltageDecimals));
    ui.batteryBar->setValue(qMax(0,qMin(static_cast<int>(this->chargeLevel), 100)));

    ui.loadLabel->setText(QString::number(this->load, 'f', loadDecimals));
    ui.loadBar->setValue(qMax(0, qMin(static_cast<int>(this->load), 100)));

    ui.receiveLossBar->setValue(qMax(0, qMin(static_cast<int>(receiveLoss), 100)));
    ui.receiveLossLabel->setText(QString::number(receiveLoss, 'f', 2));

    ui.sendLossBar->setValue(sendLoss);
    ui.sendLossLabel->setText(QString::number(sendLoss, 'f', 2));

    ui.label_5->setText(QString::number(this->load, 'f', loadDecimals));
    ui.progressBar->setValue(qMax(0, qMin(static_cast<int>(this->load), 100)));

    QString errorString;
    QMapIterator<QString, int> i(errors);
203
    while (i.hasNext()) {
204 205 206 207 208 209 210 211 212 213
        i.next();
        errorString += QString(i.key() + ": %1 ").arg(i.value());

        // FIXME
        errorString.replace("IMU:", "");


    }
    ui.errorLabel->setText(errorString);
}