diff --git a/clearpath_diagnostics/clearpath_diagnostics/battery_state/battery.py b/clearpath_diagnostics/clearpath_diagnostics/battery_state/battery.py index d98dbdf..5d8e7c1 100644 --- a/clearpath_diagnostics/clearpath_diagnostics/battery_state/battery.py +++ b/clearpath_diagnostics/clearpath_diagnostics/battery_state/battery.py @@ -94,6 +94,13 @@ def __init__( case Platform.W200: self.power_msg_voltage_index = Power.W200_MEASURED_BATTERY self.power_msg_current_index = Power.W200_TOTAL_CURRENT + case Platform.DD100 | Platform.DO100: + self.power_msg_voltage_index = Power.D100_MEASURED_BATTERY + self.power_msg_current_index = Power.D100_TOTAL_CURRENT + case Platform.DD150 | Platform.DO150: + self.power_msg_voltage_index = Power.D150_MEASURED_BATTERY + self.power_msg_current_index = Power.D150_TOTAL_CURRENT + # System capacity self._msg.capacity = self._msg.design_capacity = self.system_capacity @@ -142,6 +149,11 @@ def update(self, power_msg: Power): self._msg.current = 0.0 for i in self.power_msg_current_index: self._msg.current += power_msg.measured_currents[i] + # Set charging status + if power_msg.charger_connected == 1: + self._msg.power_supply_status = BatteryState.POWER_SUPPLY_STATUS_CHARGING + else: + self._msg.power_supply_status = BatteryState.POWER_SUPPLY_STATUS_DISCHARGING # Cells self.update_cells() @@ -168,10 +180,6 @@ def update_from_lut(self): self._msg.percentage = self.linear_interpolation(self.LUT, avg_voltage) self._msg.charge = self._msg.capacity * self._msg.percentage - # Power supply status - if self._msg.percentage == 1.0: - self._msg.power_supply_status = BatteryState.POWER_SUPPLY_STATUS_FULL - def update_cells(self): self._msg.cell_voltage = [self._msg.voltage / self.series] * self.cell_count self._msg.cell_temperature = [nan] * self.cell_count @@ -257,11 +265,35 @@ class U1_35(SLA): CAPACITY = 35.0 VOLTAGE = 12.0 + class TLV1222(SLA): + CAPACITY = 22.0 + VOLTAGE = 12.0 + + class RB20(LiION): + CAPACITY = 20.0 + VOLTAGE = 12.8 + LUT = [ + [10.5, 0.0], + [12.0, 0.05], + [13.0, 0.1], + [13.25, 0.2], + [13.3, 0.3], + [13.4, 0.4], + [13.45, 0.5], + [13.5, 0.6], + [13.6, 0.7], + [13.7, 0.8], + [13.8, 0.9], + [14.5, 1.0], + ] + # Match battery name to class BATTERIES = { BatteryConfig.HE2613: HE2613, BatteryConfig.ES20_12C: ES20_12C, - BatteryConfig.U1_35: U1_35 + BatteryConfig.U1_35: U1_35, + BatteryConfig.TLV1222: TLV1222, + BatteryConfig.RB20: RB20 } def __new__(cls, diff --git a/clearpath_diagnostics/clearpath_diagnostics/battery_state/battery_state_control b/clearpath_diagnostics/clearpath_diagnostics/battery_state/battery_state_control index 7c51651..a4c0b33 100755 --- a/clearpath_diagnostics/clearpath_diagnostics/battery_state/battery_state_control +++ b/clearpath_diagnostics/clearpath_diagnostics/battery_state/battery_state_control @@ -78,7 +78,7 @@ class BatteryStateControl(Node): qos_profile_sensor_data) match self.platform: - case Platform.J100: + case Platform.J100 | Platform.DD100 | Platform.DO100 | Platform.DD150 | Platform.DO150: self.hmi_battery_pub = self.create_publisher( UInt8, 'platform/mcu/_hmi', @@ -114,7 +114,7 @@ class BatteryStateControl(Node): def battery_state_callback(self, msg: BatteryState): match self.platform: - case Platform.J100: + case Platform.J100 | Platform.DD100 | Platform.DO100 | Platform.DD150 | Platform.DO150: self.hmi_control(msg.percentage) diff --git a/clearpath_generator_robot/clearpath_generator_robot/launch/generator.py b/clearpath_generator_robot/clearpath_generator_robot/launch/generator.py index 3957221..1e946c2 100644 --- a/clearpath_generator_robot/clearpath_generator_robot/launch/generator.py +++ b/clearpath_generator_robot/clearpath_generator_robot/launch/generator.py @@ -79,8 +79,8 @@ def __init__(self, setup_path: str = '/etc/clearpath/') -> None: ] ) - # W200 MicroROS Agent - self.w200_uros_node = LaunchFile.Node( + # Ethernet MicroROS Agent + self.eth_uros_node = LaunchFile.Node( name='micro_ros_agent', package='micro_ros_agent', executable='micro_ros_agent', @@ -205,11 +205,39 @@ def __init__(self, setup_path: str = '/etc/clearpath/') -> None: Platform.W200: common_platform_components + [ self.imu_0_filter_node, self.imu_0_filter_config, - self.w200_uros_node, + self.eth_uros_node, self.configure_mcu, self.lighting_node, self.sevcon_node - ] + ], + Platform.DD100: common_platform_components + [ + self.imu_0_filter_node, + self.imu_0_filter_config, + self.eth_uros_node, + self.configure_mcu, + self.lighting_node, + ], + Platform.DO100: common_platform_components + [ + self.imu_0_filter_node, + self.imu_0_filter_config, + self.eth_uros_node, + self.configure_mcu, + self.lighting_node, + ], + Platform.DD150: common_platform_components + [ + self.imu_0_filter_node, + self.imu_0_filter_config, + self.eth_uros_node, + self.configure_mcu, + self.lighting_node, + ], + Platform.DO150: common_platform_components + [ + self.imu_0_filter_node, + self.imu_0_filter_config, + self.eth_uros_node, + self.configure_mcu, + self.lighting_node, + ], } def generate_sensors(self) -> None: