From 302dcd94e59e0298f9da47397c19b931266f90bb Mon Sep 17 00:00:00 2001 From: Pawel Kowalski Date: Wed, 5 Apr 2023 18:21:44 +0200 Subject: [PATCH 01/10] 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 8339b6795..0be0d07d6 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 0f272982f..89c7db233 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/10] 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 b062451d7..7118bb3e4 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/10] 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 0be0d07d6..5326190e5 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/10] 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 5326190e5..164816a33 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/10] 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 164816a33..6058247df 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 89c7db233..181c3b3de 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/10] 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 6058247df..3c62afc19 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 181c3b3de..afc53c315 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/10] 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 3c62afc19..0d4fbe0c1 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/10] 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 0d4fbe0c1..a00788c9e 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/10] 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 afc53c315..5818f882c 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 0b9624d9c8589364e16e87cddac6a6660bd4e9f6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Pawe=C5=82=20Kowalski?= <82044322+pkowalsk1@users.noreply.github.com> Date: Thu, 20 Apr 2023 20:22:20 +0200 Subject: [PATCH 10/10] fix typo --- panther_battery/src/adc_node.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/panther_battery/src/adc_node.py b/panther_battery/src/adc_node.py index a00788c9e..9d5263aa2 100755 --- a/panther_battery/src/adc_node.py +++ b/panther_battery/src/adc_node.py @@ -75,9 +75,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