diff --git a/ArduSub/GCS_Mavlink.cpp b/ArduSub/GCS_Mavlink.cpp index 4cd74c9f0a..223d6481a1 100644 --- a/ArduSub/GCS_Mavlink.cpp +++ b/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; } diff --git a/ArduSub/Sub.h b/ArduSub/Sub.h index 4b69e0e165..c0edc51e4f 100644 --- a/ArduSub/Sub.h +++ b/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(); diff --git a/ArduSub/commands.cpp b/ArduSub/commands.cpp index a6a3ce66d9..eaf40ce4f3 100644 --- a/ArduSub/commands.cpp +++ b/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(); diff --git a/ArduSub/commands_logic.cpp b/ArduSub/commands_logic.cpp index 4c74666fa2..db3d7823fa 100644 --- a/ArduSub/commands_logic.cpp +++ b/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); } } } diff --git a/ArduSub/motors.cpp b/ArduSub/motors.cpp index 5d145f2d39..12a2e004b9 100644 --- a/ArduSub/motors.cpp +++ b/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