From 8e8510f3988f41189d2f0d9f3e1782e8338aad0b Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Thu, 29 Jun 2017 10:45:52 -1000 Subject: [PATCH] Added Power Brick related battery_status.msg fields system_source - This battery status is for the brick that is supplying VDD_5V_IN priority - Zero based, This battery status is for the brick that is connected to the Power controller's N-1 priority input. V1..VN. 0 would normally be Brick1, 1 for Brick2 etc Battery now assigns connected from the api in the updateBatteryStatus, as well as system_source and priority --- msg/battery_status.msg | 5 ++++- src/drivers/px4io/px4io.cpp | 4 +++- src/modules/simulator/simulator_mavlink.cpp | 2 +- src/modules/syslink/syslink_main.cpp | 2 +- src/modules/systemlib/battery.cpp | 8 ++++++-- src/modules/systemlib/battery.h | 7 ++++++- .../drivers/df_bebop_bus_wrapper/df_bebop_bus_wrapper.cpp | 3 ++- 7 files changed, 23 insertions(+), 8 deletions(-) diff --git a/msg/battery_status.msg b/msg/battery_status.msg index 32baad4a83..0422ecd979 100644 --- a/msg/battery_status.msg +++ b/msg/battery_status.msg @@ -6,7 +6,10 @@ float32 discharged_mah # Discharged amount in mAh, -1 if unknown float32 remaining # From 1 to 0, -1 if unknown float32 scale # Power scaling factor, >= 1, or -1 if unknown int32 cell_count # Number of cells -bool connected # Wether or not a battery is connected +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 priority # Zerro based priority is the connection on the Power Controller V1..Vn AKA BrickN-1 + #bool is_powering_off # Power off event imminent indication, false if unknown diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 98054b894c..5a57df0707 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1798,7 +1798,9 @@ PX4IO::io_handle_battery(uint16_t vbatt, uint16_t ibatt) float current_a = ibatt * (3.3f / 4096.0f) * _battery_amp_per_volt; current_a += _battery_amp_bias; - _battery.updateBatteryStatus(timestamp, voltage_v, current_a, _last_throttle, _armed, &battery_status); + _battery.updateBatteryStatus(timestamp, voltage_v, current_a, true, true, 0, + _last_throttle, + _armed, &battery_status); /* the announced battery status would conflict with the simulated battery status in HIL */ if (!(_pub_blocked)) { diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index c9a1cccf07..7e3a432624 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -331,7 +331,7 @@ void Simulator::handle_message(mavlink_message_t *msg, bool publish) // TODO: don't hard-code throttle. const float throttle = 0.5f; - _battery.updateBatteryStatus(now, vbatt, ibatt, throttle, armed, &battery_status); + _battery.updateBatteryStatus(now, vbatt, ibatt, true, true, 0, throttle, armed, &battery_status); // publish the battery voltage int batt_multi; diff --git a/src/modules/syslink/syslink_main.cpp b/src/modules/syslink/syslink_main.cpp index 22bb043418..5e4ceb0bbb 100644 --- a/src/modules/syslink/syslink_main.cpp +++ b/src/modules/syslink/syslink_main.cpp @@ -414,7 +414,7 @@ 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, 0, false, &_battery_status); + _battery.updateBatteryStatus(t, vbat, -1, true, true, 0, 0, false, &_battery_status); // Update battery charge state diff --git a/src/modules/systemlib/battery.cpp b/src/modules/systemlib/battery.cpp index f9937ba705..b531cdcc8a 100644 --- a/src/modules/systemlib/battery.cpp +++ b/src/modules/systemlib/battery.cpp @@ -83,7 +83,9 @@ Battery::reset(battery_status_s *battery_status) } void -Battery::updateBatteryStatus(hrt_abstime timestamp, float voltage_v, float current_a, float throttle_normalized, +Battery::updateBatteryStatus(hrt_abstime timestamp, float voltage_v, float current_a, + bool connected, bool selected_source, int priority, + float throttle_normalized, bool armed, battery_status_s *battery_status) { reset(battery_status); @@ -104,7 +106,9 @@ Battery::updateBatteryStatus(hrt_abstime timestamp, float voltage_v, float curre battery_status->discharged_mah = _discharged_mah; battery_status->warning = _warning; battery_status->remaining = _remaining; - battery_status->connected = true; + battery_status->connected = connected; + battery_status->system_source = selected_source; + battery_status->priority = priority; } } diff --git a/src/modules/systemlib/battery.h b/src/modules/systemlib/battery.h index e6efdeece1..45c2d5b8c7 100644 --- a/src/modules/systemlib/battery.h +++ b/src/modules/systemlib/battery.h @@ -85,9 +85,14 @@ public: * * @param voltage_v: current voltage in V * @param current_a: current current in A + * @param connected: Battery is connected + * @param selected_source: This battery is on the brick that the selected source for selected_source + * @param priority: The brick number -1. The term priority refers to the Vn connection on the LTC4417 * @param throttle_normalized: throttle from 0 to 1 */ - void updateBatteryStatus(hrt_abstime timestamp, float voltage_v, float current_a, float throttle_normalized, + void updateBatteryStatus(hrt_abstime timestamp, float voltage_v, float current_a, + bool connected, bool selected_source, int priority, + float throttle_normalized, bool armed, battery_status_s *status); private: diff --git a/src/platforms/posix/drivers/df_bebop_bus_wrapper/df_bebop_bus_wrapper.cpp b/src/platforms/posix/drivers/df_bebop_bus_wrapper/df_bebop_bus_wrapper.cpp index bb3e9cab4a..8446b01fd4 100644 --- a/src/platforms/posix/drivers/df_bebop_bus_wrapper/df_bebop_bus_wrapper.cpp +++ b/src/platforms/posix/drivers/df_bebop_bus_wrapper/df_bebop_bus_wrapper.cpp @@ -202,7 +202,8 @@ int DfBebopBusWrapper::_publish(struct bebop_state_data &data) // TODO Check if this is the right way for the Bebop // We don't have current measurements - _battery.updateBatteryStatus(timestamp, data.battery_voltage_v, 0.0, _last_throttle, _armed, &battery_report); + _battery.updateBatteryStatus(timestamp, data.battery_voltage_v, 0.0, true, true, 0, _last_throttle, _armed, + &battery_report); esc_status_s esc_status = {};