Use GLOBAL_POSITION_INT to set altitude and speeds in ArduPilot

dronekit · Nov 15, 2018 · 73dfa16 · 73dfa16
1 parent d985bcc
commit 73dfa16
Show file tree

Hide file tree

Showing 2 changed files with 21 additions and 28 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
@@ -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/.../main/java/org/droidplanner/services/android/impl/core/drone/autopilot/apm/ArduPlane.java b/.../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){