From 33701aa3d593283182dee92fa989aabedc9cdda2 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Wed, 3 Jul 2024 15:00:02 +0200 Subject: [PATCH] BatteryStatus: remove voltage_filtered_a Signed-off-by: Silvan Fuhrer --- boards/modalai/voxl2-slpi/src/drivers/dsp_hitl/dsp_hitl.cpp | 1 - msg/BatteryStatus.msg | 1 - src/drivers/batt_smbus/batt_smbus.cpp | 1 - src/drivers/cyphal/Subscribers/legacy/LegacyBatteryInfo.hpp | 1 - src/drivers/osd/atxxxx/atxxxx.cpp | 4 ++-- src/drivers/osd/atxxxx/atxxxx.h | 2 +- src/drivers/rc/crsf_rc/CrsfRc.cpp | 2 +- src/drivers/rc_input/crsf_telemetry.cpp | 2 +- src/drivers/rc_input/ghst_telemetry.cpp | 2 +- src/drivers/smart_battery/batmon/batmon.cpp | 1 - src/drivers/tattu_can/TattuCan.cpp | 1 - src/drivers/uavcan/sensors/battery.cpp | 1 - src/lib/drivers/smbus_sbs/SBS.hpp | 1 - src/modules/mavlink/mavlink_receiver.cpp | 3 --- src/modules/mavlink/streams/BATTERY_STATUS.hpp | 4 ++-- src/modules/mavlink/streams/SYS_STATUS.hpp | 2 +- 16 files changed, 9 insertions(+), 20 deletions(-) diff --git a/boards/modalai/voxl2-slpi/src/drivers/dsp_hitl/dsp_hitl.cpp b/boards/modalai/voxl2-slpi/src/drivers/dsp_hitl/dsp_hitl.cpp index b26f56508216..380ebc04d004 100644 --- a/boards/modalai/voxl2-slpi/src/drivers/dsp_hitl/dsp_hitl.cpp +++ b/boards/modalai/voxl2-slpi/src/drivers/dsp_hitl/dsp_hitl.cpp @@ -1148,7 +1148,6 @@ handle_message_hil_sensor_dsp(mavlink_message_t *msg) hil_battery_status.timestamp = gyro_accel_time; hil_battery_status.voltage_v = 16.0f; - hil_battery_status.voltage_filtered_v = 16.0f; hil_battery_status.current_a = 10.0f; hil_battery_status.discharged_mah = -1.0f; hil_battery_status.connected = true; diff --git a/msg/BatteryStatus.msg b/msg/BatteryStatus.msg index 8b22326d38d8..d0beda6dbc9d 100644 --- a/msg/BatteryStatus.msg +++ b/msg/BatteryStatus.msg @@ -1,7 +1,6 @@ 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 float32 current_average_a # Battery current average in amperes (for FW average in level flight), -1 if unknown float32 discharged_mah # Discharged amount in mAh, -1 if unknown diff --git a/src/drivers/batt_smbus/batt_smbus.cpp b/src/drivers/batt_smbus/batt_smbus.cpp index 7be94866d064..194b88fa94c0 100644 --- a/src/drivers/batt_smbus/batt_smbus.cpp +++ b/src/drivers/batt_smbus/batt_smbus.cpp @@ -116,7 +116,6 @@ void BATT_SMBUS::RunImpl() // Convert millivolts to volts. new_report.voltage_v = ((float)result) / 1000.0f; - new_report.voltage_filtered_v = new_report.voltage_v; // Read current. ret |= _interface->read_word(BATT_SMBUS_CURRENT, result); diff --git a/src/drivers/cyphal/Subscribers/legacy/LegacyBatteryInfo.hpp b/src/drivers/cyphal/Subscribers/legacy/LegacyBatteryInfo.hpp index 09c306848eb5..718a70844fda 100644 --- a/src/drivers/cyphal/Subscribers/legacy/LegacyBatteryInfo.hpp +++ b/src/drivers/cyphal/Subscribers/legacy/LegacyBatteryInfo.hpp @@ -76,7 +76,6 @@ class UavcanLegacyBatteryInfoSubscriber : public UavcanDynamicPortSubscriber battery_status_s bat_status {0}; bat_status.timestamp = hrt_absolute_time(); - bat_status.voltage_filtered_v = bat_info.voltage; bat_status.current_average_a = bat_info.average_power_10sec; bat_status.remaining = bat_info.state_of_charge_pct / 100.0f; bat_status.scale = -1; diff --git a/src/drivers/osd/atxxxx/atxxxx.cpp b/src/drivers/osd/atxxxx/atxxxx.cpp index b504b943c79a..a5acabd3919d 100644 --- a/src/drivers/osd/atxxxx/atxxxx.cpp +++ b/src/drivers/osd/atxxxx/atxxxx.cpp @@ -241,7 +241,7 @@ OSDatxxxx::add_battery_info(uint8_t pos_x, uint8_t pos_y) int ret = PX4_OK; // TODO: show battery symbol based on battery fill level - snprintf(buf, sizeof(buf), "%c%5.2f", OSD_SYMBOL_BATT_3, (double)_battery_voltage_filtered_v); + snprintf(buf, sizeof(buf), "%c%5.2f", OSD_SYMBOL_BATT_3, (double)_battery_voltage_v); buf[sizeof(buf) - 1] = '\0'; for (int i = 0; buf[i] != '\0'; i++) { @@ -330,7 +330,7 @@ OSDatxxxx::update_topics() _battery_sub.copy(&battery); if (battery.connected) { - _battery_voltage_filtered_v = battery.voltage_filtered_v; + _battery_voltage_v = battery.voltage_v; _battery_discharge_mah = battery.discharged_mah; _battery_valid = true; diff --git a/src/drivers/osd/atxxxx/atxxxx.h b/src/drivers/osd/atxxxx/atxxxx.h index e821e65deb32..10685eec9b98 100644 --- a/src/drivers/osd/atxxxx/atxxxx.h +++ b/src/drivers/osd/atxxxx/atxxxx.h @@ -111,7 +111,7 @@ class OSDatxxxx : public device::SPI, public ModuleParams, public I2CSPIDriverread_word(BATT_SMBUS_CURRENT, result); diff --git a/src/drivers/tattu_can/TattuCan.cpp b/src/drivers/tattu_can/TattuCan.cpp index ed1603c86921..1f414c090796 100644 --- a/src/drivers/tattu_can/TattuCan.cpp +++ b/src/drivers/tattu_can/TattuCan.cpp @@ -115,7 +115,6 @@ void TattuCan::Run() battery_status.state_of_health = static_cast(tattu_message.health_status); battery_status.voltage_v = static_cast(tattu_message.voltage) / 1000.0f; - battery_status.voltage_filtered_v = static_cast(tattu_message.voltage) / 1000.0f; battery_status.current_a = static_cast(tattu_message.current) / 1000.0f; battery_status.remaining = static_cast(tattu_message.remaining_percent) / 100.0f; battery_status.temperature = static_cast(tattu_message.temperature); diff --git a/src/drivers/uavcan/sensors/battery.cpp b/src/drivers/uavcan/sensors/battery.cpp index eecaec889667..77698cff16d1 100644 --- a/src/drivers/uavcan/sensors/battery.cpp +++ b/src/drivers/uavcan/sensors/battery.cpp @@ -104,7 +104,6 @@ UavcanBatteryBridge::battery_sub_cb(const uavcan::ReceivedDataStructure::populate_smbus_data(battery_status_s &data) // Convert millivolts to volts. data.voltage_v = ((float)result) * 0.001f; - data.voltage_filtered_v = data.voltage_v; // Read current. ret |= _interface->read_word(BATT_SMBUS_CURRENT, result); diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index cdd73df693cb..06fb97c8a65f 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -1770,7 +1770,6 @@ MavlinkReceiver::handle_message_battery_status(mavlink_message_t *msg) } battery_status.voltage_v = voltage_sum; - battery_status.voltage_filtered_v = voltage_sum; battery_status.current_a = (float)(battery_mavlink.current_battery) / 100.0f; battery_status.remaining = (float)battery_mavlink.battery_remaining / 100.0f; battery_status.discharged_mah = (float)battery_mavlink.current_consumed; @@ -2372,7 +2371,6 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) hil_battery_status.timestamp = timestamp; hil_battery_status.voltage_v = 16.0f; - hil_battery_status.voltage_filtered_v = 16.0f; hil_battery_status.current_a = 10.0f; hil_battery_status.discharged_mah = -1.0f; hil_battery_status.connected = true; @@ -2726,7 +2724,6 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) { battery_status_s hil_battery_status{}; hil_battery_status.voltage_v = 11.1f; - hil_battery_status.voltage_filtered_v = 11.1f; hil_battery_status.current_a = 10.0f; hil_battery_status.discharged_mah = -1.0f; hil_battery_status.timestamp = hrt_absolute_time(); diff --git a/src/modules/mavlink/streams/BATTERY_STATUS.hpp b/src/modules/mavlink/streams/BATTERY_STATUS.hpp index 0ab03d6d950d..c3d15054ac2f 100644 --- a/src/modules/mavlink/streams/BATTERY_STATUS.hpp +++ b/src/modules/mavlink/streams/BATTERY_STATUS.hpp @@ -161,10 +161,10 @@ class MavlinkStreamBatteryStatus : public MavlinkStream // If it doesn't fit, we have to split it into UINT16-1 chunks and the remaining // voltage for the subsequent field. // This won't work for voltages of more than 655 volts. - const int num_fields_required = static_cast(battery_status.voltage_filtered_v * 1000.f) / (UINT16_MAX - 1) + 1; + const int num_fields_required = static_cast(battery_status.voltage_v * 1000.f) / (UINT16_MAX - 1) + 1; if (num_fields_required <= mavlink_cell_slots) { - float remaining_voltage = battery_status.voltage_filtered_v * 1000.f; + float remaining_voltage = battery_status.voltage_v * 1000.f; for (int i = 0; i < num_fields_required - 1; ++i) { bat_msg.voltages[i] = UINT16_MAX - 1; diff --git a/src/modules/mavlink/streams/SYS_STATUS.hpp b/src/modules/mavlink/streams/SYS_STATUS.hpp index fc775a34d65b..272947b1ad77 100644 --- a/src/modules/mavlink/streams/SYS_STATUS.hpp +++ b/src/modules/mavlink/streams/SYS_STATUS.hpp @@ -166,7 +166,7 @@ class MavlinkStreamSysStatus : public MavlinkStream const battery_status_s &lowest_battery = battery_status[lowest_battery_index]; if (lowest_battery.connected) { - msg.voltage_battery = lowest_battery.voltage_filtered_v * 1000.0f; + msg.voltage_battery = lowest_battery.voltage_v * 1000.0f; msg.current_battery = lowest_battery.current_a * 100.0f; msg.battery_remaining = ceilf(lowest_battery.remaining * 100.0f);