Merge pull request #280 from dronekit/fix_heartbeat_detection

Fix heartbeat detection

Author: m4gr3d

ServiceApp/src/org/droidplanner/services/android/core/MAVLink/MavLinkMsgHandler.java

@@ -32,13 +32,6 @@ public void receiveData(MAVLinkMessage msg) {
                 handleHeartbeat(msg_heart);
                 break;
 
-            /*
-            TODO: find more reliable method to determine the vehicle firmware.
-            case msg_statustext.MAVLINK_MSG_ID_STATUSTEXT:
-                msg_statustext msg_statustext = (msg_statustext) msg;
-                handleStatusText(msg_statustext.getText());
-                break;*/
-
             default:
                 break;
         }
@@ -77,16 +70,4 @@ private void handleHeartbeat(msg_heartbeat heartbeat) {
         }
 
     }
-
-    private void handleStatusText(String message) {
-        if (message.startsWith("ArduCopter") || message.startsWith("APM:Copter")) {
-            droneMgr.onVehicleTypeReceived(FirmwareType.ARDU_COPTER);
-        } else if (message.startsWith("ArduPlane") || message.startsWith("APM:Plane")) {
-            droneMgr.onVehicleTypeReceived(FirmwareType.ARDU_PLANE);
-        } else if (message.startsWith("Solo")) {
-            droneMgr.onVehicleTypeReceived(FirmwareType.ARDU_SOLO);
-        } else if (message.startsWith("ArduRover") || message.startsWith("APM:Rover")) {
-            droneMgr.onVehicleTypeReceived(FirmwareType.ARDU_ROVER);
-        }
-    }
 }

ServiceApp/src/org/droidplanner/services/android/core/drone/autopilot/apm/ArduPilot.java

@@ -194,7 +194,7 @@ public int getMavlinkVersion() {
         return heartbeat.getMavlinkVersion();
     }
 
-    protected void onHeartbeat(msg_heartbeat msg) {
+    protected void onHeartbeat(MAVLinkMessage msg) {
         heartbeat.onHeartbeat(msg);
     }
 
@@ -491,10 +491,11 @@ public void onMavLinkMessageReceived(MAVLinkMessage message) {
         if (compId != AUTOPILOT_COMPONENT_ID
                 && compId != ARTOO_COMPONENT_ID
                 && compId != TELEMETRY_RADIO_COMPONENT_ID) {
-//            Timber.d("Received unsupported component id: %d", compId);
             return;
         }
 
+        onHeartbeat(message);
+
         if (getParameters().processMessage(message)) {
             return;
         }
@@ -509,7 +510,6 @@ public void onMavLinkMessageReceived(MAVLinkMessage message) {
                 processState(msg_heart);
                 ApmModes newMode = ApmModes.getMode(msg_heart.custom_mode, getType());
                 getState().setMode(newMode);
-                onHeartbeat(msg_heart);
                 break;
 
             case msg_statustext.MAVLINK_MSG_ID_STATUSTEXT:

ServiceApp/src/org/droidplanner/services/android/core/drone/variables/HeartBeat.java

@@ -1,5 +1,6 @@
 package org.droidplanner.services.android.core.drone.variables;
 
+import com.MAVLink.Messages.MAVLinkMessage;
 import com.MAVLink.common.msg_heartbeat;
 
 import org.droidplanner.services.android.core.drone.DroneInterfaces.DroneEventsType;
@@ -60,25 +61,35 @@ public short getMavlinkVersion() {
         return mMavlinkVersion;
     }
 
-    public void onHeartbeat(msg_heartbeat msg) {
-        sysid = (byte) msg.sysid;
-        compid = (byte) msg.compid;
-        mMavlinkVersion = msg.mavlink_version;
+    public void onHeartbeat(MAVLinkMessage msg) {
+        msg_heartbeat heartBeatMsg = msg instanceof msg_heartbeat ? (msg_heartbeat) msg : null;
+        if(heartBeatMsg != null){
+            sysid = (byte) msg.sysid;
+            compid = (byte) msg.compid;
+            mMavlinkVersion = heartBeatMsg.mavlink_version;
+        }
 
         switch (heartbeatState) {
             case FIRST_HEARTBEAT:
-                notifyConnected();
-                Timber.d("Received first heartbeat.");
-                myDrone.notifyDroneEvent(DroneEventsType.HEARTBEAT_FIRST);
+                if(heartBeatMsg != null) {
+                    Timber.i("Received first heartbeat.");
+
+                    heartbeatState = HeartbeatState.NORMAL_HEARTBEAT;
+                    restartWatchdog(HEARTBEAT_NORMAL_TIMEOUT);
+
+                    myDrone.notifyDroneEvent(DroneEventsType.HEARTBEAT_FIRST);
+                }
                 break;
 
             case LOST_HEARTBEAT:
                 myDrone.notifyDroneEvent(DroneEventsType.HEARTBEAT_RESTORED);
+            // FALL THROUGH
+
+            default:
+                heartbeatState = HeartbeatState.NORMAL_HEARTBEAT;
+                restartWatchdog(HEARTBEAT_NORMAL_TIMEOUT);
                 break;
         }
-
-        heartbeatState = HeartbeatState.NORMAL_HEARTBEAT;
-        restartWatchdog(HEARTBEAT_NORMAL_TIMEOUT);
     }
 
     public boolean hasHeartbeat() {
@@ -108,10 +119,6 @@ public void onDroneEvent(DroneEventsType event, MavLinkDrone drone) {
         }
     }
 
-    private void notifyConnected() {
-        restartWatchdog(HEARTBEAT_NORMAL_TIMEOUT);
-    }
-
     private void notifyDisconnected() {
         watchdog.removeCallbacks(watchdogCallback);
         heartbeatState = HeartbeatState.FIRST_HEARTBEAT;
@@ -126,7 +133,7 @@ private void onHeartbeatTimeout() {
                 break;
 
             case FIRST_HEARTBEAT:
-                Timber.d("First heartbeat timeout.");
+                Timber.i("First heartbeat timeout.");
                 myDrone.notifyDroneEvent(DroneEventsType.CONNECTION_FAILED);
                 break;