diff --git a/CMakeLists.txt b/CMakeLists.txt
index 6eaa4c2a1bbaee51fc5f33f0405550682c851ab9..c1f12c0204dc03857e9b5a02ab1d0e48d052144c 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -50,6 +50,8 @@ catkin_python_setup()
## Generate messages in the 'msg' folder
add_message_files(FILES
Depth.msg
+ BatteryStatus.msg
+ BatteryBusStatus.msg
)
## Generate services in the 'srv' folder
diff --git a/README.md b/README.md
index 51bfb7445c2b7046e3715f019be975d0621a0ce8..596df3e88e9e2f24b70ec09abe51382993126cea 100644
--- a/README.md
+++ b/README.md
@@ -143,16 +143,98 @@ temperature_units:
```
+<a id="inspired_energy_mux_driver"></a>
+### inspired_energy_mux_driver
+
+
+Publishes to `rostopic` matching this pattern:`/<InspiredEnergyMux_node_name>/depth`
+
+<a id="example-output-from-InspiredEnergyMuxnode"></a>
+```bash
+#### Example output from Inspired Energy Mux Node
+roslaunch inspired_energy_mux_driver.launch
+... logging to /home/b/.ros/log/4dc879e2-330b-11ee-b441-37a4fd214431/roslaunch-b-ThinkPad-P14s-Gen-2i-132066.log
+Checking log directory for disk usage. This may take a while.
+Press Ctrl-C to interrupt
+Done checking log file disk usage. Usage is <1GB.
+
+started roslaunch server http://b-ThinkPad-P14s-Gen-2i:33211/
+
+SUMMARY
+========
+
+PARAMETERS
+ * /rosdistro: noetic
+ * /rosversion: 1.16.0
+
+NODES
+ /
+ BatteryStatus (ros_circuitpython/inspired_energy_mux_driver.py)
+
+auto-starting new master
+process[master]: started with pid [132081]
+ROS_MASTER_URI=http://localhost:11311
+
+setting /run_id to 4dc879e2-330b-11ee-b441-37a4fd214431
+process[rosout-1]: started with pid [132098]
+started core service [/rosout]
+process[BatteryStatus-2]: started with pid [132101]
+[INFO] [1691183422.052341] [/BatteryStatus]: Inspired Energy Mux node started
+```
+
+<a id="all-defaults-1"></a>
+```bash
+##### All Defaults:
+[INFO] [1691183425.346616] [/BatteryStatus]: About to publish latest update:
+header:
+ seq: 0
+ stamp:
+ secs: 1691183424
+ nsecs: 49459457
+ frame_id: ''
+connected_batteries: 3
+bus:
+ - {'charge_remaining': 74, 'capacity_remaining': 4747, 'time_to_empty': 65535, 'battery_discharging': True, 'battery_fully_charged': False, 'voltage': 15.72, 'current': 0.0, 'temp': 25.250000000000057, 'serial_no': 42977}
+ - {'charge_remaining': 75, 'capacity_remaining': 4905, 'time_to_empty': 65535, 'battery_discharging': True, 'battery_fully_charged': False, 'voltage': 15.72, 'current': 0.0, 'temp': 25.150000000000034, 'serial_no': 42911}
+ - {'charge_remaining': 74, 'capacity_remaining': 4838, 'time_to_empty': 65535, 'battery_discharging': True, 'battery_fully_charged': False, 'voltage': 15.703, 'current': 0.0, 'temp': 24.950000000000045, 'serial_no': 43019}
+--
+
+[INFO] [1691183427.345272] [/BatteryStatus]: About to publish latest update:
+header:
+ seq: 0
+ stamp:
+ secs: 1691183426
+ nsecs: 48639535
+ frame_id: ''
+connected_batteries: 3
+bus:
+ - {'charge_remaining': 74, 'capacity_remaining': 4747, 'time_to_empty': 65535, 'battery_discharging': True, 'battery_fully_charged': False, 'voltage': 15.721, 'current': 0.0, 'temp': 25.350000000000023, 'serial_no': 42977}
+ - {'charge_remaining': 75, 'capacity_remaining': 4905, 'time_to_empty': 65535, 'battery_discharging': True, 'battery_fully_charged': False, 'voltage': 15.72, 'current': 0.0, 'temp': 25.150000000000034, 'serial_no': 42911}
+ - {'charge_remaining': 74, 'capacity_remaining': 4838, 'time_to_empty': 65535, 'battery_discharging': True, 'battery_fully_charged': False, 'voltage': 15.703, 'current': 0.0, 'temp': 24.950000000000045, 'serial_no': 43019}
+--
+```
+
+<a id="with-ros_param-formattedtrue-1"></a>
+##### with ros_param formatted="true":
+```bash
+```
+
+
<a id="keller4ld10b"></a>
### Keller4LD10B
Publishes to `rostopic` matching this pattern:`/<keller4LD10B_node_name>/depth`
-
<a id="example-output-from-keller4ld10b-node"></a>
#### Example output from Keller4LD10B Node
+```bash
+```
<a id="all-defaults-1"></a>
##### All Defaults:
+```bash
+```
<a id="with-ros_param-formattedtrue-1"></a>
-##### with ros_param formatted="true":
\ No newline at end of file
+##### with ros_param formatted="true":
+```bash
+```
\ No newline at end of file
diff --git a/launch/inspired_energy_mux_driver.launch b/launch/inspired_energy_mux_driver.launch
new file mode 100644
index 0000000000000000000000000000000000000000..d02dc9673f16dac6777485798709e9ce409ecc78
--- /dev/null
+++ b/launch/inspired_energy_mux_driver.launch
@@ -0,0 +1,8 @@
+<launch>
+ <env name="ROSCONSOLE_FORMAT" value="[${severity}] [${time: %x %r}] [${node}]: ${message}"/>
+ <node name="InspiredEnergyMuxNode" pkg="ros_circuitpython" type="inspired_energy_mux_driver.py" output="screen" >
+ <param name="formatted" type="bool" value="false" />
+ <param name="battery_bus_status_topic_name" value="battery_bus_status" type="str" />
+ <param name="apptick_hz" value="1.0" />
+ </node>
+</launch>
\ No newline at end of file
diff --git a/msg/BatteryBusStatus.msg b/msg/BatteryBusStatus.msg
new file mode 100644
index 0000000000000000000000000000000000000000..ca784595587e9bb27ce04c344e2f0c4a7834f35c
--- /dev/null
+++ b/msg/BatteryBusStatus.msg
@@ -0,0 +1,4 @@
+std_msgs/Header header
+
+uint8 total_connected_batteries
+ros_circuitpython/BatteryStatus[] bus
diff --git a/msg/BatteryStatus.msg b/msg/BatteryStatus.msg
new file mode 100644
index 0000000000000000000000000000000000000000..698c761ee767945dc3d853f57357d6c816e24096
--- /dev/null
+++ b/msg/BatteryStatus.msg
@@ -0,0 +1,11 @@
+std_msgs/Header header
+
+uint8 charge_remaining_percent
+float32 capacity_remaining_mAh
+float32 time_to_empty_mins
+bool battery_discharging
+bool battery_fully_charged
+float32 voltage_V
+float32 current_A
+float32 temp_C
+string serial_no
diff --git a/src/inspired_energy_mux_driver.py b/src/inspired_energy_mux_driver.py
new file mode 100755
index 0000000000000000000000000000000000000000..4494d417e4d5a8b3b3fce3d184488b9c16fdeb3c
--- /dev/null
+++ b/src/inspired_energy_mux_driver.py
@@ -0,0 +1,321 @@
+#!/usr/bin/env python3
+
+import os
+# must do before trying to talk to board
+os.environ['BLINKA_MCP2221'] = '1'
+
+import board
+import busio
+import adafruit_tca9548a
+import time
+import struct
+import datetime
+
+from adafruit_bus_device.i2c_device import I2CDevice
+
+# ROS related
+import rospy
+from std_msgs.msg import Header
+from ros_circuitpython.msg import BatteryBusStatus, BatteryStatus
+import logging
+
+class InspiredEnergyBatteryNH2054HD34(object):
+ """docstring for InspiredEnergyBatteryNH2054HD34"""
+ _BATTERY_ADDRESS = 0xb # 0x16 (0x16 >> 1 = 0xb)
+ _CHARGER_ADDRESS = 0x70 # 0x12 (0x12 >> 1 = 0x9)
+
+ MANUFACTURER_ACCESS_CMD = 0x00
+ REMAINING_CAPACITY_ALARM_CMD = 0x01
+ REMAINING_TIME_ALARM_CMD = 0x02
+ BATTERY_MODE_CMD = 0x03
+ AT_RATE_CMD = 0x04
+ AT_RATE_TIME_TO_FULL_CMD = 0x05
+ AT_RATE_TIME_TO_EMPTY_CMD = 0x06
+ AT_RATE_OK_CMD = 0x07
+ TEMPERATURE_CMD = 0x08
+ VOLTAGE_CMD = 0x09
+ CURRENT_CMD = 0x0a
+ AVERAGE_CURRENT_CMD = 0x0b
+ MAX_ERROR_CMD = 0x0c
+ RELATIVE_STATE_OF_CHARGE_CMD = 0x0d
+ ABSOLUTE_STATE_OF_CHARGE_CMD = 0x0e
+ REMAINING_CAPACITY_CMD = 0x0f
+ FULL_CHARGE_CAPACITY_CMD = 0x10
+ RUN_TIME_TO_EMPTY_CMD = 0x11
+ AVERAGE_TIME_TO_EMPTY_CMD = 0x12
+ AVERAGE_TIME_TO_FULL_CMD = 0x13
+ BATTERY_STATUS_CMD = 0x16
+ CYCLE_COUNT_CMD = 0x17
+ DESIGN_CAPACITY_CMD = 0x18
+ DESIGN_VOLTAGE_CMD = 0x19
+ SPECIFICATION_INFO_CMD = 0x1a
+ MANUFACTURE_DATE_CMD = 0x1b
+ SERIAL_NO_CMD = 0x1c
+ MANUFACTURER_NAME_CMD = 0x20
+ DEVICE_NAME_CMD = 0x21
+ DEVICE_CHEMISTRY_CMD = 0x22
+ MANUFACTURER_DATA_CMD = 0x23
+
+ def __init__(self, i2c):
+ super(InspiredEnergyBatteryNH2054HD34, self).__init__()
+ self.i2c = i2c
+ self.out_buf = None
+ self.in_buf = bytearray(2)
+ self._buf = bytearray(2)
+
+ self.init_battery_info()
+ self.update_status()
+
+ def init_battery_info(self):
+ self.serial_no = self.read_register(address=InspiredEnergyBatteryNH2054HD34.SERIAL_NO_CMD)
+ self.design_voltage = self.read_register(address=InspiredEnergyBatteryNH2054HD34.DESIGN_VOLTAGE_CMD) / 1000
+ self.design_capacity = self.read_register(address=InspiredEnergyBatteryNH2054HD34.DESIGN_CAPACITY_CMD)
+ self.battery_cycle_count = self.read_register(address=InspiredEnergyBatteryNH2054HD34.CYCLE_COUNT_CMD)
+
+ def update_status(self):
+ self.voltage_V = self.read_register(address=InspiredEnergyBatteryNH2054HD34.VOLTAGE_CMD) / 1000
+ self.current_A = self.read_register(address=InspiredEnergyBatteryNH2054HD34.CURRENT_CMD) / 1000
+ self.temp_C = (self.read_register(address=InspiredEnergyBatteryNH2054HD34.TEMPERATURE_CMD) * 0.1) - 273.15
+ self.charge_remaining_percent = self.read_register(address=InspiredEnergyBatteryNH2054HD34.RELATIVE_STATE_OF_CHARGE_CMD)
+ self.capacity_remaining_mAh = self.read_register(address=InspiredEnergyBatteryNH2054HD34.REMAINING_CAPACITY_CMD)
+ self.time_to_empty_mins = self.read_register(address=InspiredEnergyBatteryNH2054HD34.RUN_TIME_TO_EMPTY_CMD)
+ self.battery_status = self.read_register(address=InspiredEnergyBatteryNH2054HD34.BATTERY_STATUS_CMD, return_raw=True)
+ self.battery_discharging = bool(self.battery_status[0] >> 6 & 1)
+ self.battery_fully_charged = bool(self.battery_status[0] >> 5 & 1)
+ self.battery_fully_discharged = bool(self.battery_status[0] >> 4 & 1)
+ self.battery_mode = self.read_register(address=InspiredEnergyBatteryNH2054HD34.BATTERY_MODE_CMD, return_raw=True)
+
+ def print_info(self, update=False):
+ if update:
+ self.init_battery_info()
+
+ rospy.logdebug('{:.<30}: {}'.format('Serial No', self.serial_no))
+ rospy.logdebug('{:.<30}: {}'.format('Cycle count', self.battery_cycle_count), end='\n\n')
+
+ rospy.logdebug('{:.<30}: {} V'.format('Design voltage', self.design_voltage))
+ rospy.logdebug('{:.<30}: {} mAh'.format('Design capacity', self.design_capacity), end='\n\n')
+
+ def print_status(self, update=True):
+ if update:
+ self.update_status()
+
+ rospy.logdebug('{:.<30}: {} V'.format('Voltage', self.voltage_V))
+ rospy.logdebug('{:.<30}: {} A'.format('Current', self.current_A))
+ rospy.logdebug('{:.<30}: {} C'.format('Temp', self.temp_C), end='\n\n')
+
+ rospy.logdebug('{:.<30}: {} %'.format('Charge remaining', self.charge_remaining_percent))
+ rospy.logdebug('{:.<30}: {} mAh'.format('Capacity remaining', self.capacity_remaining_mAh))
+ rospy.logdebug('{:.<30}: {} mins'.format('Run time to empty', self.time_to_empty_mins), end='\n\n')
+
+ rospy.logdebug('{:.<30}: {}'.format('Battery discharging', self.battery_discharging))
+ rospy.logdebug('{:.<30}: {}'.format('Battery fully charged', self.battery_fully_charged))
+ rospy.logdebug('{:.<30}: {}'.format('Battery fully discharged', self.battery_fully_discharged), end='\n\n')
+
+ def status(self, update=True):
+ if update:
+ self.update_status()
+ return {
+ 'charge_remaining_percent': self.charge_remaining_percent,
+ 'capacity_remaining_mAh': self.capacity_remaining_mAh,
+ 'time_to_empty_mins': self.time_to_empty_mins,
+ 'battery_discharging': self.battery_discharging,
+ 'battery_fully_charged': self.battery_fully_charged,
+ 'voltage_V': self.voltage_V,
+ 'current_A': self.current_A,
+ 'temp_C': self.temp_C,
+ 'serial_no': self.serial_no
+ }
+
+ @property
+ def address(self):
+ return hex(self._BATTERY_ADDRESS), hex(self._CHARGER_ADDRESS)
+
+ def reset_buf(self):
+ self.out_buf = None
+ self.in_buf = self._buf
+
+ def to_uint16(self, buf):
+ return struct.unpack('I', buf)
+
+ def to_uint32(self, buf):
+ return struct.unpack('Q', buf)
+
+ def pre_read_config(self, address):
+ self.reset_buf()
+ self.out_buf = bytes([address])
+
+ def read_register(self, address, return_raw=False):
+ self.pre_read_config(address)
+ device = I2CDevice(self.i2c, self._BATTERY_ADDRESS)
+ with device:
+ device.write_then_readinto(
+ self.out_buf,
+ self.in_buf,
+ )
+
+ if return_raw:
+ return self.in_buf
+ return struct.unpack('H', self.in_buf)[0]
+
+ def print_addresses(self):
+ self.i2c.try_lock()
+ rospy.logdebug([hex(a) for a in self.i2c.scan() if a != 0x70])
+ self.i2c.unlock()
+
+class InspiredEnergyBatterySystem(object):
+ """docstring for InspiredEnergyBatterySystem"""
+ def __init__(self, mux_i2c):
+ super(InspiredEnergyBatterySystem, self).__init__()
+ self.mux_i2c = mux_i2c
+ self.mux = adafruit_tca9548a.PCA9546A(self.mux_i2c)
+ self.total_connected_batteries = 0
+ self.batteries = [None, None, None, None]
+
+ self.init_channels()
+
+ def init_channels(self):
+ for channel in range(4):
+ if self.mux[channel].try_lock():
+ addresses = self.mux[channel].scan()
+
+ if InspiredEnergyBatteryNH2054HD34._BATTERY_ADDRESS in addresses and\
+ InspiredEnergyBatteryNH2054HD34._CHARGER_ADDRESS in addresses:
+ # NH2054HD34 connected on this channel
+ self.batteries[channel] = True
+ self.total_connected_batteries += 1
+
+ self.mux[channel].unlock()
+
+ for channel in range(4):
+ if self.batteries[channel]:
+ self.batteries[channel] = InspiredEnergyBatteryNH2054HD34(i2c=self.mux[channel])
+
+ def get_status(self, battery_index=-1, update=True):
+ # battery_index = -1, return list of (dicts) select set of info for all batteries
+ # battery_index >= 0 <= 3 returns dict with select set of info for requested battery
+ if battery_index != -1 and self.batteries[battery_index]:
+ status = self.batteries[battery_index].status(update=update)
+ else:
+ status = []
+ for batt in self.batteries:
+ if batt:
+ status.append(batt.status(update=update))
+ return status
+
+
+ def update_all(self, update_info=False):
+ for i, batt in enumerate(self.batteries):
+ if batt:
+ batt.update_status()
+ if update_info:
+ batt.init_battery_info()
+
+ def print_status(self, include_info=True, update=True):
+ for i, batt in enumerate(self.batteries):
+ if batt:
+ rospy.logdebug('-' * 45)
+ rospy.logdebug('Battery {:<0}'.format(i))
+ if include_info:
+ batt.print_info()
+ batt.print_status(update=update)
+ time.sleep(0.2)
+
+
+class InspiredEnergyMuxNode(object):
+ """docstring for InspiredEnergyMuxNode"""
+ def __init__(self, debug=False):
+ super(InspiredEnergyMuxNode, self).__init__()
+ rospy.init_node('InspiredEnergyMux')
+ self.debug = rospy.get_param('~formatted', debug)
+
+ #self.unified_log = UnifiedLog(log_path='./', console_log_level=logging.INFO, rootname='acomms_logger')
+ self.rate = rospy.Rate(float(rospy.get_param('~apptick_hz', 1)))
+
+ self.battery_bus_status_topic_name = rospy.get_param('~battery_bus_status_topic_name', 'battery_bus_status')
+ self.battery_bus_status_publisher = rospy.Publisher(self.battery_bus_status_topic_name, BatteryBusStatus, latch=True, queue_size=10)
+
+ # init hardware
+
+ self.system = self.init_system()
+
+ self.tca9548a = adafruit_tca9548a.I2C(board.SCL, board.SDA)
+
+ """Helper function to initialize i2c bus and return an IE batt system with the bus"""
+ def init_system(self):
+ i2c = busio.I2C(board.SCL, board.SDA)
+ return InspiredEnergyBatterySystem(mux_i2c=i2c)
+
+ def run(self):
+ while not rospy.is_shutdown():
+ # pause between readings
+ self.rate.sleep()
+
+ try:
+ header = Header(stamp=rospy.Time.now())
+ total_connected_batteries = self.system.total_connected_batteries
+
+ # get_status() will return a list of dictionaries when called with no args
+ # each dict represents the state of a single battery on the bus
+ bus_info_list = self.system.get_status()
+
+ bus = []
+ for bus_info in bus_info_list:
+ bus_status_msg = BatteryStatus(
+ charge_remaining_percent=int(bus_info['charge_remaining_percent']),
+ capacity_remaining_mAh=float(bus_info['capacity_remaining_mAh']),
+ time_to_empty_mins=float(bus_info['time_to_empty_mins']),
+ battery_discharging=bool(bus_info['battery_discharging']),
+ battery_fully_charged=bool(bus_info['battery_fully_charged']),
+ voltage_V=float(bus_info['voltage_V']),
+ current_A=float(bus_info['current_A']),
+ temp_C=float(bus_info['temp_C']),
+ serial_no=str(bus_info['serial_no']),
+ )
+ bus.append(bus_status_msg)
+
+ # create ros msg
+ msg = BatteryBusStatus(
+ header=header,
+ total_connected_batteries=total_connected_batteries,
+ bus=bus,
+ )
+
+ if self.debug:
+ this_t_step = datetime.datetime.fromtimestamp(header.stamp.secs)
+ rospy.loginfo(f'------------------------------------------------')
+ rospy.loginfo(f'Measurement taken at:...... {this_t_step}')
+ rospy.loginfo(f'Total Connected Batteries:. {total_connected_batteries}')
+ for i, batt in enumerate(bus):
+ rospy.loginfo(f'Battery [{i}]:\n{batt}')
+ else:
+ rospy.loginfo(f'About to publish latest update:\n{msg}\n--\n')
+
+ # publish message
+ self.battery_bus_status_publisher.publish(msg)
+
+ except RuntimeError as e:
+ if "Unrecoverable I2C state failure" in str(e):
+ rospy.logwarn("Caught an I2C state failure!")
+ elif "I2C slave address was NACK'd" in str(e):
+ rospy.logwarn("Caught an I2C slave address NACK!")
+ else:
+ rospy.logwarn(f"An unhandled RuntimeError occured: {e}")
+ self.system = self.init_system()
+
+ except Exception as exception:
+ rospy.logerr(f"An unhandled exception occured: {exception}")
+ raise Exception("An unhanded exception occured") from exception
+
+if __name__ == '__main__':
+ rospy.loginfo("Inspired Energy Mux Node entered set up")
+ try:
+ node = InspiredEnergyMuxNode()
+ rospy.loginfo("Inspired Energy Mux node started")
+ node.run()
+ rospy.loginfo("Inspired Energy Mux node shutdown")
+
+ except rospy.ROSInterruptException:
+ node.close()
+ rospy.loginfo("Inspired Energy Mux node (interrupt)")
+ finally:
+ rospy.loginfo("Inspired Energy Mux Exiting...")