raw servo output & waypoint callback

dronekit/__init__.py

@@ -1120,6 +1120,45 @@ def set_rc(chnum, v):
             set_rc(8, m.chan8_raw)
             self.notify_attribute_listeners('channels', self.channels)
 
+        # Servo outputs
+        self._servo_output_raw = ServoOutputRaw()
+
+        # Create a message listener using the decorator.
+        @self.on_message('SERVO_OUTPUT_RAW')
+        def listener(self, name, message):
+            """
+            The listener is called for messages that contain the string specified in the decorator,
+            passing the vehicle, message name, and the message.
+
+            The listener writes the message to the (newly attached) ``vehicle.raw_imu`` object
+            and notifies observers.
+            """
+            self._servo_output_raw.time_boot_us = message.time_usec
+            self._servo_output_raw.port = message.port
+            self._servo_output_raw.servo1_raw = message.servo1_raw
+            self._servo_output_raw.servo2_raw = message.servo2_raw
+            self._servo_output_raw.servo3_raw = message.servo3_raw
+            self._servo_output_raw.servo4_raw = message.servo4_raw
+            self._servo_output_raw.servo5_raw = message.servo5_raw
+            self._servo_output_raw.servo6_raw = message.servo6_raw
+            self._servo_output_raw.servo7_raw = message.servo7_raw
+            self._servo_output_raw.servo8_raw = message.servo8_raw
+
+            if hasattr(message,'servo9_raw'): #Some ArduPilot implementations only send 8 channels over mavlink
+                self._servo_output_raw.servo9_raw = message.servo9_raw
+                self._servo_output_raw.servo10_raw = message.servo10_raw
+                self._servo_output_raw.servo11_raw = message.servo11_raw
+                self._servo_output_raw.servo12_raw = message.servo12_raw
+                self._servo_output_raw.servo13_raw = message.servo13_raw
+                self._servo_output_raw.servo14_raw = message.servo14_raw
+                self._servo_output_raw.servo15_raw = message.servo15_raw
+                self._servo_output_raw.servo16_raw = message.servo16_raw
+
+            # Notify all observers of new message (with new value)
+            # Note that argument `cache=False` by default so listeners
+            # are updated with every new message
+            self.notify_attribute_listeners('servo_output_raw', self._servo_output_raw)
+
         self._voltage = None
         self._current = None
         self._level = None
@@ -1148,7 +1187,10 @@ def listener(self, name, m):
 
         @self.on_message(['WAYPOINT_CURRENT', 'MISSION_CURRENT'])
         def listener(self, name, m):
-            self._current_waypoint = m.seq
+            if m.seq != self._current_waypoint:
+                self._current_waypoint = m.seq
+                self.notify_attribute_listeners('commands.next',self._current_waypoint)
+
 
         self._ekf_poshorizabs = False
         self._ekf_constposmode = False
@@ -1521,6 +1563,10 @@ def flush(self):
     # Private sugar methods
     #
 
+    @property
+    def servo_output_raw(self):
+        return self._servo_output_raw
+
     @property
     def _mode_mapping(self):
         return self._master.mode_mapping()
@@ -2748,6 +2794,65 @@ def __setitem__(self, index, value):
         self._vehicle._wpts_dirty = True
 
 
+class ServoOutputRaw(object):
+    """
+    The RAW servo readings from the OUTPUT of the autopilot
+    This contains the true raw values without any scaling to allow data capture and system debugging.
+
+    The message definition is here: https://pixhawk.ethz.ch/mavlink/#SERVO_OUTPUT_RAW
+
+
+    """
+
+    def __init__(self,
+                 time_boot_us=None, # Timestamp (microseconds since system boot)
+                 port=None,         # Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows to encode more than 8 servos.
+                 servo1_raw=None,   # Servo output 1 value, in microseconds
+                 servo2_raw=None,   # Etc..
+                 servo3_raw=None,
+                 servo4_raw=None,
+                 servo5_raw=None,
+                 servo6_raw=None,
+                 servo7_raw=None,
+                 servo8_raw=None,
+                 servo9_raw=None,
+                 servo10_raw=None,
+                 servo11_raw=None,
+                 servo12_raw=None,
+                 servo13_raw=None,
+                 servo14_raw=None,
+                 servo15_raw=None,
+                 servo16_raw=None):
+
+        self.time_boot_us = time_boot_us
+        self.port = port
+        self.servo1_raw = servo1_raw
+        self.servo2_raw = servo2_raw
+        self.servo3_raw = servo3_raw
+        self.servo4_raw = servo4_raw
+        self.servo5_raw = servo5_raw
+        self.servo6_raw = servo6_raw
+        self.servo7_raw = servo7_raw
+        self.servo8_raw = servo8_raw
+        self.servo9_raw = servo9_raw
+        self.servo10_raw = servo10_raw
+        self.servo11_raw = servo11_raw
+        self.servo12_raw = servo12_raw
+        self.servo13_raw = servo13_raw
+        self.servo14_raw = servo14_raw
+        self.servo15_raw = servo15_raw
+        self.servo16_raw = servo16_raw
+
+    def __str__(self):
+        """
+        String representation used to print the ServoOutputRaw object.
+        """
+        return "SERVO_OUTPUT_RAW: time_boot_us={},port={},servo1_raw={},servo2_raw={},servo3_raw={},servo4_raw={},servo5_raw={},servo6_raw={},servo7_raw={},servo8_raw={},servo9_raw={},servo10_raw={},servo11_raw={},servo12_raw={},servo13_raw={},servo14_raw={},servo15_raw={},servo16_raw={}".format(
+            self.time_boot_us, self.port, self.servo1_raw, self.servo2_raw, self.servo3_raw, self.servo4_raw,
+            self.servo5_raw, self.servo6_raw, self.servo7_raw, self.servo8_raw, self.servo9_raw, self.servo10_raw,
+            self.servo11_raw, self.servo12_raw, self.servo13_raw, self.servo14_raw, self.servo15_raw, self.servo16_raw)
+
+
 from dronekit.mavlink import MAVConnection