From bd8bcd5ab17176754fc1f42c3d6e1755b7c37a14 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Sat, 1 Feb 2020 16:55:28 +0900 Subject: [PATCH] Copter: correct current_loc to be alt-above-home --- ArduCopter/inertia.cpp | 13 ++++++------- 1 file changed, 6 insertions(+), 7 deletions(-) diff --git a/ArduCopter/inertia.cpp b/ArduCopter/inertia.cpp index ac9fc94eb4..5dc037e132 100644 --- a/ArduCopter/inertia.cpp +++ b/ArduCopter/inertia.cpp @@ -17,12 +17,11 @@ void Copter::read_inertia() return; } - if (ahrs.home_is_set()) { - current_loc.set_alt_cm(inertial_nav.get_altitude(), - Location::AltFrame::ABOVE_HOME); - } else { - // without home use alt above the EKF origin - current_loc.set_alt_cm(inertial_nav.get_altitude(), - Location::AltFrame::ABOVE_ORIGIN); + // current_loc.alt is alt-above-home, converted from inertial nav's alt-above-ekf-origin + const int32_t alt_above_origin_cm = inertial_nav.get_altitude(); + current_loc.set_alt_cm(alt_above_origin_cm, Location::AltFrame::ABOVE_ORIGIN); + if (!current_loc.change_alt_frame(Location::AltFrame::ABOVE_HOME)) { + // if home has not been set yet we treat alt-above-origin as alt-above-home + current_loc.set_alt_cm(alt_above_origin_cm, Location::AltFrame::ABOVE_HOME); } }