Use GLOBAL_POSITION_INT to set altitude and speeds in ArduPilot

dronekit · Nov 16, 2018 · 82fc116 · 82fc116
1 parent d985bcc
commit 82fc116
Show file tree

Hide file tree

Showing 2 changed files with 18 additions and 33 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;
@@ -416,10 +417,6 @@ public void onMavLinkMessageReceived(MAVLinkMessage message) {
                     processStatusText(msg_statustext);
                     break;
 
-                case msg_vfr_hud.MAVLINK_MSG_ID_VFR_HUD:
-                    processVfrHud((msg_vfr_hud) message);
-                    break;
-
                 case msg_raw_imu.MAVLINK_MSG_ID_RAW_IMU:
                     msg_raw_imu msg_imu = (msg_raw_imu) message;
                     mag.newData(msg_imu);
@@ -511,11 +508,25 @@ private void checkControlSensorsHealth(msg_sys_status sysStatus) {
         }
     }
 
-    protected void processVfrHud(msg_vfr_hud vfrHud) {
-        if (vfrHud == null)
+    /**
+     * 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){