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..c544247271 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; @@ -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/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){