From 5f610fdcba72b5073653a47f6f3af67b6219eee2 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.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index fe56953ea9..d7d1103f99 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -1717,7 +1717,10 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) break; case MAV_FRAME_GLOBAL_INT: default: - loc.flags.relative_alt = false; + // Copter does not support navigation to absolute altitudes. This convert the WGS84 altitude + // to a home-relative altitude before passing it to the navigation controller + loc.alt -= copter.ahrs.get_home().alt; + loc.flags.relative_alt = true; loc.flags.terrain_alt = false; break; }