diff --git a/msg/battery_status.msg b/msg/battery_status.msg index 45e0df136d..b14be9dccc 100644 --- a/msg/battery_status.msg +++ b/msg/battery_status.msg @@ -1,4 +1,5 @@ uint64 timestamp # time since system start (microseconds) +bool connected # Whether or not a battery is connected, based on a voltage threshold float32 voltage_v # Battery voltage in volts, 0 if unknown float32 voltage_filtered_v # Battery voltage in volts, filtered, 0 if unknown float32 current_a # Battery current in amperes, -1 if unknown @@ -9,7 +10,6 @@ float32 remaining # From 1 to 0, -1 if unknown float32 scale # Power scaling factor, >= 1, or -1 if unknown float32 temperature # temperature of the battery. NaN if unknown int32 cell_count # Number of cells -bool connected # Whether or not a battery is connected, based on a voltage threshold uint8 BATTERY_SOURCE_POWER_MODULE = 0 uint8 BATTERY_SOURCE_EXTERNAL = 1 diff --git a/src/lib/battery/battery.cpp b/src/lib/battery/battery.cpp index 9cd13bd90d..e0fbf561f7 100644 --- a/src/lib/battery/battery.cpp +++ b/src/lib/battery/battery.cpp @@ -125,44 +125,37 @@ void Battery::updateBatteryStatus(const hrt_abstime ×tamp, float voltage_v, if (_voltage_filter_v.getState() > 2.1f) { _battery_initialized = true; - _battery_status.voltage_v = voltage_v; - _battery_status.voltage_filtered_v = _voltage_filter_v.getState(); - _battery_status.current_a = current_a; - _battery_status.current_filtered_a = _current_filter_a.getState(); - _battery_status.current_average_a = -1.f; // support will follow - _battery_status.discharged_mah = _discharged_mah; - _battery_status.remaining = _state_of_charge; - _battery_status.scale = _scale; - _battery_status.temperature = NAN; - // Publish at least one cell such that the total voltage gets into MAVLink BATTERY_STATUS - _battery_status.cell_count = math::max(_params.n_cells, static_cast(1)); - _battery_status.connected = connected; - _battery_status.source = source; - _battery_status.priority = priority; - _battery_status.capacity = _params.capacity > 0.f ? static_cast(_params.capacity) : 0; - _battery_status.id = (uint8_t) _index; - _battery_status.warning = _warning; - - static constexpr int32_t uorb_max_cells = sizeof(_battery_status.voltage_cell_v) / sizeof( - _battery_status.voltage_cell_v[0]); - - int max_cells = math::min(_battery_status.cell_count, uorb_max_cells); - - // Fill cell voltages with average values to work around MAVLink BATTERY_STATUS not allowing to report just total voltage - for (int i = 0; i < max_cells; i++) { - _battery_status.voltage_cell_v[i] = _battery_status.voltage_filtered_v / max_cells; - } } else { - _battery_status.current_a = -1.f; - _battery_status.remaining = 1.f; - _battery_status.scale = 1.f; - // TODO: check if it is sane to reset warning to NONE - _battery_status.warning = battery_status_s::BATTERY_WARNING_NONE; - _battery_status.connected = false; - _battery_status.capacity = _params.capacity > 0.0f ? (uint16_t)_params.capacity : 0; - _battery_status.temperature = NAN; - _battery_status.id = (uint8_t) _index; + connected = false; + } + + _battery_status.voltage_v = voltage_v; + _battery_status.voltage_filtered_v = _voltage_filter_v.getState(); + _battery_status.current_a = current_a; + _battery_status.current_filtered_a = _current_filter_a.getState(); + _battery_status.current_average_a = -1.f; // support will follow + _battery_status.discharged_mah = _discharged_mah; + _battery_status.remaining = _state_of_charge; + _battery_status.scale = _scale; + _battery_status.temperature = NAN; + // Publish at least one cell such that the total voltage gets into MAVLink BATTERY_STATUS + _battery_status.cell_count = math::max(_params.n_cells, static_cast(1)); + _battery_status.connected = connected; + _battery_status.source = source; + _battery_status.priority = priority; + _battery_status.capacity = _params.capacity > 0.f ? static_cast(_params.capacity) : 0; + _battery_status.id = static_cast(_index); + _battery_status.warning = _warning; + + static constexpr int32_t uorb_max_cells = sizeof(_battery_status.voltage_cell_v) / sizeof( + _battery_status.voltage_cell_v[0]); + + int max_cells = math::min(_battery_status.cell_count, uorb_max_cells); + + // Fill cell voltages with average values to work around MAVLink BATTERY_STATUS not allowing to report just total voltage + for (int i = 0; i < max_cells; i++) { + _battery_status.voltage_cell_v[i] = _battery_status.voltage_filtered_v / max_cells; } if (source == _params.source) { diff --git a/src/modules/control_allocator/ControlAllocator.hpp b/src/modules/control_allocator/ControlAllocator.hpp index 01acd0bfb1..f46570f3e2 100644 --- a/src/modules/control_allocator/ControlAllocator.hpp +++ b/src/modules/control_allocator/ControlAllocator.hpp @@ -146,7 +146,6 @@ private: uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; - uORB::Subscription _battery_status_sub{ORB_ID(battery_status)}; /**< battery status subscription */ uORB::Subscription _airspeed_sub{ORB_ID(airspeed)}; /**< airspeed subscription */ uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp index 267dc4707c..65086c0180 100644 --- a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp +++ b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp @@ -565,10 +565,8 @@ void FixedwingAttitudeControl::Run() if (_battery_status_sub.updated()) { battery_status_s battery_status{}; - if (_battery_status_sub.copy(&battery_status)) { - if (battery_status.scale > 0.0f) { - _battery_scale = battery_status.scale; - } + if (_battery_status_sub.copy(&battery_status) && battery_status.connected && battery_status.scale > 0.f) { + _battery_scale = battery_status.scale; } } diff --git a/src/modules/mc_rate_control/MulticopterRateControl.cpp b/src/modules/mc_rate_control/MulticopterRateControl.cpp index aa3c46b551..4e907fc4d2 100644 --- a/src/modules/mc_rate_control/MulticopterRateControl.cpp +++ b/src/modules/mc_rate_control/MulticopterRateControl.cpp @@ -241,7 +241,7 @@ MulticopterRateControl::Run() if (_battery_status_sub.updated()) { battery_status_s battery_status; - if (_battery_status_sub.copy(&battery_status)) { + if (_battery_status_sub.copy(&battery_status) && battery_status.connected && battery_status.scale > 0.f) { _battery_status_scale = battery_status.scale; } }