Merge pull request #652 from MikaelFerland/master

Make dronekit working with the PX4 Pro

Author: peterbarker

dronekit/__init__.py

@@ -1177,9 +1177,12 @@ def listener(self, name, m):
             self.notify_attribute_listeners('armed', self.armed, cache=True)
             self._autopilot_type = m.autopilot
             self._vehicle_type = m.type
-            if self._is_mode_available(m.custom_mode) == False:
+            if self._is_mode_available(m.custom_mode, m.base_mode) == False:
                 raise APIException("mode %s not available on mavlink definition" % m.custom_mode)
-            self._flightmode = self._mode_mapping_bynumber[m.custom_mode]
+            if self._autopilot_type == mavutil.mavlink.MAV_AUTOPILOT_PX4:
+                self._flightmode = mavutil.interpret_px4_mode(m.base_mode, m.custom_mode)
+            else:
+                self._flightmode = self._mode_mapping_bynumber[m.custom_mode]
             self.notify_attribute_listeners('mode', self.mode, cache=True)
             self._system_status = m.system_status
             self.notify_attribute_listeners('system_status', self.system_status, cache=True)
@@ -1525,9 +1528,12 @@ def _mode_mapping(self):
     def _mode_mapping_bynumber(self):
         return mavutil.mode_mapping_bynumber(self._vehicle_type)
 
-    def _is_mode_available(self, mode_code):
-        try:
-            return mode_code in self._mode_mapping_bynumber
+    def _is_mode_available(self, custommode_code, basemode_code=0):
+        try:                       
+            if self._autopilot_type == mavutil.mavlink.MAV_AUTOPILOT_PX4:                
+                mode = mavutil.interpret_px4_mode(basemode_code, custommode_code)
+                return mode in self._mode_mapping                
+            return custommode_code in self._mode_mapping_bynumber
         except:
             return False
 
@@ -1546,7 +1552,10 @@ def mode(self):
 
     @mode.setter
     def mode(self, v):
-        self._master.set_mode(self._mode_mapping[v.name])
+        if self._autopilot_type == mavutil.mavlink.MAV_AUTOPILOT_PX4: 
+            self._master.set_mode(v.name)
+        else:
+            self._master.set_mode(self._mode_mapping[v.name])
 
     @property
     def location(self):