From c2ae6a7e24c70e5d3989343127a7b14839f71a01 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Wed, 3 Jul 2024 14:47:16 +0200 Subject: [PATCH] BatteryStatus: remove current_filtered_a Signed-off-by: Silvan Fuhrer --- msg/BatteryStatus.msg | 1 - src/drivers/batt_smbus/batt_smbus.cpp | 1 - .../cyphal/Subscribers/legacy/LegacyBatteryInfo.hpp | 1 - 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/battery/int_res_est_replay.py | 13 ++++--------- src/lib/drivers/smbus_sbs/SBS.hpp | 1 - src/modules/mavlink/mavlink_receiver.cpp | 1 - src/modules/mavlink/streams/BATTERY_STATUS.hpp | 2 +- src/modules/mavlink/streams/SYS_STATUS.hpp | 2 +- 14 files changed, 9 insertions(+), 22 deletions(-) diff --git a/msg/BatteryStatus.msg b/msg/BatteryStatus.msg index b13f721a5a..8b22326d38 100644 --- a/msg/BatteryStatus.msg +++ b/msg/BatteryStatus.msg @@ -3,7 +3,6 @@ bool connected # Whether or not a battery is connected, based on a voltage th 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_filtered_a # Battery current in amperes, filtered, 0 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 float32 remaining # From 1 to 0, -1 if unknown diff --git a/src/drivers/batt_smbus/batt_smbus.cpp b/src/drivers/batt_smbus/batt_smbus.cpp index 913bbbe7b8..7be94866d0 100644 --- a/src/drivers/batt_smbus/batt_smbus.cpp +++ b/src/drivers/batt_smbus/batt_smbus.cpp @@ -122,7 +122,6 @@ void BATT_SMBUS::RunImpl() ret |= _interface->read_word(BATT_SMBUS_CURRENT, result); new_report.current_a = (-1.0f * ((float)(*(int16_t *)&result)) / 1000.0f) * _c_mult; - new_report.current_filtered_a = new_report.current_a; // Read average current. ret |= _interface->read_word(BATT_SMBUS_AVERAGE_CURRENT, result); diff --git a/src/drivers/cyphal/Subscribers/legacy/LegacyBatteryInfo.hpp b/src/drivers/cyphal/Subscribers/legacy/LegacyBatteryInfo.hpp index 4a4d316ee0..09c306848e 100644 --- a/src/drivers/cyphal/Subscribers/legacy/LegacyBatteryInfo.hpp +++ b/src/drivers/cyphal/Subscribers/legacy/LegacyBatteryInfo.hpp @@ -77,7 +77,6 @@ public: battery_status_s bat_status {0}; bat_status.timestamp = hrt_absolute_time(); bat_status.voltage_filtered_v = bat_info.voltage; - bat_status.current_filtered_a = bat_info.current; 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/rc/crsf_rc/CrsfRc.cpp b/src/drivers/rc/crsf_rc/CrsfRc.cpp index 119e24b07c..930b7f1384 100644 --- a/src/drivers/rc/crsf_rc/CrsfRc.cpp +++ b/src/drivers/rc/crsf_rc/CrsfRc.cpp @@ -225,7 +225,7 @@ void CrsfRc::Run() if (_battery_status_sub.update(&battery_status)) { uint16_t voltage = battery_status.voltage_filtered_v * 10; - uint16_t current = battery_status.current_filtered_a * 10; + uint16_t current = battery_status.current_a * 10; int fuel = battery_status.discharged_mah; uint8_t remaining = battery_status.remaining * 100; this->SendTelemetryBattery(voltage, current, fuel, remaining); diff --git a/src/drivers/rc_input/crsf_telemetry.cpp b/src/drivers/rc_input/crsf_telemetry.cpp index 720437c823..aa4c5ef85d 100644 --- a/src/drivers/rc_input/crsf_telemetry.cpp +++ b/src/drivers/rc_input/crsf_telemetry.cpp @@ -82,7 +82,7 @@ bool CRSFTelemetry::send_battery() } uint16_t voltage = battery_status.voltage_filtered_v * 10; - uint16_t current = battery_status.current_filtered_a * 10; + uint16_t current = battery_status.current_a * 10; int fuel = battery_status.discharged_mah; uint8_t remaining = battery_status.remaining * 100; return crsf_send_telemetry_battery(_uart_fd, voltage, current, fuel, remaining); diff --git a/src/drivers/rc_input/ghst_telemetry.cpp b/src/drivers/rc_input/ghst_telemetry.cpp index 6edd0de598..910a519a50 100644 --- a/src/drivers/rc_input/ghst_telemetry.cpp +++ b/src/drivers/rc_input/ghst_telemetry.cpp @@ -91,7 +91,7 @@ bool GHSTTelemetry::send_battery_status() if (_battery_status_sub.update(&battery_status)) { voltage_in_10mV = battery_status.voltage_filtered_v * FACTOR_VOLTS_TO_10MV; - current_in_10mA = battery_status.current_filtered_a * FACTOR_AMPS_TO_10MA; + current_in_10mA = battery_status.current_a * FACTOR_AMPS_TO_10MA; fuel_in_10mAh = battery_status.discharged_mah * FACTOR_MAH_TO_10MAH; success = ghst_send_telemetry_battery_status(_uart_fd, static_cast(voltage_in_10mV), diff --git a/src/drivers/smart_battery/batmon/batmon.cpp b/src/drivers/smart_battery/batmon/batmon.cpp index 8cfe3031ca..6c6c484948 100644 --- a/src/drivers/smart_battery/batmon/batmon.cpp +++ b/src/drivers/smart_battery/batmon/batmon.cpp @@ -152,7 +152,6 @@ void Batmon::RunImpl() ret |= _interface->read_word(BATT_SMBUS_CURRENT, result); new_report.current_a = (-1.0f * ((float)(*(int16_t *)&result)) / 1000.0f); - new_report.current_filtered_a = new_report.current_a; // Read average current. ret |= _interface->read_word(BATT_SMBUS_AVERAGE_CURRENT, result); diff --git a/src/drivers/tattu_can/TattuCan.cpp b/src/drivers/tattu_can/TattuCan.cpp index 746a241372..ed1603c869 100644 --- a/src/drivers/tattu_can/TattuCan.cpp +++ b/src/drivers/tattu_can/TattuCan.cpp @@ -117,7 +117,6 @@ void TattuCan::Run() 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.current_filtered_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); battery_status.capacity = tattu_message.standard_capacity; diff --git a/src/drivers/uavcan/sensors/battery.cpp b/src/drivers/uavcan/sensors/battery.cpp index ab09c32740..eecaec8896 100644 --- a/src/drivers/uavcan/sensors/battery.cpp +++ b/src/drivers/uavcan/sensors/battery.cpp @@ -106,7 +106,6 @@ UavcanBatteryBridge::battery_sub_cb(const uavcan::ReceivedDataStructure::populate_smbus_data(battery_status_s &data) ret |= _interface->read_word(BATT_SMBUS_CURRENT, result); data.current_a = (-1.0f * ((float)(*(int16_t *)&result)) * 0.001f); - data.current_filtered_a = data.current_a; // Read remaining capacity. ret |= _interface->read_word(BATT_SMBUS_RELATIVE_SOC, result); diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index f8e12d51cc..cdd73df693 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -1772,7 +1772,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.current_filtered_a = battery_status.current_a; battery_status.remaining = (float)battery_mavlink.battery_remaining / 100.0f; battery_status.discharged_mah = (float)battery_mavlink.current_consumed; battery_status.cell_count = cell_count; diff --git a/src/modules/mavlink/streams/BATTERY_STATUS.hpp b/src/modules/mavlink/streams/BATTERY_STATUS.hpp index 73068f0de3..0ab03d6d95 100644 --- a/src/modules/mavlink/streams/BATTERY_STATUS.hpp +++ b/src/modules/mavlink/streams/BATTERY_STATUS.hpp @@ -74,7 +74,7 @@ private: bat_msg.type = MAV_BATTERY_TYPE_LIPO; bat_msg.current_consumed = (battery_status.connected) ? battery_status.discharged_mah : -1; bat_msg.energy_consumed = -1; - bat_msg.current_battery = (battery_status.connected) ? battery_status.current_filtered_a * 100 : -1; + bat_msg.current_battery = (battery_status.connected) ? battery_status.current_a * 100 : -1; bat_msg.battery_remaining = (battery_status.connected) ? roundf(battery_status.remaining * 100.f) : -1; // MAVLink extension: 0 is unsupported, in uORB it's NAN bat_msg.time_remaining = (battery_status.connected && (PX4_ISFINITE(battery_status.time_remaining_s))) ? diff --git a/src/modules/mavlink/streams/SYS_STATUS.hpp b/src/modules/mavlink/streams/SYS_STATUS.hpp index 9a2b2cb691..fc775a34d6 100644 --- a/src/modules/mavlink/streams/SYS_STATUS.hpp +++ b/src/modules/mavlink/streams/SYS_STATUS.hpp @@ -167,7 +167,7 @@ private: if (lowest_battery.connected) { msg.voltage_battery = lowest_battery.voltage_filtered_v * 1000.0f; - msg.current_battery = lowest_battery.current_filtered_a * 100.0f; + msg.current_battery = lowest_battery.current_a * 100.0f; msg.battery_remaining = ceilf(lowest_battery.remaining * 100.0f); } else {