read battery status and publish

duckietown · Jul 29, 2022 · 942bb6b · 942bb6b
1 parent e0d988a
commit 942bb6b
Show file tree

Hide file tree

Showing 2 changed files with 22 additions and 0 deletions.
diff --git 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
@@ -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()