From 302dcd94e59e0298f9da47397c19b931266f90bb Mon Sep 17 00:00:00 2001 From: Pawel Kowalski Date: Wed, 5 Apr 2023 18:21:44 +0200 Subject: [PATCH 01/15] add health info --- panther_battery/src/adc_node.py | 38 +++++++++++++++---- .../src/roboteq_republisher_node.py | 9 +++++ 2 files changed, 39 insertions(+), 8 deletions(-) diff --git a/panther_battery/src/adc_node.py b/panther_battery/src/adc_node.py index 8339b679..0be0d07d 100755 --- a/panther_battery/src/adc_node.py +++ b/panther_battery/src/adc_node.py @@ -1,6 +1,7 @@ #!/usr/bin/python3 import math +from threading import Lock import rospy @@ -11,14 +12,16 @@ class ADCNode: BAT02_DETECT_THRESH = 3.03 + V_BAT_FATAL_MIN = 25.0 def __init__(self, name: str) -> None: rospy.init_node(name, anonymous=False) - self._V_driv_front = None - self._V_driv_rear = None - self._I_driv_front = None - self._I_driv_rear = None + self._high_bat_temp = rospy.get_param('~high_bat_temp', 55.0) + + self._driver_battery_last_info_time = None + self._driver_battery_voltage = None + self._driver_battery_current = None self._A = 298.15 self._B = 3977.0 @@ -29,6 +32,8 @@ def __init__(self, name: str) -> None: self._battery_count = self._check_battery_count() self._battery_charging = True # const for now, later this will be evaluated + self._lock = Lock() + # ------------------------------- # Subscribers # ------------------------------- @@ -59,10 +64,12 @@ def __init__(self, name: str) -> None: rospy.loginfo(f'[{rospy.get_name()}] Node started') def _motor_controllers_state_cb(self, driver_state: DriverState) -> None: - self._V_driv_front = driver_state.front.voltage - self._V_driv_rear = driver_state.front.current - self._I_driv_front = driver_state.rear.voltage - self._I_driv_rear = driver_state.rear.current + with self._lock: + self._driver_battery_last_info_time = rospy.get_time() + self._driver_battery_voltage = ( + driver_state.front.voltage + driver_state.rear.voltage + ) / 2.0 + self._driver_battery_current = driver_state.front.current + driver_state.rear.current def _io_state_cb(self, io_state: IOState) -> None: self._charger_connected = io_state.charger_connected @@ -186,6 +193,21 @@ def _publish_battery_msg( else: battery_msg.power_supply_status = BatteryState.POWER_SUPPLY_STATUS_DISCHARGING + # check battery health + with self._lock: + if V_bat < self.V_BAT_FATAL_MIN: + battery_msg.power_supply_health = BatteryState.POWER_SUPPLY_HEALTH_DEAD + elif battery_msg.percentage > 1.1: + battery_msg.power_supply_health = BatteryState.POWER_SUPPLY_HEALTH_OVERVOLTAGE + elif temp_bat >= self._high_bat_temp: + battery_msg.power_supply_health = BatteryState.POWER_SUPPLY_HEALTH_OVERHEAT + elif self._driver_battery_last_info_time is None: + battery_msg.power_supply_health = BatteryState.POWER_SUPPLY_HEALTH_UNKNOWN + elif rospy.get_time() - self._driver_battery_last_info_time < 0.1 and abs(V_bat - self._driver_battery_voltage) > 1.0: + battery_msg.power_supply_health = BatteryState.POWER_SUPPLY_HEALTH_UNSPEC_FAILURE + else: + battery_msg.power_supply_health = BatteryState.POWER_SUPPLY_HEALTH_GOOD + bat_pub.publish(battery_msg) @staticmethod diff --git a/panther_battery/src/roboteq_republisher_node.py b/panther_battery/src/roboteq_republisher_node.py index 0f272982..89c7db23 100755 --- a/panther_battery/src/roboteq_republisher_node.py +++ b/panther_battery/src/roboteq_republisher_node.py @@ -10,6 +10,8 @@ class RoboteqRepublisherNode: + V_BAT_FATAL_MIN = 25.0 + def __init__(self, name: str) -> None: rospy.init_node(name, anonymous=False) @@ -71,6 +73,13 @@ def _battery_pub_timer_cb(self, *args) -> None: battery_msg.present = True # TODO: # battery_msg.power_supply_health + + if self._battery_voltage < self.V_BAT_FATAL_MIN: + battery_msg.power_supply_health = BatteryState.POWER_SUPPLY_HEALTH_DEAD + elif battery_msg.percentage > 1.1: + battery_msg.power_supply_health = BatteryState.POWER_SUPPLY_HEALTH_OVERVOLTAGE + else: + battery_msg.power_supply_health = BatteryState.POWER_SUPPLY_HEALTH_GOOD self._battery_pub.publish(battery_msg) From dfef905e076f335a59f4f51667b443fa4b1cbe3b Mon Sep 17 00:00:00 2001 From: Pawel Kowalski Date: Wed, 5 Apr 2023 18:30:01 +0200 Subject: [PATCH 02/15] Update README.md --- panther_battery/README.md | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/panther_battery/README.md b/panther_battery/README.md index b062451d..7118bb3e 100644 --- a/panther_battery/README.md +++ b/panther_battery/README.md @@ -29,3 +29,7 @@ Node publishing Panther battery state read from motor controllers. Used in Panth #### Subscribes - `/panther/driver/motor_controllers_state` [*panther_msgs/DriverState*]: current motor controllers' state and error flags. + +#### Parameters + +- `~high_bat_temp` [*float*, default: **55.0**]: The temperature of the battery at which the battery health state is incorrect. \ No newline at end of file From 396903429238964dab633bf4d5d6d634ed5d74f7 Mon Sep 17 00:00:00 2001 From: Pawel Kowalski Date: Thu, 6 Apr 2023 10:42:57 +0200 Subject: [PATCH 03/15] add logs --- panther_battery/src/adc_node.py | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/panther_battery/src/adc_node.py b/panther_battery/src/adc_node.py index 0be0d07d..5326190e 100755 --- a/panther_battery/src/adc_node.py +++ b/panther_battery/src/adc_node.py @@ -12,7 +12,7 @@ class ADCNode: BAT02_DETECT_THRESH = 3.03 - V_BAT_FATAL_MIN = 25.0 + V_BAT_FATAL_MIN = 27.0 def __init__(self, name: str) -> None: rospy.init_node(name, anonymous=False) @@ -194,16 +194,20 @@ def _publish_battery_msg( battery_msg.power_supply_status = BatteryState.POWER_SUPPLY_STATUS_DISCHARGING # check battery health - with self._lock: + with self._lock: if V_bat < self.V_BAT_FATAL_MIN: battery_msg.power_supply_health = BatteryState.POWER_SUPPLY_HEALTH_DEAD + rospy.logerr(f'[{rospy.get_name()}] battery voltage critically low!') elif battery_msg.percentage > 1.1: battery_msg.power_supply_health = BatteryState.POWER_SUPPLY_HEALTH_OVERVOLTAGE elif temp_bat >= self._high_bat_temp: battery_msg.power_supply_health = BatteryState.POWER_SUPPLY_HEALTH_OVERHEAT elif self._driver_battery_last_info_time is None: battery_msg.power_supply_health = BatteryState.POWER_SUPPLY_HEALTH_UNKNOWN - elif rospy.get_time() - self._driver_battery_last_info_time < 0.1 and abs(V_bat - self._driver_battery_voltage) > 1.0: + elif ( + rospy.get_time() - self._driver_battery_last_info_time < 0.1 + and abs(V_bat - self._driver_battery_voltage) > 1.0 + ): battery_msg.power_supply_health = BatteryState.POWER_SUPPLY_HEALTH_UNSPEC_FAILURE else: battery_msg.power_supply_health = BatteryState.POWER_SUPPLY_HEALTH_GOOD From 237fa1a49e0320b6cdde3c132c35a089eece4a79 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Pawe=C5=82=20Kowalski?= Date: Thu, 13 Apr 2023 14:56:04 +0200 Subject: [PATCH 04/15] add voltage mean count --- panther_battery/src/adc_node.py | 48 +++++++++++++++++++++++++-------- 1 file changed, 37 insertions(+), 11 deletions(-) diff --git a/panther_battery/src/adc_node.py b/panther_battery/src/adc_node.py index 5326190e..164816a3 100755 --- a/panther_battery/src/adc_node.py +++ b/panther_battery/src/adc_node.py @@ -1,7 +1,9 @@ #!/usr/bin/python3 +from collections import defaultdict import math from threading import Lock +from typing import Union, Optional import rospy @@ -19,9 +21,14 @@ def __init__(self, name: str) -> None: self._high_bat_temp = rospy.get_param('~high_bat_temp', 55.0) - self._driver_battery_last_info_time = None - self._driver_battery_voltage = None - self._driver_battery_current = None + self._driver_battery_last_info_time: Optional[float] = None + self._I_driv: Optional[float] = None + self._V_driv: Optional[float] = None + + self._volt_mean_length = 10 + self._V_bat_hist = defaultdict(lambda: [37.0] * self._volt_mean_length) + self._V_bat_mean = defaultdict(lambda: 37.0) + self._V_driv_mean: Optional[float] = None self._A = 298.15 self._B = 3977.0 @@ -66,10 +73,12 @@ def __init__(self, name: str) -> None: def _motor_controllers_state_cb(self, driver_state: DriverState) -> None: with self._lock: self._driver_battery_last_info_time = rospy.get_time() - self._driver_battery_voltage = ( - driver_state.front.voltage + driver_state.rear.voltage - ) / 2.0 - self._driver_battery_current = driver_state.front.current + driver_state.rear.current + + drver_voltage = (driver_state.front.voltage + driver_state.rear.voltage) / 2.0 + self._V_driv_mean = self._count_volt_mean('V_driv', drver_voltage) + self._V_driv = drver_voltage + + self._I_driv = driver_state.front.current + driver_state.rear.current def _io_state_cb(self, io_state: IOState) -> None: self._charger_connected = io_state.charger_connected @@ -182,6 +191,8 @@ def _publish_battery_msg( battery_msg.power_supply_technology = BatteryState.POWER_SUPPLY_TECHNOLOGY_LIPO battery_msg.present = True + V_bat_mean = self._count_volt_mean(bat_pub, V_bat) + # check battery status if self._charger_connected: if battery_msg.percentage >= 1.0: @@ -195,25 +206,40 @@ def _publish_battery_msg( # check battery health with self._lock: - if V_bat < self.V_BAT_FATAL_MIN: + erro_msg = None + if V_bat_mean < self.V_BAT_FATAL_MIN: battery_msg.power_supply_health = BatteryState.POWER_SUPPLY_HEALTH_DEAD - rospy.logerr(f'[{rospy.get_name()}] battery voltage critically low!') - elif battery_msg.percentage > 1.1: + erro_msg = 'Battery voltage is critically low!' + elif V_bat_mean > 43.0: battery_msg.power_supply_health = BatteryState.POWER_SUPPLY_HEALTH_OVERVOLTAGE + erro_msg = 'Battery overvoltage!' elif temp_bat >= self._high_bat_temp: battery_msg.power_supply_health = BatteryState.POWER_SUPPLY_HEALTH_OVERHEAT + erro_msg = 'Battery temperature is dangerously high!' elif self._driver_battery_last_info_time is None: battery_msg.power_supply_health = BatteryState.POWER_SUPPLY_HEALTH_UNKNOWN elif ( rospy.get_time() - self._driver_battery_last_info_time < 0.1 - and abs(V_bat - self._driver_battery_voltage) > 1.0 + and abs(V_bat_mean - self._V_driv_mean) > 4.0 ): battery_msg.power_supply_health = BatteryState.POWER_SUPPLY_HEALTH_UNSPEC_FAILURE else: battery_msg.power_supply_health = BatteryState.POWER_SUPPLY_HEALTH_GOOD + if erro_msg is not None: + rospy.logerr_throttle_identical(5.0, f'[{rospy.get_name()}] {erro_msg}') + bat_pub.publish(battery_msg) + def _count_volt_mean(self, pub: Union[rospy.Publisher, str], new_val: float) -> float: + self._V_bat_mean[pub] += ( + -self._V_bat_hist[pub][0] / self._volt_mean_length + new_val / self._volt_mean_length + ) + self._V_bat_hist[pub].pop(0) + self._V_bat_hist[pub].append(new_val) + + return self._V_bat_mean[pub] + @staticmethod def _read_file(path: str) -> int: with open(path, 'r') as file: From 3d22a748fe31e2edc7c63cc0b68e362e6bb5e2b0 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Pawe=C5=82=20Kowalski?= Date: Thu, 13 Apr 2023 15:50:39 +0200 Subject: [PATCH 05/15] update republisher node --- panther_battery/src/adc_node.py | 19 ++++---- .../src/roboteq_republisher_node.py | 44 +++++++++++++++---- 2 files changed, 45 insertions(+), 18 deletions(-) diff --git a/panther_battery/src/adc_node.py b/panther_battery/src/adc_node.py index 164816a3..6058247d 100755 --- a/panther_battery/src/adc_node.py +++ b/panther_battery/src/adc_node.py @@ -15,6 +15,7 @@ class ADCNode: BAT02_DETECT_THRESH = 3.03 V_BAT_FATAL_MIN = 27.0 + V_BAT_FATAL_MAX = 43.0 def __init__(self, name: str) -> None: rospy.init_node(name, anonymous=False) @@ -210,7 +211,7 @@ def _publish_battery_msg( if V_bat_mean < self.V_BAT_FATAL_MIN: battery_msg.power_supply_health = BatteryState.POWER_SUPPLY_HEALTH_DEAD erro_msg = 'Battery voltage is critically low!' - elif V_bat_mean > 43.0: + elif V_bat_mean > self.V_BAT_FATAL_MAX: battery_msg.power_supply_health = BatteryState.POWER_SUPPLY_HEALTH_OVERVOLTAGE erro_msg = 'Battery overvoltage!' elif temp_bat >= self._high_bat_temp: @@ -220,25 +221,25 @@ def _publish_battery_msg( battery_msg.power_supply_health = BatteryState.POWER_SUPPLY_HEALTH_UNKNOWN elif ( rospy.get_time() - self._driver_battery_last_info_time < 0.1 - and abs(V_bat_mean - self._V_driv_mean) > 4.0 + and abs(V_bat_mean - self._V_driv_mean) > self.V_BAT_FATAL_MAX * 0.1 ): battery_msg.power_supply_health = BatteryState.POWER_SUPPLY_HEALTH_UNSPEC_FAILURE else: battery_msg.power_supply_health = BatteryState.POWER_SUPPLY_HEALTH_GOOD if erro_msg is not None: - rospy.logerr_throttle_identical(5.0, f'[{rospy.get_name()}] {erro_msg}') + rospy.logerr_throttle_identical(10.0, f'[{rospy.get_name()}] {erro_msg}') bat_pub.publish(battery_msg) - def _count_volt_mean(self, pub: Union[rospy.Publisher, str], new_val: float) -> float: - self._V_bat_mean[pub] += ( - -self._V_bat_hist[pub][0] / self._volt_mean_length + new_val / self._volt_mean_length + def _count_volt_mean(self, label: Union[rospy.Publisher, str], new_val: float) -> float: + self._V_bat_mean[label] += ( + -self._V_bat_hist[label][0] / self._volt_mean_length + new_val / self._volt_mean_length ) - self._V_bat_hist[pub].pop(0) - self._V_bat_hist[pub].append(new_val) + self._V_bat_hist[label].pop(0) + self._V_bat_hist[label].append(new_val) - return self._V_bat_mean[pub] + return self._V_bat_mean[label] @staticmethod def _read_file(path: str) -> int: diff --git a/panther_battery/src/roboteq_republisher_node.py b/panther_battery/src/roboteq_republisher_node.py index 89c7db23..181c3b3d 100755 --- a/panther_battery/src/roboteq_republisher_node.py +++ b/panther_battery/src/roboteq_republisher_node.py @@ -1,6 +1,7 @@ #!/usr/bin/python3 from threading import Lock +from typing import Optional import rospy @@ -10,13 +11,19 @@ class RoboteqRepublisherNode: - V_BAT_FATAL_MIN = 25.0 + V_BAT_FATAL_MIN = 27.0 + V_BAT_FATAL_MAX = 43.0 def __init__(self, name: str) -> None: rospy.init_node(name, anonymous=False) - self._battery_voltage = None - self._battery_current = None + self._battery_voltage: Optional[float] = None + self._battery_current: Optional[float] = None + self._battery_voltage_mean = 37.0 + + self._volt_mean_length = 10 + self._battery_voltage_hist = [37.0] * self._volt_mean_length + self._battery_timeout = 1.0 self._last_battery_info_time = rospy.get_time() self._lock = Lock() @@ -46,9 +53,12 @@ def __init__(self, name: str) -> None: def _motor_controllers_state_cb(self, msg: DriverState) -> None: with self._lock: + new_voltage = (msg.front.voltage + msg.rear.voltage) / 2.0 + self._last_battery_info_time = rospy.get_time() - self._battery_voltage = (msg.front.voltage + msg.rear.voltage) / 2.0 + self._battery_voltage = new_voltage self._battery_current = msg.front.current + msg.rear.current + self._update_volt_mean(new_voltage) def _battery_pub_timer_cb(self, *args) -> None: with self._lock: @@ -57,6 +67,7 @@ def _battery_pub_timer_cb(self, *args) -> None: battery_msg.capacity = 20.0 battery_msg.design_capacity = 20.0 battery_msg.power_supply_technology = BatteryState.POWER_SUPPLY_TECHNOLOGY_LIPO + if ( self._battery_voltage == None or self._battery_current == None @@ -64,25 +75,40 @@ def _battery_pub_timer_cb(self, *args) -> None: ): battery_msg.power_supply_status = BatteryState.POWER_SUPPLY_STATUS_UNKNOWN else: - battery_msg.power_supply_status = BatteryState.POWER_SUPPLY_STATUS_DISCHARGING battery_msg.voltage = self._battery_voltage battery_msg.temperature = float('nan') battery_msg.current = self._battery_current battery_msg.percentage = (battery_msg.voltage - 32.0) / 10.0 battery_msg.charge = battery_msg.percentage * battery_msg.design_capacity battery_msg.present = True - # TODO: - # battery_msg.power_supply_health - + + # TODO: check battery status + battery_msg.power_supply_status = BatteryState.POWER_SUPPLY_STATUS_DISCHARGING + + # check battery health + erro_msg = None if self._battery_voltage < self.V_BAT_FATAL_MIN: battery_msg.power_supply_health = BatteryState.POWER_SUPPLY_HEALTH_DEAD - elif battery_msg.percentage > 1.1: + erro_msg = 'Battery voltage is critically low!' + elif self._battery_voltage > self.V_BAT_FATAL_MAX: battery_msg.power_supply_health = BatteryState.POWER_SUPPLY_HEALTH_OVERVOLTAGE + erro_msg = 'Battery overvoltage!' else: battery_msg.power_supply_health = BatteryState.POWER_SUPPLY_HEALTH_GOOD + if erro_msg is not None: + rospy.logerr_throttle_identical(10.0, f'[{rospy.get_name()}] {erro_msg}') + self._battery_pub.publish(battery_msg) + def _update_volt_mean(self, new_val: float) -> float: + self._battery_voltage_mean += ( + -self._battery_voltage_hist[0] / self._volt_mean_length + + new_val / self._volt_mean_length + ) + self._battery_voltage_hist.pop(0) + self._battery_voltage_hist.append(new_val) + def main(): roboteq_republisher_node = RoboteqRepublisherNode('roboteq_republisher_node') From 7d11dc6c92e068f2491a2a712a7f7dae35966722 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Pawe=C5=82=20Kowalski?= Date: Thu, 13 Apr 2023 16:07:43 +0200 Subject: [PATCH 06/15] fix typo --- panther_battery/src/adc_node.py | 16 ++++++++-------- panther_battery/src/roboteq_republisher_node.py | 10 +++++----- 2 files changed, 13 insertions(+), 13 deletions(-) diff --git a/panther_battery/src/adc_node.py b/panther_battery/src/adc_node.py index 6058247d..3c62afc1 100755 --- a/panther_battery/src/adc_node.py +++ b/panther_battery/src/adc_node.py @@ -3,7 +3,7 @@ from collections import defaultdict import math from threading import Lock -from typing import Union, Optional +from typing import Optional, Union import rospy @@ -76,8 +76,8 @@ def _motor_controllers_state_cb(self, driver_state: DriverState) -> None: self._driver_battery_last_info_time = rospy.get_time() drver_voltage = (driver_state.front.voltage + driver_state.rear.voltage) / 2.0 - self._V_driv_mean = self._count_volt_mean('V_driv', drver_voltage) self._V_driv = drver_voltage + self._V_driv_mean = self._count_volt_mean('V_driv', drver_voltage) self._I_driv = driver_state.front.current + driver_state.rear.current @@ -207,16 +207,16 @@ def _publish_battery_msg( # check battery health with self._lock: - erro_msg = None + error_msg = None if V_bat_mean < self.V_BAT_FATAL_MIN: battery_msg.power_supply_health = BatteryState.POWER_SUPPLY_HEALTH_DEAD - erro_msg = 'Battery voltage is critically low!' + error_msg = 'Battery voltage is critically low!' elif V_bat_mean > self.V_BAT_FATAL_MAX: battery_msg.power_supply_health = BatteryState.POWER_SUPPLY_HEALTH_OVERVOLTAGE - erro_msg = 'Battery overvoltage!' + error_msg = 'Battery overvoltage!' elif temp_bat >= self._high_bat_temp: battery_msg.power_supply_health = BatteryState.POWER_SUPPLY_HEALTH_OVERHEAT - erro_msg = 'Battery temperature is dangerously high!' + error_msg = 'Battery temperature is dangerously high!' elif self._driver_battery_last_info_time is None: battery_msg.power_supply_health = BatteryState.POWER_SUPPLY_HEALTH_UNKNOWN elif ( @@ -227,8 +227,8 @@ def _publish_battery_msg( else: battery_msg.power_supply_health = BatteryState.POWER_SUPPLY_HEALTH_GOOD - if erro_msg is not None: - rospy.logerr_throttle_identical(10.0, f'[{rospy.get_name()}] {erro_msg}') + if error_msg is not None: + rospy.logerr_throttle_identical(10.0, f'[{rospy.get_name()}] {error_msg}') bat_pub.publish(battery_msg) diff --git a/panther_battery/src/roboteq_republisher_node.py b/panther_battery/src/roboteq_republisher_node.py index 181c3b3d..afc53c31 100755 --- a/panther_battery/src/roboteq_republisher_node.py +++ b/panther_battery/src/roboteq_republisher_node.py @@ -86,18 +86,18 @@ def _battery_pub_timer_cb(self, *args) -> None: battery_msg.power_supply_status = BatteryState.POWER_SUPPLY_STATUS_DISCHARGING # check battery health - erro_msg = None + error_msg = None if self._battery_voltage < self.V_BAT_FATAL_MIN: battery_msg.power_supply_health = BatteryState.POWER_SUPPLY_HEALTH_DEAD - erro_msg = 'Battery voltage is critically low!' + error_msg = 'Battery voltage is critically low!' elif self._battery_voltage > self.V_BAT_FATAL_MAX: battery_msg.power_supply_health = BatteryState.POWER_SUPPLY_HEALTH_OVERVOLTAGE - erro_msg = 'Battery overvoltage!' + error_msg = 'Battery overvoltage!' else: battery_msg.power_supply_health = BatteryState.POWER_SUPPLY_HEALTH_GOOD - if erro_msg is not None: - rospy.logerr_throttle_identical(10.0, f'[{rospy.get_name()}] {erro_msg}') + if error_msg is not None: + rospy.logerr_throttle_identical(10.0, f'[{rospy.get_name()}] {error_msg}') self._battery_pub.publish(battery_msg) From 91389c38e5c84d86b660627398369a4f2f4a864b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Pawe=C5=82=20Kowalski?= <82044322+pkowalsk1@users.noreply.github.com> Date: Tue, 18 Apr 2023 21:32:32 +0200 Subject: [PATCH 07/15] Update panther_battery/src/adc_node.py Co-authored-by: Krzysztof Wojciechowski <49921081+Kotochleb@users.noreply.github.com> --- panther_battery/src/adc_node.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/panther_battery/src/adc_node.py b/panther_battery/src/adc_node.py index 3c62afc1..0d4fbe0c 100755 --- a/panther_battery/src/adc_node.py +++ b/panther_battery/src/adc_node.py @@ -216,7 +216,7 @@ def _publish_battery_msg( error_msg = 'Battery overvoltage!' elif temp_bat >= self._high_bat_temp: battery_msg.power_supply_health = BatteryState.POWER_SUPPLY_HEALTH_OVERHEAT - error_msg = 'Battery temperature is dangerously high!' + error_msg = 'Battery is overheating!' elif self._driver_battery_last_info_time is None: battery_msg.power_supply_health = BatteryState.POWER_SUPPLY_HEALTH_UNKNOWN elif ( From 29a902afedfe487a0660a28bb667b82514f2e598 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Pawe=C5=82=20Kowalski?= <82044322+pkowalsk1@users.noreply.github.com> Date: Tue, 18 Apr 2023 21:48:44 +0200 Subject: [PATCH 08/15] Update adc_node.py --- panther_battery/src/adc_node.py | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/panther_battery/src/adc_node.py b/panther_battery/src/adc_node.py index 0d4fbe0c..a00788c9 100755 --- a/panther_battery/src/adc_node.py +++ b/panther_battery/src/adc_node.py @@ -233,9 +233,10 @@ def _publish_battery_msg( bat_pub.publish(battery_msg) def _count_volt_mean(self, label: Union[rospy.Publisher, str], new_val: float) -> float: - self._V_bat_mean[label] += ( - -self._V_bat_hist[label][0] / self._volt_mean_length + new_val / self._volt_mean_length - ) + # Updates the average by adding the newest and removing the oldest component of mean value, + # in order to avoid recalculating the entire sum every time. + self._V_bat_mean[label] += (new_val - self._V_bat_hist[label][0]) / self._volt_mean_length + self._V_bat_hist[label].pop(0) self._V_bat_hist[label].append(new_val) From 960584467d3fbdaf4ed3213ef6c3aaa4f54760f2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Pawe=C5=82=20Kowalski?= <82044322+pkowalsk1@users.noreply.github.com> Date: Wed, 19 Apr 2023 13:52:01 +0200 Subject: [PATCH 09/15] Update roboteq_republisher_node.py --- panther_battery/src/roboteq_republisher_node.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/panther_battery/src/roboteq_republisher_node.py b/panther_battery/src/roboteq_republisher_node.py index afc53c31..5818f882 100755 --- a/panther_battery/src/roboteq_republisher_node.py +++ b/panther_battery/src/roboteq_republisher_node.py @@ -102,10 +102,10 @@ def _battery_pub_timer_cb(self, *args) -> None: self._battery_pub.publish(battery_msg) def _update_volt_mean(self, new_val: float) -> float: - self._battery_voltage_mean += ( - -self._battery_voltage_hist[0] / self._volt_mean_length - + new_val / self._volt_mean_length - ) + # Updates the average by adding the newest and removing the oldest component of mean value, + # in order to avoid recalculating the entire sum every time. + self._battery_voltage_mean += (new_val - self._battery_voltage_hist[0]) / self._volt_mean_length + self._battery_voltage_hist.pop(0) self._battery_voltage_hist.append(new_val) From 31614e5de6f3ecdf6679939f0eb6cc484da625d8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Pawe=C5=82=20Kowalski?= Date: Thu, 18 May 2023 13:01:36 +0200 Subject: [PATCH 10/15] small fixes --- panther_battery/src/adc_node.py | 70 +++++++++++++------ .../src/roboteq_republisher_node.py | 15 ++-- 2 files changed, 58 insertions(+), 27 deletions(-) diff --git a/panther_battery/src/adc_node.py b/panther_battery/src/adc_node.py index a00788c9..043e0e9a 100755 --- a/panther_battery/src/adc_node.py +++ b/panther_battery/src/adc_node.py @@ -13,9 +13,12 @@ class ADCNode: + BAT01_CHARGING_CURR_THRESH = 0.1 BAT02_DETECT_THRESH = 3.03 V_BAT_FATAL_MIN = 27.0 V_BAT_FATAL_MAX = 43.0 + V_BAT_FULL = 41.4 + V_BAT_MIN = 32.0 def __init__(self, name: str) -> None: rospy.init_node(name, anonymous=False) @@ -31,6 +34,10 @@ def __init__(self, name: str) -> None: self._V_bat_mean = defaultdict(lambda: 37.0) self._V_driv_mean: Optional[float] = None + self._curr_mean_length = 10 + self._I_bat_charge_hist = defaultdict(lambda: [0.0] * self._curr_mean_length) + self._I_bat_charge_mean = defaultdict(lambda: 0.0) + self._A = 298.15 self._B = 3977.0 self._R1 = 10000.0 @@ -38,7 +45,7 @@ def __init__(self, name: str) -> None: self._u_supply = 3.28 self._battery_count = self._check_battery_count() - self._battery_charging = True # const for now, later this will be evaluated + self._I_bat_charging_thresh = {} self._lock = Lock() @@ -61,6 +68,16 @@ def __init__(self, name: str) -> None: self._battery_1_pub = rospy.Publisher('battery_1', BatteryState, queue_size=1) self._battery_2_pub = rospy.Publisher('battery_2', BatteryState, queue_size=1) + self._I_bat_charging_thresh.update( + { + self._battery_pub: 2 * self.BAT01_CHARGING_CURR_THRESH, + self._battery_1_pub: self.BAT01_CHARGING_CURR_THRESH, + self._battery_2_pub: self.BAT01_CHARGING_CURR_THRESH, + } + ) + else: + self._I_bat_charging_thresh.update({self._battery_pub: self.BAT01_CHARGING_CURR_THRESH}) + # ------------------------------- # Timers # ------------------------------- @@ -75,9 +92,9 @@ def _motor_controllers_state_cb(self, driver_state: DriverState) -> None: with self._lock: self._driver_battery_last_info_time = rospy.get_time() - drver_voltage = (driver_state.front.voltage + driver_state.rear.voltage) / 2.0 - self._V_driv = drver_voltage - self._V_driv_mean = self._count_volt_mean('V_driv', drver_voltage) + driver_voltage = (driver_state.front.voltage + driver_state.rear.voltage) / 2.0 + self._V_driv = driver_voltage + self._V_driv_mean = self._count_volt_mean('V_driv', driver_voltage) self._I_driv = driver_state.front.current + driver_state.rear.current @@ -119,22 +136,18 @@ def _battery_timer_cb(self, *args) -> None: temp_bat_2 = self._voltage_to_deg(V_temp_bat_2) self._publish_battery_msg( - self._battery_1_pub, V_bat_1, temp_bat_1, -I_bat_1 + I_charge_bat_1 + self._battery_1_pub, V_bat_1, temp_bat_1, -I_bat_1 + I_charge_bat_1, I_charge_bat_1 ) self._publish_battery_msg( - self._battery_2_pub, V_bat_2, temp_bat_2, -I_bat_2 + I_charge_bat_2 + self._battery_2_pub, V_bat_2, temp_bat_2, -I_bat_2 + I_charge_bat_2, I_charge_bat_2 ) - V_bat_avereage = (V_bat_1 + V_bat_2) / 2.0 - temp_bat_average = (temp_bat_1 + temp_bat_2) / 2.0 - I_bat_sum = I_bat_1 + I_bat_2 - I_charge_bat = I_charge_bat_1 + I_charge_bat_2 - self._publish_battery_msg( self._battery_pub, - V_bat_avereage, - temp_bat_average, - -I_bat_sum + I_charge_bat, + (V_bat_1 + V_bat_2) / 2.0, + (temp_bat_1 + temp_bat_2) / 2.0, + -(I_bat_1 + I_bat_2) + I_charge_bat_1 + I_charge_bat_2, + I_charge_bat_1 + I_charge_bat_2, ) else: @@ -178,14 +191,16 @@ def _voltage_to_deg(self, V_temp: float) -> float: return (self._A * self._B / (self._A * math.log(R_therm / self._R0) + self._B)) - 273.15 def _publish_battery_msg( - self, bat_pub: rospy.Publisher, V_bat: float, temp_bat: float, I_bat: float + self, bat_pub: rospy.Publisher, V_bat: float, temp_bat: float, I_bat: float, I_charge: float ) -> None: battery_msg = BatteryState() battery_msg.header.stamp = rospy.Time.now() battery_msg.voltage = V_bat battery_msg.temperature = temp_bat battery_msg.current = I_bat - battery_msg.percentage = (battery_msg.voltage - 32.0) / 10.0 + battery_msg.percentage = (battery_msg.voltage - self.V_BAT_MIN) / ( + self.V_BAT_FULL - self.V_BAT_MIN + ) battery_msg.capacity = 20.0 battery_msg.design_capacity = 20.0 battery_msg.charge = battery_msg.percentage * battery_msg.design_capacity @@ -193,12 +208,14 @@ def _publish_battery_msg( battery_msg.present = True V_bat_mean = self._count_volt_mean(bat_pub, V_bat) + I_bat_mean = self._count_curr_mean(bat_pub, I_charge) + rospy.loginfo(f"{I_bat_mean}") # check battery status if self._charger_connected: if battery_msg.percentage >= 1.0: battery_msg.power_supply_status = BatteryState.POWER_SUPPLY_STATUS_FULL - elif self._battery_charging: + elif I_bat_mean > self._I_bat_charging_thresh[bat_pub]: battery_msg.power_supply_status = BatteryState.POWER_SUPPLY_STATUS_CHARGING else: battery_msg.power_supply_status = BatteryState.POWER_SUPPLY_STATUS_NOT_CHARGING @@ -219,11 +236,6 @@ def _publish_battery_msg( error_msg = 'Battery is overheating!' elif self._driver_battery_last_info_time is None: battery_msg.power_supply_health = BatteryState.POWER_SUPPLY_HEALTH_UNKNOWN - elif ( - rospy.get_time() - self._driver_battery_last_info_time < 0.1 - and abs(V_bat_mean - self._V_driv_mean) > self.V_BAT_FATAL_MAX * 0.1 - ): - battery_msg.power_supply_health = BatteryState.POWER_SUPPLY_HEALTH_UNSPEC_FAILURE else: battery_msg.power_supply_health = BatteryState.POWER_SUPPLY_HEALTH_GOOD @@ -236,12 +248,24 @@ def _count_volt_mean(self, label: Union[rospy.Publisher, str], new_val: float) - # Updates the average by adding the newest and removing the oldest component of mean value, # in order to avoid recalculating the entire sum every time. self._V_bat_mean[label] += (new_val - self._V_bat_hist[label][0]) / self._volt_mean_length - + self._V_bat_hist[label].pop(0) self._V_bat_hist[label].append(new_val) return self._V_bat_mean[label] + def _count_curr_mean(self, label: rospy.Publisher, new_val: float) -> float: + # Updates the average by adding the newest and removing the oldest component of mean value, + # in order to avoid recalculating the entire sum every time. + self._I_bat_charge_mean[label] += ( + new_val - self._I_bat_charge_hist[label][0] + ) / self._curr_mean_length + + self._I_bat_charge_hist[label].pop(0) + self._I_bat_charge_hist[label].append(new_val) + + return self._I_bat_charge_mean[label] + @staticmethod def _read_file(path: str) -> int: with open(path, 'r') as file: diff --git a/panther_battery/src/roboteq_republisher_node.py b/panther_battery/src/roboteq_republisher_node.py index 5818f882..610bceb3 100755 --- a/panther_battery/src/roboteq_republisher_node.py +++ b/panther_battery/src/roboteq_republisher_node.py @@ -13,10 +13,14 @@ class RoboteqRepublisherNode: V_BAT_FATAL_MIN = 27.0 V_BAT_FATAL_MAX = 43.0 - + V_BAT_FULL = 41.4 + V_BAT_MIN = 32.0 + def __init__(self, name: str) -> None: rospy.init_node(name, anonymous=False) + self._lock = Lock() + self._battery_voltage: Optional[float] = None self._battery_current: Optional[float] = None self._battery_voltage_mean = 37.0 @@ -26,7 +30,6 @@ def __init__(self, name: str) -> None: self._battery_timeout = 1.0 self._last_battery_info_time = rospy.get_time() - self._lock = Lock() # ------------------------------- # Subscribers @@ -78,7 +81,9 @@ def _battery_pub_timer_cb(self, *args) -> None: battery_msg.voltage = self._battery_voltage battery_msg.temperature = float('nan') battery_msg.current = self._battery_current - battery_msg.percentage = (battery_msg.voltage - 32.0) / 10.0 + battery_msg.percentage = (battery_msg.voltage - self.V_BAT_MIN) / ( + self.V_BAT_FULL - self.V_BAT_MIN + ) battery_msg.charge = battery_msg.percentage * battery_msg.design_capacity battery_msg.present = True @@ -104,7 +109,9 @@ def _battery_pub_timer_cb(self, *args) -> None: def _update_volt_mean(self, new_val: float) -> float: # Updates the average by adding the newest and removing the oldest component of mean value, # in order to avoid recalculating the entire sum every time. - self._battery_voltage_mean += (new_val - self._battery_voltage_hist[0]) / self._volt_mean_length + self._battery_voltage_mean += ( + new_val - self._battery_voltage_hist[0] + ) / self._volt_mean_length self._battery_voltage_hist.pop(0) self._battery_voltage_hist.append(new_val) From d011e75341c7994ca385bdbea0d357dd919abe3b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Pawe=C5=82=20Kowalski?= Date: Wed, 31 May 2023 12:16:10 +0200 Subject: [PATCH 11/15] review changes --- panther_battery/src/adc_node.py | 19 +++++++++++-------- .../src/roboteq_republisher_node.py | 8 +++++--- 2 files changed, 16 insertions(+), 11 deletions(-) diff --git a/panther_battery/src/adc_node.py b/panther_battery/src/adc_node.py index 1fd499c5..97947dfc 100755 --- a/panther_battery/src/adc_node.py +++ b/panther_battery/src/adc_node.py @@ -13,7 +13,7 @@ class ADCNode: - BAT01_CHARGING_CURR_THRESH = 0.1 + BAT_CHARGING_CURR_THRESH = 0.1 BAT02_DETECT_THRESH = 3.03 V_BAT_FATAL_MIN = 27.0 V_BAT_FATAL_MAX = 43.0 @@ -70,13 +70,13 @@ def __init__(self, name: str) -> None: self._I_bat_charging_thresh.update( { - self._battery_pub: 2 * self.BAT01_CHARGING_CURR_THRESH, - self._battery_1_pub: self.BAT01_CHARGING_CURR_THRESH, - self._battery_2_pub: self.BAT01_CHARGING_CURR_THRESH, + self._battery_pub: 2 * self.BAT_CHARGING_CURR_THRESH, + self._battery_1_pub: self.BAT_CHARGING_CURR_THRESH, + self._battery_2_pub: self.BAT_CHARGING_CURR_THRESH, } ) else: - self._I_bat_charging_thresh.update({self._battery_pub: self.BAT01_CHARGING_CURR_THRESH}) + self._I_bat_charging_thresh.update({self._battery_pub: self.BAT_CHARGING_CURR_THRESH}) # ------------------------------- # Timers @@ -199,8 +199,8 @@ def _publish_battery_msg( battery_msg.voltage = V_bat battery_msg.temperature = temp_bat battery_msg.current = I_bat - battery_msg.percentage = (battery_msg.voltage - self.V_BAT_MIN) / ( - self.V_BAT_FULL - self.V_BAT_MIN + battery_msg.percentage = self._clamp( + (battery_msg.voltage - self.V_BAT_MIN) / (self.V_BAT_FULL - self.V_BAT_MIN) ) battery_msg.capacity = 20.0 battery_msg.design_capacity = 20.0 @@ -214,7 +214,7 @@ def _publish_battery_msg( with self._lock: # check battery status if self._charger_connected: - if battery_msg.percentage >= 1.0: + if battery_msg.percentage == 1.0: battery_msg.power_supply_status = BatteryState.POWER_SUPPLY_STATUS_FULL elif I_bat_mean > self._I_bat_charging_thresh[bat_pub]: battery_msg.power_supply_status = BatteryState.POWER_SUPPLY_STATUS_CHARGING @@ -273,6 +273,9 @@ def _read_file(path: str) -> int: return int(data) + def _clamp(value, max_value, min_value): + return max(min(value, max_value), min_value) + def main(): adc_node = ADCNode('adc_node') diff --git a/panther_battery/src/roboteq_republisher_node.py b/panther_battery/src/roboteq_republisher_node.py index 1d1ea49e..ed6ba06d 100755 --- a/panther_battery/src/roboteq_republisher_node.py +++ b/panther_battery/src/roboteq_republisher_node.py @@ -81,13 +81,12 @@ def _battery_pub_timer_cb(self, *args) -> None: battery_msg.voltage = self._battery_voltage battery_msg.temperature = float('nan') battery_msg.current = self._battery_current - battery_msg.percentage = (battery_msg.voltage - self.V_BAT_MIN) / ( - self.V_BAT_FULL - self.V_BAT_MIN + battery_msg.percentage = self._clamp( + (battery_msg.voltage - self.V_BAT_MIN) / (self.V_BAT_FULL - self.V_BAT_MIN) ) battery_msg.charge = battery_msg.percentage * battery_msg.design_capacity battery_msg.present = True - # TODO: check battery status battery_msg.power_supply_status = BatteryState.POWER_SUPPLY_STATUS_DISCHARGING # check battery health @@ -116,6 +115,9 @@ def _update_volt_mean(self, new_val: float) -> float: self._battery_voltage_hist.pop(0) self._battery_voltage_hist.append(new_val) + def _clamp(value, max_value, min_value): + return max(min(value, max_value), min_value) + def main(): roboteq_republisher_node = RoboteqRepublisherNode('roboteq_republisher_node') From 8986096dd1e91a427cf6a683b8d1dccdab5aebde Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Pawe=C5=82=20Kowalski?= Date: Wed, 31 May 2023 12:38:11 +0200 Subject: [PATCH 12/15] small fixes --- panther_battery/src/adc_node.py | 4 +++- panther_battery/src/roboteq_republisher_node.py | 4 +++- 2 files changed, 6 insertions(+), 2 deletions(-) diff --git a/panther_battery/src/adc_node.py b/panther_battery/src/adc_node.py index 97947dfc..0047ed5e 100755 --- a/panther_battery/src/adc_node.py +++ b/panther_battery/src/adc_node.py @@ -200,7 +200,9 @@ def _publish_battery_msg( battery_msg.temperature = temp_bat battery_msg.current = I_bat battery_msg.percentage = self._clamp( - (battery_msg.voltage - self.V_BAT_MIN) / (self.V_BAT_FULL - self.V_BAT_MIN) + (battery_msg.voltage - self.V_BAT_MIN) / (self.V_BAT_FULL - self.V_BAT_MIN), + 0.0, + 1.0, ) battery_msg.capacity = 20.0 battery_msg.design_capacity = 20.0 diff --git a/panther_battery/src/roboteq_republisher_node.py b/panther_battery/src/roboteq_republisher_node.py index ed6ba06d..054219d0 100755 --- a/panther_battery/src/roboteq_republisher_node.py +++ b/panther_battery/src/roboteq_republisher_node.py @@ -82,7 +82,9 @@ def _battery_pub_timer_cb(self, *args) -> None: battery_msg.temperature = float('nan') battery_msg.current = self._battery_current battery_msg.percentage = self._clamp( - (battery_msg.voltage - self.V_BAT_MIN) / (self.V_BAT_FULL - self.V_BAT_MIN) + (battery_msg.voltage - self.V_BAT_MIN) / (self.V_BAT_FULL - self.V_BAT_MIN), + 0.0, + 1.0, ) battery_msg.charge = battery_msg.percentage * battery_msg.design_capacity battery_msg.present = True From 2e740a95f1f1b83e9f3b2439bde995f97e6b485a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Pawe=C5=82=20Kowalski?= Date: Wed, 31 May 2023 12:39:18 +0200 Subject: [PATCH 13/15] fix typo --- panther_battery/src/adc_node.py | 2 +- panther_battery/src/roboteq_republisher_node.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/panther_battery/src/adc_node.py b/panther_battery/src/adc_node.py index 0047ed5e..ce6501ac 100755 --- a/panther_battery/src/adc_node.py +++ b/panther_battery/src/adc_node.py @@ -275,7 +275,7 @@ def _read_file(path: str) -> int: return int(data) - def _clamp(value, max_value, min_value): + def _clamp(value, min_value, max_value): return max(min(value, max_value), min_value) diff --git a/panther_battery/src/roboteq_republisher_node.py b/panther_battery/src/roboteq_republisher_node.py index 054219d0..77146bb1 100755 --- a/panther_battery/src/roboteq_republisher_node.py +++ b/panther_battery/src/roboteq_republisher_node.py @@ -117,7 +117,7 @@ def _update_volt_mean(self, new_val: float) -> float: self._battery_voltage_hist.pop(0) self._battery_voltage_hist.append(new_val) - def _clamp(value, max_value, min_value): + def _clamp(value, min_value, max_value): return max(min(value, max_value), min_value) From 186b02b1a1716d7a851b91d7431c5d9759abe747 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Pawe=C5=82=20Kowalski?= Date: Wed, 31 May 2023 13:00:11 +0200 Subject: [PATCH 14/15] add decorator --- panther_battery/src/adc_node.py | 1 + panther_battery/src/roboteq_republisher_node.py | 1 + 2 files changed, 2 insertions(+) diff --git a/panther_battery/src/adc_node.py b/panther_battery/src/adc_node.py index ce6501ac..81ea9504 100755 --- a/panther_battery/src/adc_node.py +++ b/panther_battery/src/adc_node.py @@ -275,6 +275,7 @@ def _read_file(path: str) -> int: return int(data) + @staticmethod def _clamp(value, min_value, max_value): return max(min(value, max_value), min_value) diff --git a/panther_battery/src/roboteq_republisher_node.py b/panther_battery/src/roboteq_republisher_node.py index 77146bb1..7131c8cb 100755 --- a/panther_battery/src/roboteq_republisher_node.py +++ b/panther_battery/src/roboteq_republisher_node.py @@ -117,6 +117,7 @@ def _update_volt_mean(self, new_val: float) -> float: self._battery_voltage_hist.pop(0) self._battery_voltage_hist.append(new_val) + @staticmethod def _clamp(value, min_value, max_value): return max(min(value, max_value), min_value) From d8adb981eac009231792a166b29a76407c761e27 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Pawe=C5=82=20Kowalski?= Date: Thu, 15 Jun 2023 14:00:09 +0200 Subject: [PATCH 15/15] review fixes --- panther_battery/src/adc_node.py | 50 +++++++++++++-------------------- 1 file changed, 20 insertions(+), 30 deletions(-) diff --git a/panther_battery/src/adc_node.py b/panther_battery/src/adc_node.py index 81ea9504..2ca4bd17 100755 --- a/panther_battery/src/adc_node.py +++ b/panther_battery/src/adc_node.py @@ -3,7 +3,7 @@ from collections import defaultdict import math from threading import Lock -from typing import Optional, Union +from typing import Dict, Optional, Union import rospy @@ -29,13 +29,10 @@ def __init__(self, name: str) -> None: self._I_driv: Optional[float] = None self._V_driv: Optional[float] = None - self._volt_mean_length = 10 - self._V_bat_hist = defaultdict(lambda: [37.0] * self._volt_mean_length) + self._mean_length = 10 + self._V_bat_hist = defaultdict(lambda: [37.0] * self._mean_length) self._V_bat_mean = defaultdict(lambda: 37.0) - self._V_driv_mean: Optional[float] = None - - self._curr_mean_length = 10 - self._I_bat_charge_hist = defaultdict(lambda: [0.0] * self._curr_mean_length) + self._I_bat_charge_hist = defaultdict(lambda: [0.0] * self._mean_length) self._I_bat_charge_mean = defaultdict(lambda: 0.0) self._A = 298.15 @@ -92,10 +89,7 @@ def _motor_controllers_state_cb(self, driver_state: DriverState) -> None: with self._lock: self._driver_battery_last_info_time = rospy.get_time() - driver_voltage = (driver_state.front.voltage + driver_state.rear.voltage) / 2.0 - self._V_driv = driver_voltage - self._V_driv_mean = self._count_volt_mean('V_driv', driver_voltage) - + self._V_driv = (driver_state.front.voltage + driver_state.rear.voltage) / 2.0 self._I_driv = driver_state.front.current + driver_state.rear.current def _io_state_cb(self, io_state: IOState) -> None: @@ -210,8 +204,10 @@ def _publish_battery_msg( battery_msg.power_supply_technology = BatteryState.POWER_SUPPLY_TECHNOLOGY_LIPO battery_msg.present = True - V_bat_mean = self._count_volt_mean(bat_pub, V_bat) - I_bat_mean = self._count_curr_mean(bat_pub, I_charge) + V_bat_mean = self._count_mean(bat_pub, V_bat, self._V_bat_mean, self._V_bat_hist) + I_bat_mean = self._count_mean( + bat_pub, I_charge, self._I_bat_charge_mean, self._I_bat_charge_hist + ) with self._lock: # check battery status @@ -246,27 +242,21 @@ def _publish_battery_msg( bat_pub.publish(battery_msg) - def _count_volt_mean(self, label: Union[rospy.Publisher, str], new_val: float) -> float: - # Updates the average by adding the newest and removing the oldest component of mean value, - # in order to avoid recalculating the entire sum every time. - self._V_bat_mean[label] += (new_val - self._V_bat_hist[label][0]) / self._volt_mean_length - - self._V_bat_hist[label].pop(0) - self._V_bat_hist[label].append(new_val) - - return self._V_bat_mean[label] - - def _count_curr_mean(self, label: rospy.Publisher, new_val: float) -> float: + def _count_mean( + self, + label: Union[rospy.Publisher, str], + new_val: float, + mean_dict: dict, + hist_dict: Dict[Union[rospy.Publisher, str], list], + ) -> float: # Updates the average by adding the newest and removing the oldest component of mean value, # in order to avoid recalculating the entire sum every time. - self._I_bat_charge_mean[label] += ( - new_val - self._I_bat_charge_hist[label][0] - ) / self._curr_mean_length + mean_dict[label] += (new_val - hist_dict[label][0]) / self._mean_length - self._I_bat_charge_hist[label].pop(0) - self._I_bat_charge_hist[label].append(new_val) + hist_dict[label].pop(0) + hist_dict[label].append(new_val) - return self._I_bat_charge_mean[label] + return mean_dict[label] @staticmethod def _read_file(path: str) -> int: