Sub: consolidate set_home functions

added lock argument instead of having twice as many functions
no functional change

ArduSub/GCS_Mavlink.cpp

@@ -1138,7 +1138,7 @@ void GCS_MAVLINK_Sub::handleMessage(mavlink_message_t* msg)
             // param7 : altitude (absolute)
             result = MAV_RESULT_FAILED; // assume failure
             if (is_equal(packet.param1,1.0f) || (is_zero(packet.param5) && is_zero(packet.param6) && is_zero(packet.param7))) {
-                if (sub.set_home_to_current_location_and_lock()) {
+                if (sub.set_home_to_current_location(true)) {
                     result = MAV_RESULT_ACCEPTED;
                 }
             } else {
@@ -1151,7 +1151,7 @@ void GCS_MAVLINK_Sub::handleMessage(mavlink_message_t* msg)
                 new_home_loc.lng = (int32_t)(packet.param6 * 1.0e7f);
                 new_home_loc.alt = (int32_t)(packet.param7 * 100.0f);
                 if (!sub.far_from_EKF_origin(new_home_loc)) {
-                    if (sub.set_home_and_lock(new_home_loc)) {
+                    if (sub.set_home(new_home_loc, true)) {
                         result = MAV_RESULT_ACCEPTED;
                     }
                 }
@@ -1789,7 +1789,7 @@ void GCS_MAVLINK_Sub::handleMessage(mavlink_message_t* msg)
         mavlink_set_home_position_t packet;
         mavlink_msg_set_home_position_decode(msg, &packet);
         if ((packet.latitude == 0) && (packet.longitude == 0) && (packet.altitude == 0)) {
-            sub.set_home_to_current_location_and_lock();
+            sub.set_home_to_current_location(true);
         } else {
             // sanity check location
             if (!check_latlng(packet.latitude, packet.longitude)) {
@@ -1802,7 +1802,7 @@ void GCS_MAVLINK_Sub::handleMessage(mavlink_message_t* msg)
             if (sub.far_from_EKF_origin(new_home_loc)) {
                 break;
             }
-            sub.set_home_and_lock(new_home_loc);
+            sub.set_home(new_home_loc, true);
         }
         break;
     }

ArduSub/Sub.h

@@ -525,10 +525,8 @@ private:
     void userhook_SuperSlowLoop();
     void update_home_from_EKF();
     void set_home_to_current_location_inflight();
-    bool set_home_to_current_location();
-    bool set_home_to_current_location_and_lock();
-    bool set_home_and_lock(const Location& loc);
-    bool set_home(const Location& loc);
+    bool set_home_to_current_location(bool lock);
+    bool set_home(const Location& loc, bool lock);
     bool far_from_EKF_origin(const Location& loc);
     void set_system_time_from_GPS();
     void exit_mission();

ArduSub/commands.cpp

@@ -20,7 +20,7 @@ void Sub::update_home_from_EKF()
         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(false);
     }
 }
 
@@ -32,12 +32,12 @@ void Sub::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);
+        set_home(temp_loc, false);
     }
 }
 
 // set_home_to_current_location - set home to current GPS location
-bool Sub::set_home_to_current_location()
+bool Sub::set_home_to_current_location(bool lock)
 {
     // get current location from EKF
     Location temp_loc;
@@ -48,28 +48,7 @@ bool Sub::set_home_to_current_location()
         // This also ensures that mission items with relative altitude frame, are always
         // relative to the water's surface, whether in a high elevation lake, or at sea level.
         temp_loc.alt -= barometer.get_altitude() * 100.0f;
-        return set_home(temp_loc);
-    }
-    return false;
-}
-
-// set_home_to_current_location_and_lock - set home to current location and lock so it cannot be moved
-bool Sub::set_home_to_current_location_and_lock()
-{
-    if (set_home_to_current_location()) {
-        set_home_state(HOME_SET_AND_LOCKED);
-        return true;
-    }
-    return false;
-}
-
-// set_home_and_lock - sets ahrs home (used for RTL) to specified location and locks so it cannot be moved
-//  unless this function is called again
-bool Sub::set_home_and_lock(const Location& loc)
-{
-    if (set_home(loc)) {
-        set_home_state(HOME_SET_AND_LOCKED);
-        return true;
+        return set_home(temp_loc, lock);
     }
     return false;
 }
@@ -77,7 +56,7 @@ bool Sub::set_home_and_lock(const Location& loc)
 // 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 Sub::set_home(const Location& loc)
+bool Sub::set_home(const Location& loc, bool lock)
 {
     // check location is valid
     if (loc.lat == 0 && loc.lng == 0) {
@@ -107,6 +86,11 @@ bool Sub::set_home(const Location& loc)
         }
     }
 
+    // lock home position
+    if (lock) {
+        set_home_state(HOME_SET_AND_LOCKED);
+    }
+
     // log ahrs home and ekf origin dataflash
     Log_Write_Home_And_Origin();
 

ArduSub/commands_logic.cpp

@@ -840,10 +840,10 @@ void Sub::do_change_speed(const AP_Mission::Mission_Command& cmd)
 void Sub::do_set_home(const AP_Mission::Mission_Command& cmd)
 {
     if (cmd.p1 == 1 || (cmd.content.location.lat == 0 && cmd.content.location.lng == 0 && cmd.content.location.alt == 0)) {
-        set_home_to_current_location();
+        set_home_to_current_location(false);
     } else {
         if (!far_from_EKF_origin(cmd.content.location)) {
-            set_home(cmd.content.location);
+            set_home(cmd.content.location, false);
         }
     }
 }

ArduSub/motors.cpp

@@ -55,7 +55,7 @@ bool Sub::init_arm_motors(bool arming_from_gcs)
         // Log_Write_Event(DATA_EKF_ALT_RESET);
     } else if (ap.home_state == HOME_SET_NOT_LOCKED) {
         // Reset home position if it has already been set before (but not locked)
-        set_home_to_current_location();
+        set_home_to_current_location(false);
     }
 	
     // enable gps velocity based centrefugal force compensation