From aec18c35382d64b5826789ad61cf2d4bb0216024 Mon Sep 17 00:00:00 2001 From: Jonathan Challinger Date: Mon, 11 Jan 2016 13:29:06 -0800 Subject: [PATCH] Copter: support SET_POSITION_TARGET with WGS84 altitudes --- ArduCopter/GCS_Mavlink.pde | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/ArduCopter/GCS_Mavlink.pde b/ArduCopter/GCS_Mavlink.pde index 0575757a92..ff94e8737b 100644 --- a/ArduCopter/GCS_Mavlink.pde +++ b/ArduCopter/GCS_Mavlink.pde @@ -1657,7 +1657,10 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) case MAV_FRAME_GLOBAL: case MAV_FRAME_GLOBAL_INT: default: - loc.flags.relative_alt = false; + // Copter does not support navigation to absolute altitudes. This is a temporary hack + // to convert the WGS84 altitude to a home-relative altitude before passing it to Copter + loc.alt -= ahrs.get_home().alt; + loc.flags.relative_alt = true; loc.flags.terrain_alt = false; break; }