Merge pull request #463 from billbonney/fix-sik-radio-msg

Fixes #462: Allow SiK Radio Messages to pass through to vehicle.
dronekit · Nov 19, 2016 · 8fcbdd6 · 8fcbdd6
2 parents 6107fdd + 5279a5d
commit 8fcbdd6
Show file tree

Hide file tree

Showing 2 changed files with 20 additions and 5 deletions.
diff --git a/.../main/java/org/droidplanner/services/android/impl/core/drone/autopilot/apm/ArduPilot.java b/.../main/java/org/droidplanner/services/android/impl/core/drone/autopilot/apm/ArduPilot.java
@@ -74,7 +74,6 @@
 public abstract class ArduPilot extends GenericMavLinkDrone {
     public static final int AUTOPILOT_COMPONENT_ID = 1;
     public static final int ARTOO_COMPONENT_ID = 0;
-    public static final int TELEMETRY_RADIO_COMPONENT_ID = 68;
 
     public static final String FIRMWARE_VERSION_NUMBER_REGEX = "\\d+(\\.\\d{1,2})?";
 
@@ -386,15 +385,16 @@ protected boolean performTakeoff(Bundle data, ICommandListener listener) {
     @Override
     public void onMavLinkMessageReceived(MAVLinkMessage message) {
 
-        if (message.sysid != this.getSysid()) {
+        if ((message.sysid != this.getSysid()) && !isMavLinkMessageException(message)) {
             // Reject Messages that are not for the system id
             return;
         }
 
+        // Filter Components IDs to be specifically the IDs that can be processed
         int compId = message.compid;
         if (compId != AUTOPILOT_COMPONENT_ID
                 && compId != ARTOO_COMPONENT_ID
-                && compId != TELEMETRY_RADIO_COMPONENT_ID) {
+                && compId != SiK_RADIO_FIXED_COMPID ){
             return;
         }
 

diff --git a/.../droidplanner/services/android/impl/core/drone/autopilot/generic/GenericMavLinkDrone.java b/.../droidplanner/services/android/impl/core/drone/autopilot/generic/GenericMavLinkDrone.java
@@ -83,6 +83,11 @@
  */
 public class GenericMavLinkDrone implements MavLinkDrone {
 
+    /** Fixed SYSTEM_ID - {@value}, for SiK Telemetry Radio (aka 3DR Telemetry Radio) .*/
+    public static final int SiK_RADIO_FIXED_SYSID = 0x33;  // '3' 0x33 51
+    /** Fixed COMPONENT_ID - {@value}, for SiK Telemetry Radio (aka 3DR Telemetry Radio) .*/
+    public static final int SiK_RADIO_FIXED_COMPID = 0x44; // 'D' 0x44 68
+
     private final DataLink.DataLinkProvider<MAVLinkMessage> mavClient;
 
     protected final VideoManager videoMgr;
@@ -572,11 +577,21 @@ private void onHeartbeat(MAVLinkMessage msg) {
         heartbeat.onHeartbeat(msg);
     }
 
+    // Check if message should be allowed to pass even if sysid/compid mismatch
+    protected boolean isMavLinkMessageException(MAVLinkMessage message){
+
+        // Allows SiK Radio Messages through. // TODO Make it a configurable setting
+        if (message.sysid == SiK_RADIO_FIXED_SYSID && message.compid == SiK_RADIO_FIXED_COMPID) {
+            return true;
+        }
+        return false;
+    }
+
     @Override
     public void onMavLinkMessageReceived(MAVLinkMessage message) {
 
-        if (message.sysid != this.getSysid()) {
-            // Reject Messages that are not for the system id
+        if ( (message.sysid != this.getSysid()) && !isMavLinkMessageException(message) ) {
+            // Reject messages that are not for this drone's system id
             return;
         }