From 51c97af5b54289bf5bd963bf1b11db552e7a9708 Mon Sep 17 00:00:00 2001 From: Michael du Breuil Date: Mon, 27 Feb 2017 12:14:51 -0700 Subject: [PATCH] Rover: Reset home to AHRS location rather then snapshotting GPS Also corrects rover locking home altitude to EKF origin altitude --- APMrover2/commands.cpp | 15 +++++---------- 1 file changed, 5 insertions(+), 10 deletions(-) diff --git a/APMrover2/commands.cpp b/APMrover2/commands.cpp index 3b5987ef8f..d52dba6a4e 100644 --- a/APMrover2/commands.cpp +++ b/APMrover2/commands.cpp @@ -95,17 +95,12 @@ void Rover::restart_nav() void Rover::update_home() { if (home_is_set == HOME_SET_NOT_LOCKED) { - Location loc = gps.location(); - Location origin; - // if an EKF origin is available then we leave home equal to - // the height of that origin. This ensures that our relative - // height calculations are using the same origin - if (ahrs.get_origin(origin)) { - loc.alt = origin.alt; + Location loc; + if (ahrs.get_position(loc)) { + ahrs.set_home(loc); + Log_Write_Home_And_Origin(); + GCS_MAVLINK::send_home_all(gps.location()); } - ahrs.set_home(loc); - Log_Write_Home_And_Origin(); - GCS_MAVLINK::send_home_all(gps.location()); } barometer.update_calibration(); }