From f650b91718ee92278cf36900e52922c5a67e2195 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 25 Mar 2020 11:27:05 +0100 Subject: [PATCH] battery: check source param inside battery lib This moves the handling of the BAT%d_SOURCE param inside of the battery library. Users of the library now pass the source instead of the flag whether to publish. The battery library then checks if the source is selected using the param and publishes accordingly. Since we removed the strange system_source flag, we now need to look at all batteries in commander. For current estimation - I think - it makes sense to sum them up. --- .../crazyflie/syslink/syslink_main.cpp | 3 +- msg/battery_status.msg | 6 +++- src/drivers/power_monitor/ina226/ina226.cpp | 22 ++++++-------- src/drivers/power_monitor/voxlpm/voxlpm.cpp | 6 ++-- src/drivers/uavcan/sensors/battery.cpp | 4 +-- src/lib/battery/battery.cpp | 10 ++++--- src/lib/battery/battery.h | 9 ++---- src/modules/battery_status/analog_battery.cpp | 4 +-- src/modules/battery_status/analog_battery.h | 4 +-- src/modules/battery_status/battery_status.cpp | 29 +++++++------------ src/modules/commander/Commander.cpp | 7 +++-- src/modules/esc_battery/EscBattery.cpp | 7 ++--- src/modules/simulator/simulator_mavlink.cpp | 5 ++-- 13 files changed, 55 insertions(+), 61 deletions(-) diff --git a/boards/bitcraze/crazyflie/syslink/syslink_main.cpp b/boards/bitcraze/crazyflie/syslink/syslink_main.cpp index ccc5de0261..e504bb28a3 100644 --- a/boards/bitcraze/crazyflie/syslink/syslink_main.cpp +++ b/boards/bitcraze/crazyflie/syslink/syslink_main.cpp @@ -411,7 +411,8 @@ Syslink::handle_message(syslink_message_t *msg) memcpy(&vbat, &msg->data[1], sizeof(float)); //memcpy(&iset, &msg->data[5], sizeof(float)); - _battery.updateBatteryStatus(t, vbat, -1, true, true, 0, 0, true); + _battery.updateBatteryStatus(t, vbat, -1, true, + battery_status_s::BATTERY_SOURCE_POWER_MODULE, 0, 0); // Update battery charge state diff --git a/msg/battery_status.msg b/msg/battery_status.msg index 02c5a041e7..b1ac564641 100644 --- a/msg/battery_status.msg +++ b/msg/battery_status.msg @@ -10,7 +10,11 @@ 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 -bool system_source # Whether or not a this battery is the active power source for VDD_5V_IN + +uint8 BATTERY_SOURCE_POWER_MODULE = 0 +uint8 BATTERY_SOURCE_EXTERNAL = 0 +uint8 BATTERY_SOURCE_ESCS = 0 +uint8 source # Battery source uint8 priority # Zero based priority is the connection on the Power Controller V1..Vn AKA BrickN-1 uint16 capacity # actual capacity of the battery uint16 cycle_count # number of discharge cycles the battery has experienced diff --git a/src/drivers/power_monitor/ina226/ina226.cpp b/src/drivers/power_monitor/ina226/ina226.cpp index 17e724eb93..d013ea62a0 100644 --- a/src/drivers/power_monitor/ina226/ina226.cpp +++ b/src/drivers/power_monitor/ina226/ina226.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019 PX4 Development Team. All rights reserved. + * Copyright (c) 2019-2020 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -88,10 +88,9 @@ INA226::INA226(I2CSPIBusOption bus_option, const int bus, int bus_frequency, int 0.0, 0.0, false, - false, // TODO: selected source? + battery_status_s::BATTERY_SOURCE_POWER_MODULE, 0, - 0.0, - true + 0.0 ); } @@ -262,10 +261,9 @@ INA226::collect() (float) _bus_voltage * INA226_VSCALE, (float) _current * _current_lsb, true, - true, // TODO: Determine if this is the selected source + battery_status_s::BATTERY_SOURCE_POWER_MODULE, 0, - _actuator_controls.control[actuator_controls_s::INDEX_THROTTLE], - true + _actuator_controls.control[actuator_controls_s::INDEX_THROTTLE] ); ret = OK; @@ -276,10 +274,9 @@ INA226::collect() 0.0, 0.0, false, - false, // TODO: selected source? + battery_status_s::BATTERY_SOURCE_POWER_MODULE, 0, - 0.0, - true + 0.0 ); ret = -1; perf_count(_comms_errors); @@ -352,10 +349,9 @@ INA226::RunImpl() 0.0f, 0.0f, false, - false, + battery_status_s::BATTERY_SOURCE_POWER_MODULE, 0, - 0.0f, - true + 0.0f ); if (init() != OK) { diff --git a/src/drivers/power_monitor/voxlpm/voxlpm.cpp b/src/drivers/power_monitor/voxlpm/voxlpm.cpp index bad222681e..b610e28ece 100644 --- a/src/drivers/power_monitor/voxlpm/voxlpm.cpp +++ b/src/drivers/power_monitor/voxlpm/voxlpm.cpp @@ -149,7 +149,8 @@ VOXLPM::measure() switch (_ch_type) { case VOXLPM_CH_TYPE_VBATT: { - _battery.updateBatteryStatus(tnow, _voltage, _amperage, true, true, 0, 0, true); + _battery.updateBatteryStatus(tnow, _voltage, _amperage, true, + battery_status_s::BATTERY_SOURCE_POWER_MODULE, 0, 0); } // fallthrough @@ -170,7 +171,8 @@ VOXLPM::measure() } else { switch (_ch_type) { case VOXLPM_CH_TYPE_VBATT: { - _battery.updateBatteryStatus(tnow, 0.0, 0.0, true, true, 0, 0, true); + _battery.updateBatteryStatus(tnow, 0.0, 0.0, true, + battery_status_s::BATTERY_SOURCE_POWER_MODULE, 0, 0); } break; diff --git a/src/drivers/uavcan/sensors/battery.cpp b/src/drivers/uavcan/sensors/battery.cpp index 6b04819da7..0377277b9e 100644 --- a/src/drivers/uavcan/sensors/battery.cpp +++ b/src/drivers/uavcan/sensors/battery.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019 PX4 Development Team. All rights reserved. + * Copyright (c) 2019-2020 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -95,7 +95,7 @@ UavcanBatteryBridge::battery_sub_cb(const uavcan::ReceivedDataStructuresource() == 0) { - actuator_controls_s ctrl{}; - _actuator_ctrl_0_sub.copy(&ctrl); - _analogBatteries[b]->updateBatteryStatusADC( - hrt_absolute_time(), - bat_voltage_adc_readings[b], - bat_current_adc_readings[b], - selected_source == b, - b, - ctrl.control[actuator_controls_s::INDEX_THROTTLE] - ); - } + actuator_controls_s ctrl{}; + _actuator_ctrl_0_sub.copy(&ctrl); + + _analogBatteries[b]->updateBatteryStatusADC( + hrt_absolute_time(), + bat_voltage_adc_readings[b], + bat_current_adc_readings[b], + battery_status_s::BATTERY_SOURCE_POWER_MODULE, + b, + ctrl.control[actuator_controls_s::INDEX_THROTTLE] + ); } } } diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 7f681c7a80..752121c886 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -3821,6 +3821,8 @@ void Commander::battery_status_check() // oldest timestamp. hrt_abstime oldest_update = hrt_absolute_time(); + _battery_current = 0.0f; + // Only iterate over connected batteries. We don't care if a disconnected battery is not regularly publishing. for (size_t i = 0; i < num_connected_batteries; i++) { if (batteries[i].warning > worst_warning) { @@ -3831,9 +3833,8 @@ void Commander::battery_status_check() oldest_update = batteries[i].timestamp; } - if (batteries[i].system_source) { - _battery_current = batteries[i].current_filtered_a; - } + // Sum up current from all batteries. + _battery_current += batteries[i].current_filtered_a; } bool battery_warning_level_increased_while_armed = false; diff --git a/src/modules/esc_battery/EscBattery.cpp b/src/modules/esc_battery/EscBattery.cpp index a1391f5c24..e1a655ab0a 100644 --- a/src/modules/esc_battery/EscBattery.cpp +++ b/src/modules/esc_battery/EscBattery.cpp @@ -103,9 +103,7 @@ EscBattery::Run() average_voltage_v /= esc_status.esc_count; const bool connected = true; - const bool selected_source = true; const int priority = 0; - const bool should_publish = true; actuator_controls_s ctrl; _actuator_ctrl_0_sub.copy(&ctrl); @@ -115,10 +113,9 @@ EscBattery::Run() average_voltage_v, total_current_a, connected, - selected_source, + battery_status_s::BATTERY_SOURCE_ESCS, priority, - ctrl.control[actuator_controls_s::INDEX_THROTTLE], - should_publish); + ctrl.control[actuator_controls_s::INDEX_THROTTLE]); _battery.publish(); } } diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index 61b01d2b24..7b336b9796 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -2,7 +2,7 @@ * * Copyright (c) 2015 Mark Charlebois. All rights reserved. * Copyright (c) 2016 Anton Matosov. All rights reserved. - * Copyright (c) 2017-2019 PX4 Development Team. All rights reserved. + * Copyright (c) 2017-2020 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -366,7 +366,8 @@ void Simulator::handle_message_hil_sensor(const mavlink_message_t *msg) vbatt *= _battery.cell_count(); const float throttle = 0.0f; // simulate no throttle compensation to make the estimate predictable - _battery.updateBatteryStatus(now_us, vbatt, ibatt, true, true, 0, throttle, true); + _battery.updateBatteryStatus(now_us, vbatt, ibatt, true, battery_status_s::BATTERY_SOURCE_POWER_MODULE, + 0, throttle); _last_battery_timestamp = now_us; }