diff --git a/msg/vehicle_status.msg b/msg/vehicle_status.msg index 2a7e493a39..430f7dd768 100644 --- a/msg/vehicle_status.msg +++ b/msg/vehicle_status.msg @@ -147,6 +147,8 @@ float32 load # processor load from 0 to 1 float32 battery_voltage float32 battery_current float32 battery_remaining +float32 battery_discharged_mah +uint32 battery_cell_count uint8 battery_warning # current battery warning mode, as defined by VEHICLE_BATTERY_WARNING enum uint16 drop_rate_comm diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 7fbb0a2dac..9676ad53e2 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1502,7 +1502,9 @@ int commander_thread_main(int argc, char *argv[]) if (hrt_absolute_time() > commander_boot_timestamp + 2000000 && battery.voltage_filtered_v > 0.0f) { status.battery_voltage = battery.voltage_filtered_v; status.battery_current = battery.current_a; + status.battery_discharged_mah = battery.discharged_mah; status.condition_battery_voltage_valid = true; + status.battery_cell_count = battery_get_n_cells(); /* get throttle (if armed), as we only care about energy negative throttle also counts */ float throttle = (armed.armed) ? fabsf(actuator_controls.control[3]) : 0.0f; diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index 99b926be4d..c0f8561fda 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -330,6 +330,10 @@ void rgbled_set_pattern(rgbled_pattern_t *pattern) ioctl(rgbleds, RGBLED_SET_PATTERN, (unsigned long)pattern); } +unsigned battery_get_n_cells() { + return bat_n_cells; +} + float battery_remaining_estimate_voltage(float voltage, float discharged, float throttle_normalized) { float ret = 0; diff --git a/src/modules/commander/commander_helper.h b/src/modules/commander/commander_helper.h index bf0c0505d2..d2aace2a40 100644 --- a/src/modules/commander/commander_helper.h +++ b/src/modules/commander/commander_helper.h @@ -89,4 +89,6 @@ int battery_init(); */ float battery_remaining_estimate_voltage(float voltage, float discharged, float throttle_normalized); +unsigned battery_get_n_cells(); + #endif /* COMMANDER_HELPER_H_ */ diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 7ddc52fd1a..3883cb84a9 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -543,6 +543,27 @@ protected: status.battery_remaining * 100.0f : -1; _mavlink->send_message(MAVLINK_MSG_ID_SYS_STATUS, &msg); + + /* battery status message with higher resolution */ + mavlink_battery_status_t bat_msg; + bat_msg.id = 0; + bat_msg.battery_function = MAV_BATTERY_FUNCTION_ALL; + bat_msg.type = MAV_BATTERY_TYPE_LIPO; + bat_msg.temperature = INT16_MAX; + for (unsigned i = 0; i < (sizeof(bat_msg.voltages) / sizeof(bat_msg.voltages[0])); i++) { + if (i < status.battery_cell_count) { + bat_msg.voltages[i] = (status.battery_voltage / status.battery_cell_count) * 1000.0f; + } else { + bat_msg.voltages[i] = 0; + } + } + bat_msg.current_battery = status.battery_current * 100.0f; + bat_msg.current_consumed = status.battery_discharged_mah; + bat_msg.energy_consumed = -1.0f; + bat_msg.battery_remaining = (status.battery_voltage > 0) ? + status.battery_remaining * 100.0f : -1; + + _mavlink->send_message(MAVLINK_MSG_ID_BATTERY_STATUS, &bat_msg); } } };