From a7ae20ed9ab720b18b8dfe7978bfe127d3281fce Mon Sep 17 00:00:00 2001 From: lm Date: Fri, 11 Feb 2011 18:07:47 +0100 Subject: [PATCH] Fixed battery voltage setting, fixed mean in linechart --- src/comm/MAVLinkSimulationMAV.cc | 2 +- src/uas/UAS.cc | 84 +++++++++++++++++++---------- src/uas/UAS.h | 5 +- src/ui/linechart/LinechartPlot.cc | 32 +++++------ src/ui/linechart/LinechartWidget.cc | 2 +- src/ui/uas/UASView.cc | 2 +- 6 files changed, 79 insertions(+), 48 deletions(-) diff --git a/src/comm/MAVLinkSimulationMAV.cc b/src/comm/MAVLinkSimulationMAV.cc index 5be693b6d..f86a50e30 100644 --- a/src/comm/MAVLinkSimulationMAV.cc +++ b/src/comm/MAVLinkSimulationMAV.cc @@ -163,7 +163,7 @@ void MAVLinkSimulationMAV::mainloop() status.packet_drop = 0; status.vbat = 10500; status.status = sys_state; - status.battery_remaining = 900; + status.battery_remaining = 912; mavlink_msg_sys_status_encode(systemid, MAV_COMP_ID_IMU, &msg, &status); link->sendMAVLinkMessage(&msg); timer10Hz = 5; diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index d4567ce97..c61bc9d0e 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -41,8 +41,10 @@ thrustSum(0), thrustMax(10), startVoltage(0), warnVoltage(9.5f), +warnLevelPercent(20.0f), currentVoltage(12.0f), lpVoltage(12.0f), +batteryRemainingEstimateEnabled(true), mode(MAV_MODE_UNINIT), status(MAV_STATE_UNINIT), onboardTimeOffset(0), @@ -305,6 +307,10 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) lpVoltage = filterVoltage(currentVoltage); if (startVoltage == 0) startVoltage = currentVoltage; timeRemaining = calculateTimeRemaining(); + if (!batteryRemainingEstimateEnabled) + { + chargeLevel = state.battery_remaining/10.0f; + } //qDebug() << "Voltage: " << currentVoltage << " Chargelevel: " << getChargeLevel() << " Time remaining " << timeRemaining; emit batteryChanged(this, lpVoltage, getChargeLevel(), timeRemaining); emit voltageChanged(message.sysid, state.vbat/1000.0f); @@ -1906,29 +1912,49 @@ void UAS::setBattery(BatteryType type, int cells) void UAS::setBatterySpecs(const QString& specs) { - QString stringList = specs; - stringList = stringList.remove("V"); - stringList = stringList.remove("v"); - QStringList parts = stringList.split(","); - if (parts.length() == 3) + if (specs.length() == 0 || specs.contains("%")) { - float temp; + batteryRemainingEstimateEnabled = false; bool ok; - // Get the empty voltage - temp = parts.at(0).toFloat(&ok); - if (ok) emptyVoltage = temp; - // Get the warning voltage - temp = parts.at(1).toFloat(&ok); - if (ok) warnVoltage = temp; - // Get the full voltage - temp = parts.at(2).toFloat(&ok); - if (ok) fullVoltage = temp; + QString percent = specs; + percent = percent.remove("%"); + float temp = percent.toFloat(&ok); + if (ok) warnLevelPercent = temp; + } + else + { + batteryRemainingEstimateEnabled = true; + QString stringList = specs; + stringList = stringList.remove("V"); + stringList = stringList.remove("v"); + QStringList parts = stringList.split(","); + if (parts.length() == 3) + { + float temp; + bool ok; + // Get the empty voltage + temp = parts.at(0).toFloat(&ok); + if (ok) emptyVoltage = temp; + // Get the warning voltage + temp = parts.at(1).toFloat(&ok); + if (ok) warnVoltage = temp; + // Get the full voltage + temp = parts.at(2).toFloat(&ok); + if (ok) fullVoltage = temp; + } } } QString UAS::getBatterySpecs() { - return QString("%1V,%2V,%3V").arg(emptyVoltage).arg(warnVoltage).arg(fullVoltage); + if (batteryRemainingEstimateEnabled) + { + return QString("%1V,%2V,%3V").arg(emptyVoltage).arg(warnVoltage).arg(fullVoltage); + } + else + { + return QString("%1%").arg(warnLevelPercent); + } } int UAS::calculateTimeRemaining() @@ -1947,20 +1973,22 @@ int UAS::calculateTimeRemaining() /** * @return charge level in percent - 0 - 100 */ -double UAS::getChargeLevel() +float UAS::getChargeLevel() { - float chargeLevel; - if (lpVoltage < emptyVoltage) + if (batteryRemainingEstimateEnabled) { - chargeLevel = 0.0f; - } - else if (lpVoltage > fullVoltage) - { - chargeLevel = 100.0f; - } - else - { - chargeLevel = 100.0f * ((lpVoltage - emptyVoltage)/(fullVoltage - emptyVoltage)); + if (lpVoltage < emptyVoltage) + { + chargeLevel = 0.0f; + } + else if (lpVoltage > fullVoltage) + { + chargeLevel = 100.0f; + } + else + { + chargeLevel = 100.0f * ((lpVoltage - emptyVoltage)/(fullVoltage - emptyVoltage)); + } } return chargeLevel; } diff --git a/src/uas/UAS.h b/src/uas/UAS.h index 274cff386..7261fe2a6 100644 --- a/src/uas/UAS.h +++ b/src/uas/UAS.h @@ -124,8 +124,11 @@ protected: //COMMENTS FOR TEST UNIT float emptyVoltage; ///< Voltage of the empty battery (0%) float startVoltage; ///< Voltage at system start float warnVoltage; ///< Voltage where QGC will start to warn about low battery + float warnLevelPercent; ///< Warning level, in percent double currentVoltage; ///< Voltage currently measured float lpVoltage; ///< Low-pass filtered voltage + bool batteryRemainingEstimateEnabled; ///< If the estimate is enabled, QGC will try to estimate the remaining battery life + float chargeLevel; ///< Charge level of battery, in percent int timeRemaining; ///< Remaining time calculated based on previous and current unsigned int mode; ///< The current mode of the MAV int status; ///< The current status of the MAV @@ -167,7 +170,7 @@ public: /** @brief Estimate how much flight time is remaining */ int calculateTimeRemaining(); /** @brief Get the current charge level */ - double getChargeLevel(); + float getChargeLevel(); /** @brief Get the human-readable status message for this code */ void getStatusForCode(int statusCode, QString& uasState, QString& stateDescription); /** @brief Check if vehicle is in autonomous mode */ diff --git a/src/ui/linechart/LinechartPlot.cc b/src/ui/linechart/LinechartPlot.cc index 85af32ffe..cbce1e45b 100644 --- a/src/ui/linechart/LinechartPlot.cc +++ b/src/ui/linechart/LinechartPlot.cc @@ -789,13 +789,13 @@ void TimeSeriesData::append(quint64 ms, double value) this->value[count] = value; this->lastValue = value; this->mean = 0; - QList medianList = QList(); + //QList medianList = QList(); for (unsigned int i = 0; (i < averageWindow) && (((int)count - (int)i) >= 0); ++i) { this->mean += this->value[count-i]; - medianList.append(this->value[count-i]); + //medianList.append(this->value[count-i]); } - mean = mean / static_cast(qMin(averageWindow,static_cast(count))); + this->mean = mean / static_cast(qMin(averageWindow,static_cast(count))); this->variance = 0; for (unsigned int i = 0; (i < averageWindow) && (((int)count - (int)i) >= 0); ++i) @@ -804,19 +804,19 @@ void TimeSeriesData::append(quint64 ms, double value) } this->variance = this->variance / static_cast(qMin(averageWindow,static_cast(count))); - qSort(medianList); - - if (medianList.count() > 2) - { - if (medianList.count() % 2 == 0) - { - median = (medianList.at(medianList.count()/2) + medianList.at(medianList.count()/2+1)) / 2.0; - } - else - { - median = medianList.at(medianList.count()/2+1); - } - } +// qSort(medianList); + +// if (medianList.count() > 2) +// { +// if (medianList.count() % 2 == 0) +// { +// median = (medianList.at(medianList.count()/2) + medianList.at(medianList.count()/2+1)) / 2.0; +// } +// else +// { +// median = medianList.at(medianList.count()/2+1); +// } +// } // Update statistical values if(ms < startTime) startTime = ms; diff --git a/src/ui/linechart/LinechartWidget.cc b/src/ui/linechart/LinechartWidget.cc index 21ac666aa..90f2ea04c 100644 --- a/src/ui/linechart/LinechartWidget.cc +++ b/src/ui/linechart/LinechartWidget.cc @@ -416,7 +416,7 @@ void LinechartWidget::refresh() QMap::iterator j; for (j = curveMeans->begin(); j != curveMeans->end(); ++j) { - double val = activePlot->getCurrentValue(j.key()); + double val = activePlot->getMean(j.key()); int intval = static_cast(val); if (intval >= 100000 || intval <= -100000) { diff --git a/src/ui/uas/UASView.cc b/src/ui/uas/UASView.cc index fb922f469..e1db9567e 100644 --- a/src/ui/uas/UASView.cc +++ b/src/ui/uas/UASView.cc @@ -435,7 +435,7 @@ void UASView::setBatterySpecs() { bool ok; QString newName = QInputDialog::getText(this, tr("Set Battery Specifications for %1").arg(uas->getUASName()), - tr("Specs: (empty,warn,full), e.g. (9V,9.5V,12.6V)"), QLineEdit::Normal, + tr("Specs: (empty,warn,full), e.g. (9V,9.5V,12.6V) or just warn level in percent (e.g. 15%) to use estimate from MAV"), QLineEdit::Normal, uas->getBatterySpecs(), &ok); if (ok && !newName.isEmpty()) uas->setBatterySpecs(newName); -- 2.22.0