yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-21 01:12:11 +00:00
Code Issues Packages Projects Releases Wiki Activity

battery: publish measured values also when battery not connected

Browse Source
This commit is contained in:
Matthias Grob
2021-06-30 12:00:50 +02:00
parent a99ddd0845
commit b824f33ae9
5 changed files with 33 additions and 43 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
msg/battery_status.msg
View File

@@ -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

65
src/lib/battery/battery.cpp
View File

@@ -125,44 +125,37 @@ void Battery::updateBatteryStatus(const hrt_abstime &timestamp, 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<int32_t>(1));
_battery_status.connected = connected;
_battery_status.source = source;
_battery_status.priority = priority;
_battery_status.capacity = _params.capacity > 0.f ? static_cast<uint16_t>(_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<int32_t>(1));
_battery_status.connected = connected;
_battery_status.source = source;
_battery_status.priority = priority;
_battery_status.capacity = _params.capacity > 0.f ? static_cast<uint16_t>(_params.capacity) : 0;
_battery_status.id = static_cast<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;
}
if (source == _params.source) {

1
src/modules/control_allocator/ControlAllocator.hpp
View File

@@ -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)};

6
src/modules/fw_att_control/FixedwingAttitudeControl.cpp
View File

@@ -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;
}
}

2
src/modules/mc_rate_control/MulticopterRateControl.cpp
View File

@@ -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;
}
}