From 73dfa16a46b5bf1a79e87719b9ce8733406b358d Mon Sep 17 00:00:00 2001
From: anbello <anbello@gmail.com>
Date: Wed, 24 Oct 2018 10:45:45 +0200
Subject: [PATCH] Use GLOBAL_POSITION_INT to set altitude and speeds in
 ArduPilot

---
 .../core/drone/autopilot/apm/ArduPilot.java   | 23 ++++++++++++++--
 .../core/drone/autopilot/apm/ArduPlane.java   | 26 -------------------
 2 files changed, 21 insertions(+), 28 deletions(-)

diff --git a/ClientLib/src/main/java/org/droidplanner/services/android/impl/core/drone/autopilot/apm/ArduPilot.java b/ClientLib/src/main/java/org/droidplanner/services/android/impl/core/drone/autopilot/apm/ArduPilot.java
index 4a52913290..1bba5fbcbe 100644
--- a/ClientLib/src/main/java/org/droidplanner/services/android/impl/core/drone/autopilot/apm/ArduPilot.java
+++ b/ClientLib/src/main/java/org/droidplanner/services/android/impl/core/drone/autopilot/apm/ArduPilot.java
@@ -13,6 +13,7 @@
 import com.MAVLink.ardupilotmega.msg_mount_configure;
 import com.MAVLink.ardupilotmega.msg_mount_status;
 import com.MAVLink.ardupilotmega.msg_radio;
+import com.MAVLink.common.msg_global_position_int;
 import com.MAVLink.common.msg_named_value_int;
 import com.MAVLink.common.msg_raw_imu;
 import com.MAVLink.common.msg_rc_channels_raw;
@@ -512,10 +513,28 @@ private void checkControlSensorsHealth(msg_sys_status sysStatus) {
     }
 
     protected void processVfrHud(msg_vfr_hud vfrHud) {
-        if (vfrHud == null)
+        //Nothing to do. Copter now use GLOBAL_POSITION_INT to set altitude and speeds
+    }
+
+    /**
+     * Used to update the vehicle location, and altitude.
+     * @param gpi
+     */
+    @Override
+    protected void processGlobalPositionInt(msg_global_position_int gpi) {
+        if(gpi == null)
             return;
 
-        setAltitudeGroundAndAirSpeeds(vfrHud.alt, vfrHud.groundspeed, vfrHud.airspeed, vfrHud.climb);
+        super.processGlobalPositionInt(gpi);
+
+        final double relativeAlt = gpi.relative_alt / 1000.0;
+
+        final double groundSpeedX = gpi.vx / 100.0;
+        final double groundSpeedY = gpi.vy / 100.0;
+        final double groundSpeed = Math.sqrt(Math.pow(groundSpeedX, 2) + Math.pow(groundSpeedY, 2));
+
+        final double climbRate = gpi.vz / 100.0;
+        setAltitudeGroundAndAirSpeeds(relativeAlt, groundSpeed, groundSpeed, climbRate);
     }
 
     protected void processMountStatus(msg_mount_status mountStatus) {
diff --git a/ClientLib/src/main/java/org/droidplanner/services/android/impl/core/drone/autopilot/apm/ArduPlane.java b/ClientLib/src/main/java/org/droidplanner/services/android/impl/core/drone/autopilot/apm/ArduPlane.java
index 2a76271aa7..74a0f87a72 100644
--- a/ClientLib/src/main/java/org/droidplanner/services/android/impl/core/drone/autopilot/apm/ArduPlane.java
+++ b/ClientLib/src/main/java/org/droidplanner/services/android/impl/core/drone/autopilot/apm/ArduPlane.java
@@ -41,32 +41,6 @@ public FirmwareType getFirmwareType() {
         return FirmwareType.ARDU_PLANE;
     }
 
-    @Override
-    protected void processVfrHud(msg_vfr_hud vfrHud){
-        //Nothing to do. Plane used GLOBAL_POSITION_INT to set altitude and speeds unlike copter
-    }
-
-    /**
-     * Used to update the vehicle location, and altitude.
-     * @param gpi
-     */
-    @Override
-    protected void processGlobalPositionInt(msg_global_position_int gpi){
-        if(gpi == null)
-            return;
-
-        super.processGlobalPositionInt(gpi);
-
-        final double relativeAlt = gpi.relative_alt / 1000.0;
-
-        final double groundSpeedX = gpi.vx / 100.0;
-        final double groundSpeedY = gpi.vy / 100.0;
-        final double groundSpeed = Math.sqrt(Math.pow(groundSpeedX, 2) + Math.pow(groundSpeedY, 2));
-
-        final double climbRate = gpi.vz / 100.0;
-        setAltitudeGroundAndAirSpeeds(relativeAlt, groundSpeed, groundSpeed, climbRate);
-    }
-
     @Override
     protected boolean isFeatureSupported(String featureId){
         switch(featureId){