Skip to content

Commit

Permalink
add battery health info (#102)
Browse files Browse the repository at this point in the history
* 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

* fix typo

---------

Co-authored-by: Dawid Kmak <73443304+KmakD@users.noreply.github.com>
Co-authored-by: Krzysztof Wojciechowski <49921081+Kotochleb@users.noreply.github.com>
  • Loading branch information
3 people authored Apr 21, 2023
1 parent a521b24 commit 7724a64
Show file tree
Hide file tree
Showing 3 changed files with 107 additions and 14 deletions.
4 changes: 4 additions & 0 deletions panther_battery/README.md
Original file line number Diff line number Diff line change
Expand Up @@ -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.
70 changes: 62 additions & 8 deletions panther_battery/src/adc_node.py
Original file line number Diff line number Diff line change
@@ -1,6 +1,9 @@
#!/usr/bin/python3

from collections import defaultdict
import math
from threading import Lock
from typing import Optional, Union

import rospy

Expand All @@ -11,14 +14,22 @@

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)

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: 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
Expand All @@ -29,6 +40,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
# -------------------------------
Expand Down Expand Up @@ -59,10 +72,14 @@ 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()

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

def _io_state_cb(self, io_state: IOState) -> None:
self._charger_connected = io_state.charger_connected
Expand Down Expand Up @@ -175,6 +192,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:
Expand All @@ -186,8 +205,43 @@ def _publish_battery_msg(
else:
battery_msg.power_supply_status = BatteryState.POWER_SUPPLY_STATUS_DISCHARGING

# check battery health
with self._lock:
error_msg = None
if V_bat_mean < self.V_BAT_FATAL_MIN:
battery_msg.power_supply_health = BatteryState.POWER_SUPPLY_HEALTH_DEAD
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
error_msg = 'Battery overvoltage!'
elif temp_bat >= self._high_bat_temp:
battery_msg.power_supply_health = BatteryState.POWER_SUPPLY_HEALTH_OVERHEAT
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

if error_msg is not None:
rospy.logerr_throttle_identical(10.0, f'[{rospy.get_name()}] {error_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]

@staticmethod
def _read_file(path: str) -> int:
with open(path, 'r') as file:
Expand Down
47 changes: 41 additions & 6 deletions panther_battery/src/roboteq_republisher_node.py
Original file line number Diff line number Diff line change
@@ -1,6 +1,7 @@
#!/usr/bin/python3

from threading import Lock
from typing import Optional

import rospy

Expand All @@ -10,11 +11,19 @@


class RoboteqRepublisherNode:
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()
Expand Down Expand Up @@ -44,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:
Expand All @@ -55,25 +67,48 @@ 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
or rospy.get_time() - self._last_battery_info_time > self._battery_timeout
):
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
error_msg = None
if self._battery_voltage < self.V_BAT_FATAL_MIN:
battery_msg.power_supply_health = BatteryState.POWER_SUPPLY_HEALTH_DEAD
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
error_msg = 'Battery overvoltage!'
else:
battery_msg.power_supply_health = BatteryState.POWER_SUPPLY_HEALTH_GOOD

if error_msg is not None:
rospy.logerr_throttle_identical(10.0, f'[{rospy.get_name()}] {error_msg}')

self._battery_pub.publish(battery_msg)

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_hist.pop(0)
self._battery_voltage_hist.append(new_val)


def main():
roboteq_republisher_node = RoboteqRepublisherNode('roboteq_republisher_node')
Expand Down

0 comments on commit 7724a64

Please sign in to comment.