diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index 4d5c1c23234a4a26f1f1b121a75dd5fef5f5a533..3b5529f4b237651af927f2149d169640b1abbbc7 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -41,8 +41,6 @@ QGC_LOGGING_CATEGORY(UASLog, "UASLog") -#define UAS_DEFAULT_BATTERY_WARNLEVEL 20 - /** * Gets the settings from the previous UAS (name, airframe, autopilot, battery specs) * by calling readSettings. This means the new UAS will have the same settings @@ -63,17 +61,6 @@ UAS::UAS(MAVLinkProtocol* protocol, Vehicle* vehicle, FirmwarePluginManager * fi custom_mode(0), status(-1), - startVoltage(-1.0f), - tickVoltage(10.5f), - lastTickVoltageValue(13.0f), - tickLowpassVoltage(12.0f), - warnLevelPercent(UAS_DEFAULT_BATTERY_WARNLEVEL), - currentVoltage(12.6f), - lpVoltage(-1.0f), - currentCurrent(0.4f), - chargeLevel(-1), - lowBattAlarm(false), - startTime(QGC::groundTimeMilliseconds()), onboardTimeOffset(0), @@ -294,12 +281,8 @@ void UAS::receiveMessage(mavlink_message_t message) QString audiostring = QString("System %1").arg(uasId); QString stateAudio = ""; - QString modeAudio = ""; QString navModeAudio = ""; bool statechanged = false; - bool modechanged = false; - - QString audiomodeText = _firmwarePluginManager->firmwarePluginForAutopilot((MAV_AUTOPILOT)state.autopilot, (MAV_TYPE)state.type)->flightMode(state.base_mode, state.custom_mode); if ((state.system_status != this->status) && state.system_status != MAV_STATE_UNINIT) { @@ -318,27 +301,14 @@ void UAS::receiveMessage(mavlink_message_t message) stateAudio = uasState; } - if (this->base_mode != state.base_mode || this->custom_mode != state.custom_mode) - { - modechanged = true; - this->base_mode = state.base_mode; - this->custom_mode = state.custom_mode; - modeAudio = " is now in " + audiomodeText + " flight mode"; - } - // We got the mode receivedMode = true; // AUDIO - if (modechanged && statechanged) - { - // Output both messages - audiostring += modeAudio + " and " + stateAudio; - } - else if (modechanged || statechanged) + if (statechanged) { // Output the one message - audiostring += modeAudio + stateAudio; + audiostring += stateAudio; } if (statechanged && ((int)state.system_status == (int)MAV_STATE_CRITICAL || state.system_status == (int)MAV_STATE_EMERGENCY)) @@ -346,7 +316,7 @@ void UAS::receiveMessage(mavlink_message_t message) _say(QString("Emergency for system %1").arg(this->getUASID()), GAudioOutput::AUDIO_SEVERITY_EMERGENCY); QTimer::singleShot(3000, qgcApp()->toolbox()->audioOutput(), SLOT(startEmergency())); } - else if (modechanged || statechanged) + else if (statechanged) { _say(audiostring.toLower()); } @@ -378,62 +348,6 @@ void UAS::receiveMessage(mavlink_message_t message) emit loadChanged(this,state.load/10.0f); emit valueChanged(uasId, name.arg("load"), "%", state.load/10.0f, time); - currentVoltage = state.voltage_battery/1000.0f; - - if (state.voltage_battery > 0.0f && state.voltage_battery != UINT16_MAX) { - // Battery charge/time remaining/voltage calculations - currentVoltage = state.voltage_battery/1000.0f; - filterVoltage(currentVoltage); - tickLowpassVoltage = tickLowpassVoltage * 0.8f + 0.2f * currentVoltage; - - // We don't want to tick above the threshold - if (tickLowpassVoltage > tickVoltage) - { - lastTickVoltageValue = tickLowpassVoltage; - } - - if ((startVoltage > 0.0f) && (tickLowpassVoltage < tickVoltage) && (fabs(lastTickVoltageValue - tickLowpassVoltage) > 0.1f) - /* warn if lower than treshold */ - && (lpVoltage < tickVoltage) - /* warn only if we have at least the voltage of an empty LiPo cell, else we're sampling something wrong */ - && (currentVoltage > 3.3f) - /* warn only if current voltage is really still lower by a reasonable amount */ - && ((currentVoltage - 0.2f) < tickVoltage) - /* warn only every 20 seconds */ - && (QGC::groundTimeUsecs() - lastVoltageWarning) > 20000000) - { - _say(QString("Low battery system %1: %2 volts").arg(getUASID()).arg(lpVoltage, 0, 'f', 1, QChar(' '))); - lastVoltageWarning = QGC::groundTimeUsecs(); - lastTickVoltageValue = tickLowpassVoltage; - } - - if (startVoltage == -1.0f && currentVoltage > 0.1f) startVoltage = currentVoltage; - chargeLevel = state.battery_remaining; - - emit batteryChanged(this, lpVoltage, currentCurrent, getChargeLevel(), 0); - } - - emit valueChanged(uasId, name.arg("battery_remaining"), "%", getChargeLevel(), time); - emit valueChanged(uasId, name.arg("battery_voltage"), "V", currentVoltage, time); - - // And if the battery current draw is measured, log that also. - if (state.current_battery != -1) - { - currentCurrent = ((double)state.current_battery)/100.0f; - emit valueChanged(uasId, name.arg("battery_current"), "A", currentCurrent, time); - } - - // LOW BATTERY ALARM - if (chargeLevel >= 0 && (getChargeLevel() < warnLevelPercent)) - { - // An audio alarm. Does not generate any signals. - startLowBattAlarm(); - } - else - { - stopLowBattAlarm(); - } - // control_sensors_enabled: // relevant bits: 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control emit attitudeControlEnabled(state.onboard_control_sensors_enabled & (1 << 11)); @@ -1178,19 +1092,6 @@ quint64 UAS::getUnixTime(quint64 time) return ret; } -/** - * @param value battery voltage - */ -float UAS::filterVoltage(float value) -{ - if (lpVoltage < 0.0f) { - lpVoltage = value; - } - - lpVoltage = lpVoltage * 0.6f + value * 0.4f; - return lpVoltage; -} - /** * Get the status of the code and a description of the status. * Status can be unitialized, booting up, calibrating sensors, active @@ -2077,31 +1978,6 @@ QMap UAS::getComponents() return components; } -/** - * @return charge level in percent - 0 - 100 - */ -float UAS::getChargeLevel() -{ - return chargeLevel; -} - -void UAS::startLowBattAlarm() -{ - if (!lowBattAlarm) - { - _say(tr("System %1 has low battery").arg(getUASID())); - lowBattAlarm = true; - } -} - -void UAS::stopLowBattAlarm() -{ - if (lowBattAlarm) - { - lowBattAlarm = false; - } -} - void UAS::sendMapRCToParam(QString param_id, float scale, float value0, quint8 param_rc_channel_index, float valueMin, float valueMax) { if (!_vehicle) { @@ -2164,8 +2040,8 @@ void UAS::unsetRCToParameterMap() void UAS::_say(const QString& text, int severity) { - if (!qgcApp()->runningUnitTests()) - qgcApp()->toolbox()->audioOutput()->say(text, severity); + Q_UNUSED(severity); + qgcApp()->toolbox()->audioOutput()->say(text); } void UAS::shutdownVehicle(void) diff --git a/src/uas/UAS.h b/src/uas/UAS.h index 4917bcbfa9811a224b6703fc5fd9e00cb863eb76..1a859f6ffdc7afcb04ce3c34ffbf29c9a555cd5d 100644 --- a/src/uas/UAS.h +++ b/src/uas/UAS.h @@ -77,8 +77,6 @@ public: /** @brief The time interval the robot is switched on */ quint64 getUptime() const; - /** @brief Add one measurement and get low-passed voltage */ - float filterVoltage(float value); Q_PROPERTY(double latitude READ getLatitude WRITE setLatitude NOTIFY latitudeChanged) Q_PROPERTY(double longitude READ getLongitude WRITE setLongitude NOTIFY longitudeChanged) @@ -376,21 +374,6 @@ protected: //COMMENTS FOR TEST UNIT uint32_t custom_mode; ///< The current mode of the MAV int status; ///< The current status of the MAV - // dongfang: This looks like a candidate for being moved off to a separate class. - /// BATTERY / ENERGY - float startVoltage; ///< Voltage at system start - float tickVoltage; ///< Voltage where 0.1 V ticks are told - float lastTickVoltageValue; ///< The last voltage where a tick was announced - float tickLowpassVoltage; ///< Lowpass-filtered voltage for the tick announcement - float warnLevelPercent; ///< Warning level, in percent - double currentVoltage; ///< Voltage currently measured - float lpVoltage; ///< Low-pass filtered voltage - double currentCurrent; ///< Battery current currently measured - bool batteryRemainingEstimateEnabled; ///< If the estimate is enabled, QGC will try to estimate the remaining battery life - float chargeLevel; ///< Charge level of battery, in percent - bool lowBattAlarm; ///< Switch if battery is low - - /// TIMEKEEPING quint64 startTime; ///< The time the UAS was switched on quint64 onboardTimeOffset; @@ -486,8 +469,6 @@ protected: //COMMENTS FOR TEST UNIT #endif public: - /** @brief Get the current charge level */ - float getChargeLevel(); /** @brief Get the human-readable status message for this code */ void getStatusForCode(int statusCode, QString& uasState, QString& stateDescription); @@ -559,9 +540,6 @@ public slots: void stopHil(); #endif - void startLowBattAlarm(); - void stopLowBattAlarm(); - /** @brief Set the values for the manual control of the vehicle */ void setExternalControlSetpoint(float roll, float pitch, float yaw, float thrust, quint16 buttons, int joystickMode);