From 942bb6bccfd13efd34caabc513194f00de666599 Mon Sep 17 00:00:00 2001 From: Jason Hu Date: Fri, 29 Jul 2022 19:12:52 +0200 Subject: [PATCH] read battery status and publish --- .../flight_controller_node/default.yaml | 1 + .../src/flight_controller_node.py | 21 +++++++++++++++++++ 2 files changed, 22 insertions(+) diff --git a/packages/flight_controller_driver/config/flight_controller_node/default.yaml b/packages/flight_controller_driver/config/flight_controller_node/default.yaml index 487326f5..094d2ab9 100644 --- a/packages/flight_controller_driver/config/flight_controller_node/default.yaml +++ b/packages/flight_controller_driver/config/flight_controller_node/default.yaml @@ -6,6 +6,7 @@ frequency: commands: 40 imu: 30 motors: 5 + battery: 1 heartbeats: altitude: true diff --git a/packages/flight_controller_driver/src/flight_controller_node.py b/packages/flight_controller_driver/src/flight_controller_node.py index d5acf099..0a0b107a 100755 --- a/packages/flight_controller_driver/src/flight_controller_node.py +++ b/packages/flight_controller_driver/src/flight_controller_node.py @@ -142,6 +142,7 @@ def __init__(self): # reminders self._motors_reminder = DTReminder(frequency=self._frequency["motors"]) self._imu_reminder = DTReminder(frequency=self._frequency["imu"]) + self._battery_reminder = DTReminder(frequency=self._frequency["battery"]) # store the command to send to the flight controller, initialize as disarmed self._command = self.rc_command(DroneMode.DISARMED) @@ -299,6 +300,21 @@ def _fly_commands_cb(self, msg): # compile command to be sent to the flight controller board via MultiWii self._command = [int(msg.roll), int(msg.pitch), int(msg.yaw), int(msg.throttle)] + def _read_battery_status(self): + try: + analog = self._board.getData(MultiWii.ANALOG) + voltage = analog['vbat'] / 10.0 # reading scale + assert voltage >= 0, f"Voltage reading {voltage} is invalid" + except Exception as e: + self.logwarn(f"Unable to get Battery data {e}, retry...") + raise FCError(f"Unable to get Battery data {e}, retry...") + + msg = BatteryState() + msg.header = Header(stamp=rospy.Time.now()) + msg.voltage = voltage + msg.present = True if voltage > 6.0 else False # ~5V: power from Pi | 7V to 12.6V: power from battery + return msg + def run(self): # noinspection PyBroadException try: @@ -326,6 +342,11 @@ def run(self): motor_msg = self._read_motor_pwm_signals() self._motor_pub.publish(motor_msg) + # read battery status + if self._battery_reminder.is_time(): + bat_msg = self._read_battery_status() + self._bat_pub.publish(bat_msg) + # update and send the flight commands to the board self._compute_flight_commands() self._send_flight_commands()