From d8c140be041f6bb03451cc943b3d0dd823bb11e2 Mon Sep 17 00:00:00 2001 From: AlexKlimaj Date: Mon, 6 Apr 2020 19:09:02 -0600 Subject: [PATCH] UAVCAN Smart Battery Improvements --- ROMFS/px4fmu_common/init.d/rcS | 6 +- .../px4_work_queue/WorkQueueManager.hpp | 2 +- src/drivers/uavcan/sensors/battery.cpp | 80 ++++++++++++++----- src/drivers/uavcan/sensors/battery.hpp | 20 ++++- 4 files changed, 83 insertions(+), 25 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index a570a55dcb..c09f55493c 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -353,7 +353,11 @@ else esc_battery start fi - battery_status start + if ! param compare BAT1_SOURCE 1 + then + battery_status start + fi + commander start fi diff --git a/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp b/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp index 1804bd6646..3b7c6cd36f 100644 --- a/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp +++ b/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp @@ -69,7 +69,7 @@ static constexpr wq_config_t att_pos_ctrl{"wq:att_pos_ctrl", 7200, -13}; static constexpr wq_config_t hp_default{"wq:hp_default", 1900, -14}; -static constexpr wq_config_t uavcan{"wq:uavcan", 2400, -15}; +static constexpr wq_config_t uavcan{"wq:uavcan", 2800, -15}; static constexpr wq_config_t UART0{"wq:UART0", 1400, -16}; static constexpr wq_config_t UART1{"wq:UART1", 1400, -17}; diff --git a/src/drivers/uavcan/sensors/battery.cpp b/src/drivers/uavcan/sensors/battery.cpp index 0377277b9e..27791d6b10 100644 --- a/src/drivers/uavcan/sensors/battery.cpp +++ b/src/drivers/uavcan/sensors/battery.cpp @@ -33,13 +33,17 @@ #include "battery.hpp" -#include +#include +#include const char *const UavcanBatteryBridge::NAME = "battery"; UavcanBatteryBridge::UavcanBatteryBridge(uavcan::INode &node) : UavcanCDevSensorBridgeBase("uavcan_battery", "/dev/uavcan/battery", "/dev/battery", ORB_ID(battery_status)), - _sub_battery(node) + ModuleParams(nullptr), + _sub_battery(node), + _warning(battery_status_s::BATTERY_WARNING_NONE), + _last_timestamp(0) { } @@ -69,36 +73,72 @@ UavcanBatteryBridge::battery_sub_cb(const uavcan::ReceivedDataStructure 0) { - battery.remaining = msg.remaining_capacity_wh / msg.full_charge_capacity_wh; - - } else { - battery.remaining = 0; - - } + sumDischarged(battery.timestamp, battery.current_a); + battery.discharged_mah = _discharged_mah; + battery.remaining = msg.state_of_charge_pct / 100.0f; // between 0 and 1 // battery.scale = msg.; // Power scaling factor, >= 1, or -1 if unknown - battery.temperature = msg.temperature; + battery.temperature = msg.temperature + CONSTANTS_ABSOLUTE_NULL_CELSIUS; // Kelvin to Celcius // battery.cell_count = msg.; - // battery.voltage_cell_v[4] = msg.; - // battery.max_cell_voltage_delta = msg.; + battery.connected = true; + battery.source = msg.status_flags & uavcan::equipment::power::BatteryInfo::STATUS_FLAG_IN_USE; + // battery.priority = msg.; battery.capacity = msg.full_charge_capacity_wh; // battery.cycle_count = msg.; // battery.run_time_to_empty = msg.; // battery.average_time_to_empty = msg.; battery.serial_number = msg.model_instance_id; - battery.connected = true; - battery.source = battery_status_s::BATTERY_SOURCE_POWER_MODULE; - // battery.priority = msg.; + battery.id = msg.getSrcNodeID().get(); + + // battery.voltage_cell_v[0] = msg.; + // battery.max_cell_voltage_delta = msg.; + // battery.is_powering_off = msg.; - // battery.warning = msg.; + + determineWarning(battery.remaining); + battery.warning = _warning; publish(msg.getSrcNodeID().get(), &battery); } + +void +UavcanBatteryBridge::sumDischarged(hrt_abstime timestamp, float current_a) +{ + // Not a valid measurement + if (current_a < 0.f) { + // Because the measurement was invalid we need to stop integration + // and re-initialize with the next valid measurement + _last_timestamp = 0; + return; + } + + // Ignore first update because we don't know dt. + if (_last_timestamp != 0) { + const float dt = (timestamp - _last_timestamp) / 1e6; + // mAh since last loop: (current[A] * 1000 = [mA]) * (dt[s] / 3600 = [h]) + _discharged_mah_loop = (current_a * 1e3f) * (dt / 3600.f); + _discharged_mah += _discharged_mah_loop; + } + + _last_timestamp = timestamp; +} + +void +UavcanBatteryBridge::determineWarning(float remaining) +{ + // propagate warning state only if the state is higher, otherwise remain in current warning state + if (remaining < _param_bat_emergen_thr.get() || (_warning == battery_status_s::BATTERY_WARNING_EMERGENCY)) { + _warning = battery_status_s::BATTERY_WARNING_EMERGENCY; + + } else if (remaining < _param_bat_crit_thr.get() || (_warning == battery_status_s::BATTERY_WARNING_CRITICAL)) { + _warning = battery_status_s::BATTERY_WARNING_CRITICAL; + + } else if (remaining < _param_bat_low_thr.get() || (_warning == battery_status_s::BATTERY_WARNING_LOW)) { + _warning = battery_status_s::BATTERY_WARNING_LOW; + } +} diff --git a/src/drivers/uavcan/sensors/battery.hpp b/src/drivers/uavcan/sensors/battery.hpp index 34cad6ff13..4dfdfd010c 100644 --- a/src/drivers/uavcan/sensors/battery.hpp +++ b/src/drivers/uavcan/sensors/battery.hpp @@ -32,17 +32,18 @@ ****************************************************************************/ /** * @author Jacob Dahl + * @author Alex Klimaj */ #pragma once #include "sensor_bridge.hpp" - #include - #include +#include +#include -class UavcanBatteryBridge : public UavcanCDevSensorBridgeBase +class UavcanBatteryBridge : public UavcanCDevSensorBridgeBase, public ModuleParams { public: static const char *const NAME; @@ -56,6 +57,8 @@ public: private: void battery_sub_cb(const uavcan::ReceivedDataStructure &msg); + void sumDischarged(hrt_abstime timestamp, float current_a); + void determineWarning(float remaining); typedef uavcan::MethodBinder < UavcanBatteryBridge *, void (UavcanBatteryBridge::*) @@ -63,4 +66,15 @@ private: BatteryInfoCbBinder; uavcan::Subscriber _sub_battery; + + DEFINE_PARAMETERS( + (ParamFloat) _param_bat_low_thr, + (ParamFloat) _param_bat_crit_thr, + (ParamFloat) _param_bat_emergen_thr + ) + + float _discharged_mah = 0.f; + float _discharged_mah_loop = 0.f; + uint8_t _warning; + hrt_abstime _last_timestamp; };