From c772e2d3fd9c68275504bb04b0b9f0078d9d0897 Mon Sep 17 00:00:00 2001 From: Dylan Herman Date: Mon, 15 Jan 2018 12:03:41 -0500 Subject: [PATCH] Copter: sets SmartRTL home after AHRS home is set Now calls AP_SmartRTL::set_home when arming. In addition, it calls it whenever the ahrs home is set to the current location, whether by GCS or in-flight Copter: merge --- ArduCopter/commands.cpp | 13 +++++++++++-- ArduCopter/motors.cpp | 2 +- 2 files changed, 12 insertions(+), 3 deletions(-) diff --git a/ArduCopter/commands.cpp b/ArduCopter/commands.cpp index 9eae1f572c..c55f5c6433 100644 --- a/ArduCopter/commands.cpp +++ b/ArduCopter/commands.cpp @@ -31,7 +31,11 @@ void Copter::set_home_to_current_location_inflight() { 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, false); + if (!set_home(temp_loc, false)) { + return; + } + // we have successfully set AHRS home, set it for SmartRTL + g2.smart_rtl.set_home(true); } } @@ -40,7 +44,12 @@ bool Copter::set_home_to_current_location(bool lock) { // get current location from EKF Location temp_loc; if (inertial_nav.get_location(temp_loc)) { - return set_home(temp_loc, lock); + if (!set_home(temp_loc, lock)) { + return false; + } + // we have successfully set AHRS home, set it for SmartRTL + g2.smart_rtl.set_home(true); + return true; } return false; } diff --git a/ArduCopter/motors.cpp b/ArduCopter/motors.cpp index 58d1cbc7fe..b5ccff8b4f 100644 --- a/ArduCopter/motors.cpp +++ b/ArduCopter/motors.cpp @@ -180,7 +180,7 @@ bool Copter::init_arm_motors(bool arming_from_gcs) update_super_simple_bearing(false); // Reset SmartRTL return location. If activated, SmartRTL will ultimately try to land at this point - g2.smart_rtl.reset_path(position_ok()); + g2.smart_rtl.set_home(position_ok()); // enable gps velocity based centrefugal force compensation ahrs.set_correct_centrifugal(true);