From ecae17e7f30ed72888c1c685c920b928f9408307 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Pawe=C5=82=20Kowalski?= <82044322+pkowalsk1@users.noreply.github.com> Date: Mon, 19 Jun 2023 12:40:32 +0200 Subject: [PATCH] Ros1 check bat current (#118) * add health info * Update README.md * add logs * add voltage mean count * update republisher node * fix typo * Update panther_battery/src/adc_node.py Co-authored-by: Krzysztof Wojciechowski <49921081+Kotochleb@users.noreply.github.com> * Update adc_node.py * Update roboteq_republisher_node.py * small fixes * review changes * small fixes * fix typo * add decorator * review fixes --------- Co-authored-by: Krzysztof Wojciechowski <49921081+Kotochleb@users.noreply.github.com> --- panther_battery/src/adc_node.py | 96 +++++++++++-------- .../src/roboteq_republisher_node.py | 17 +++- 2 files changed, 70 insertions(+), 43 deletions(-) diff --git a/panther_battery/src/adc_node.py b/panther_battery/src/adc_node.py index 662c9596..e61e46b7 100755 --- a/panther_battery/src/adc_node.py +++ b/panther_battery/src/adc_node.py @@ -1,11 +1,9 @@ #!/usr/bin/python3 -from threading import Lock - from collections import defaultdict import math from threading import Lock -from typing import Optional, Union +from typing import Dict, Optional, Union import rospy @@ -15,9 +13,12 @@ class ADCNode: + BAT_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) @@ -28,10 +29,11 @@ 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._I_bat_charge_hist = defaultdict(lambda: [0.0] * self._mean_length) + self._I_bat_charge_mean = defaultdict(lambda: 0.0) self._A = 298.15 self._B = 3977.0 @@ -40,8 +42,8 @@ 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._charger_connected = False self._lock = Lock() @@ -65,6 +67,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.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.BAT_CHARGING_CURR_THRESH}) + # ------------------------------- # Timers # ------------------------------- @@ -79,10 +91,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: @@ -124,22 +133,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: @@ -183,36 +188,42 @@ 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 = self._clamp( + (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 battery_msg.charge = battery_msg.percentage * battery_msg.design_capacity 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) + 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 + ) - # check battery status 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 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 else: battery_msg.power_supply_status = BatteryState.POWER_SUPPLY_STATUS_DISCHARGING - # check battery health - with self._lock: + # check battery health error_msg = None if V_bat_mean < self.V_BAT_FATAL_MIN: battery_msg.power_supply_health = BatteryState.POWER_SUPPLY_HEALTH_DEAD @@ -225,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 @@ -238,15 +244,21 @@ def _publish_battery_msg( bat_pub.publish(battery_msg) - def _count_volt_mean(self, label: Union[rospy.Publisher, str], 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._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) + mean_dict[label] += (new_val - hist_dict[label][0]) / self._mean_length - return self._V_bat_mean[label] + hist_dict[label].pop(0) + hist_dict[label].append(new_val) + + return mean_dict[label] @staticmethod def _read_file(path: str) -> int: @@ -255,6 +267,10 @@ 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) + 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 4ddb4837..f7d43ec5 100755 --- a/panther_battery/src/roboteq_republisher_node.py +++ b/panther_battery/src/roboteq_republisher_node.py @@ -13,6 +13,8 @@ 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) @@ -83,11 +85,14 @@ 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 = self._clamp( + (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 - # TODO: check battery status battery_msg.power_supply_status = BatteryState.POWER_SUPPLY_STATUS_DISCHARGING # check battery health @@ -109,11 +114,17 @@ 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) + @staticmethod + def _clamp(value, min_value, max_value): + return max(min(value, max_value), min_value) + def main(): roboteq_republisher_node = RoboteqRepublisherNode('roboteq_republisher_node')