Factor handling of set_home up into AP_Vehicle

AntennaTracker/Tracker.h

@@ -197,7 +197,6 @@ class Tracker : public AP_Vehicle {
     void init_ardupilot() override;
     bool get_home_eeprom(Location &loc) const;
     bool set_home_eeprom(const Location &temp) WARN_IF_UNUSED;
-    bool set_home_to_current_location(bool lock) override WARN_IF_UNUSED;
     bool set_home(const Location &temp, bool lock) override WARN_IF_UNUSED;
     void prepare_servos();
     void set_mode(Mode &newmode, ModeReason reason);

AntennaTracker/system.cpp

@@ -116,25 +116,19 @@ bool Tracker::set_home_eeprom(const Location &temp)
     return true;
 }
 
-bool Tracker::set_home_to_current_location(bool lock)
-{
-    return set_home(AP::gps().location(), lock);
-}
-
 bool Tracker::set_home(const Location &temp, bool lock)
 {
-    // check EKF origin has been set
-    Location ekf_origin;
-    if (ahrs.get_origin(ekf_origin)) {
-        if (!ahrs.set_home(temp)) {
-            return false;
-        }
+    if (!AP_Vehicle::set_home(temp, lock)) {
+        return false;
     }
 
+    // also store this away for next boot:
     if (!set_home_eeprom(temp)) {
         return false;
     }
 
+    // and move our current location there, in case we don't have an
+    // absolute position source:
     current_loc = temp;
 
     return true;

ArduCopter/Copter.h

@@ -740,7 +740,6 @@ class Copter : public AP_Vehicle {
     void update_home_from_EKF();
     void set_home_to_current_location_inflight();
     bool set_home_to_current_location(bool lock) override WARN_IF_UNUSED;
-    bool set_home(const Location& loc, bool lock) override WARN_IF_UNUSED;
 
     // compassmot.cpp
     MAV_RESULT mavlink_compassmot(const GCS_MAVLINK &gcs_chan);

ArduCopter/commands.cpp

@@ -37,42 +37,16 @@ void Copter::set_home_to_current_location_inflight() {
 }
 
 // set_home_to_current_location - set home to current GPS location
-bool Copter::set_home_to_current_location(bool lock) {
-    // get current location from EKF
-    Location temp_loc;
-    if (ahrs.get_location(temp_loc)) {
-        if (!set_home(temp_loc, lock)) {
-            return false;
-        }
-        // we have successfully set AHRS home, set it for SmartRTL
-#if MODE_SMARTRTL_ENABLED == ENABLED
-        g2.smart_rtl.set_home(true);
-#endif
-        return true;
-    }
-    return false;
-}
-
-// set_home - sets ahrs home (used for RTL) to specified location
-//  returns true if home location set successfully
-bool Copter::set_home(const Location& loc, bool lock)
+bool Copter::set_home_to_current_location(bool lock)
 {
-    // check EKF origin has been set
-    Location ekf_origin;
-    if (!ahrs.get_origin(ekf_origin)) {
-        return false;
-    }
-
-    // set ahrs home (used for RTL)
-    if (!ahrs.set_home(loc)) {
+    if (!AP_Vehicle::set_home_to_current_location(lock)) {
         return false;
     }
 
-    // lock home position
-    if (lock) {
-        ahrs.lock_home();
-    }
+    // we have successfully set AHRS home, set it for SmartRTL
+#if MODE_SMARTRTL_ENABLED == ENABLED
+    g2.smart_rtl.set_home(true);
+#endif
 
-    // return success
     return true;
 }

ArduSub/Sub.h

@@ -424,7 +424,6 @@ class Sub : public AP_Vehicle {
     void update_home_from_EKF();
     void set_home_to_current_location_inflight();
     bool set_home_to_current_location(bool lock) override WARN_IF_UNUSED;
-    bool set_home(const Location& loc, bool lock) override WARN_IF_UNUSED;
     float get_alt_rel() const WARN_IF_UNUSED;
     float get_alt_msl() const WARN_IF_UNUSED;
     void exit_mission();

ArduSub/commands.cpp

@@ -49,27 +49,3 @@ bool Sub::set_home_to_current_location(bool lock)
     }
     return false;
 }
-
-// set_home - sets ahrs home (used for RTL) to specified location
-//  returns true if home location set successfully
-bool Sub::set_home(const Location& loc, bool lock)
-{
-    // check if EKF origin has been set
-    Location ekf_origin;
-    if (!ahrs.get_origin(ekf_origin)) {
-        return false;
-    }
-
-    // set ahrs home (used for RTL)
-    if (!ahrs.set_home(loc)) {
-        return false;
-    }
-
-    // lock home position
-    if (lock) {
-        ahrs.lock_home();
-    }
-
-    // return success
-    return true;
-}

Blimp/Blimp.h

@@ -310,8 +310,6 @@ class Blimp : public AP_Vehicle
     // commands.cpp
     void update_home_from_EKF();
     void set_home_to_current_location_inflight();
-    bool set_home_to_current_location(bool lock) override WARN_IF_UNUSED;
-    bool set_home(const Location& loc, bool lock) override WARN_IF_UNUSED;
 
     // ekf_check.cpp
     void ekf_check();

Blimp/commands.cpp

@@ -32,42 +32,3 @@ void Blimp::set_home_to_current_location_inflight()
         }
     }
 }
-
-// set_home_to_current_location - set home to current GPS location
-bool Blimp::set_home_to_current_location(bool lock)
-{
-    // get current location from EKF
-    Location temp_loc;
-    if (ahrs.get_location(temp_loc)) {
-        if (!set_home(temp_loc, lock)) {
-            return false;
-        }
-        return true;
-    }
-    return false;
-}
-
-// set_home - sets ahrs home (used for RTL) to specified location
-//  initialises inertial nav and compass on first call
-//  returns true if home location set successfully
-bool Blimp::set_home(const Location& loc, bool lock)
-{
-    // check EKF origin has been set
-    Location ekf_origin;
-    if (!ahrs.get_origin(ekf_origin)) {
-        return false;
-    }
-
-    // set ahrs home (used for RTL)
-    if (!ahrs.set_home(loc)) {
-        return false;
-    }
-
-    // lock home position
-    if (lock) {
-        ahrs.lock_home();
-    }
-
-    // return success
-    return true;
-}

Rover/commands.cpp

@@ -3,32 +3,24 @@
 // set ahrs home to current location from inertial-nav location
 bool Rover::set_home_to_current_location(bool lock)
 {
-    Location temp_loc;
-    if (ahrs.have_inertial_nav() && ahrs.get_location(temp_loc)) {
-        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;
+    if (!AP_Vehicle::set_home_to_current_location(lock)) {
+        return false;
     }
-    return false;
+
+    // we have successfully set AHRS home, set it for SmartRTL
+    g2.smart_rtl.set_home(true);
+
+    return true;
 }
 
 // sets ahrs home to specified location
 // returns true if home location set successfully
 bool Rover::set_home(const Location& loc, bool lock)
 {
-    // set ahrs home
-    if (!ahrs.set_home(loc)) {
+    if (!AP_Vehicle::set_home(loc, lock)) {
         return false;
     }
 
-    // lock home position
-    if (lock) {
-        ahrs.lock_home();
-    }
-
     // Save Home to EEPROM
     mode_auto.mission.write_home_to_storage();
 

libraries/AP_Vehicle/AP_Vehicle.cpp

@@ -1098,6 +1098,49 @@ bool AP_Vehicle::block_GCS_mode_change(uint8_t mode_num, const uint8_t *mode_lis
 }
 #endif
 
+#if AP_AHRS_ENABLED
+// set_home - sets ahrs home (used for RTL) to specified location
+//  returns true if home location set successfully
+bool AP_Vehicle::set_home(const Location& loc, bool lock)
+{
+    // check EKF origin has been set
+    Location ekf_origin;
+    if (!ahrs.get_origin(ekf_origin)) {
+        return false;
+    }
+
+    // set ahrs home (used for RTL)
+    if (!ahrs.set_home(loc)) {
+        return false;
+    }
+
+    // lock home position
+    if (lock) {
+        ahrs.lock_home();
+    }
+
+    // return success
+    return true;
+}
+
+// set_home_to_current_location - set home to current AHRS location
+bool AP_Vehicle::set_home_to_current_location(bool lock)
+{
+    // get current location from AHRS
+    Location temp_loc;
+    if (!ahrs.get_location(temp_loc)) {
+        return false;
+    }
+
+    if (!set_home(temp_loc, lock)) {
+        return false;
+    }
+
+    return true;
+}
+
+#endif  // AP_AHRS_ENABLED
+
 AP_Vehicle *AP_Vehicle::_singleton = nullptr;
 
 AP_Vehicle *AP_Vehicle::get_singleton()

libraries/AP_Vehicle/AP_Vehicle.h

@@ -290,8 +290,8 @@ class AP_Vehicle : public AP_HAL::HAL::Callbacks {
     virtual bool get_rate_ef_targets(Vector3f& rate_ef_targets) const { return false; }
 
 #if AP_AHRS_ENABLED
-    virtual bool set_home_to_current_location(bool lock) WARN_IF_UNUSED { return false; }
-    virtual bool set_home(const Location& loc, bool lock) WARN_IF_UNUSED { return false; }
+    virtual bool set_home_to_current_location(bool lock) WARN_IF_UNUSED;
+    virtual bool set_home(const Location& loc, bool lock) WARN_IF_UNUSED;
 #endif
 
 protected: