Ros1 check bat current

panther_battery/src/adc_node.py

@@ -1,7 +1,5 @@
 #!/usr/bin/python3
 
-from threading import Lock
-
 from collections import defaultdict
 import math
 from threading import Lock
@@ -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)
@@ -33,14 +34,18 @@ 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
         self._R0 = 10000.0
         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()
 
@@ -63,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.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
         # -------------------------------
@@ -122,22 +137,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:
@@ -181,36 +192,40 @@ 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)
+        I_bat_mean = self._count_curr_mean(bat_pub, I_charge)
 
-        # 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
@@ -223,11 +238,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
 
@@ -240,19 +250,35 @@ 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:
             data = file.read().rstrip()
 
         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')

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)
@@ -79,11 +81,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
@@ -105,11 +110,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')