From 1f96cbd015b8d8245dd5cfe258c2de8ff77d6d55 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 13 May 2015 15:25:50 +0900 Subject: [PATCH] Copter: home-alt set to EKF-origin if captured in flight --- ArduCopter/commands.pde | 20 ++++++++++++++++++-- 1 file changed, 18 insertions(+), 2 deletions(-) diff --git a/ArduCopter/commands.pde b/ArduCopter/commands.pde index a2d12d89f4..a7919b9800 100644 --- a/ArduCopter/commands.pde +++ b/ArduCopter/commands.pde @@ -15,8 +15,24 @@ static void update_home_from_EKF() return; } - // move home to current ekf location (this will set home_state to HOME_SET) - set_home_to_current_location(); + // special logic if home is set in-flight + if (motors.armed()) { + set_home_to_current_location_inflight(); + } else { + // move home to current ekf location (this will set home_state to HOME_SET) + set_home_to_current_location(); + } +} + +// set_home_to_current_location_inflight - set home to current GPS location (horizontally) and EKF origin+ takeoff_alt_above_origin +static void set_home_to_current_location_inflight() { + // get current location from EKF + Location temp_loc; + if (inertial_nav.get_location(temp_loc)) { + const struct Location &ekf_origin = inertial_nav.get_origin(); + temp_loc.alt = ekf_origin.alt; + set_home(temp_loc); + } } // set_home_to_current_location - set home to current GPS location