From 814cadac68698f7de5a109f87c972f0400004e84 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 5 Jun 2017 17:03:50 +0900 Subject: [PATCH] Copter: consolidate set_home functions added lock argument instead of having twice as many functions no functional change --- ArduCopter/Copter.h | 6 ++---- ArduCopter/GCS_Mavlink.cpp | 8 ++++---- ArduCopter/commands.cpp | 36 ++++++++++------------------------- ArduCopter/commands_logic.cpp | 4 ++-- ArduCopter/motors.cpp | 2 +- 5 files changed, 19 insertions(+), 37 deletions(-) diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 2261cf5cbe..595b1fce98 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -762,10 +762,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/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index 8f0b35c178..98a240aa34 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -1156,7 +1156,7 @@ void GCS_MAVLINK_Copter::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 (copter.set_home_to_current_location_and_lock()) { + if (copter.set_home_to_current_location(true)) { result = MAV_RESULT_ACCEPTED; } } else { @@ -1168,7 +1168,7 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg) new_home_loc.lat = (int32_t)(packet.param5 * 1.0e7f); new_home_loc.lng = (int32_t)(packet.param6 * 1.0e7f); new_home_loc.alt = (int32_t)(packet.param7 * 100.0f); - if (copter.set_home_and_lock(new_home_loc)) { + if (copter.set_home(new_home_loc, true)) { result = MAV_RESULT_ACCEPTED; } } @@ -2005,7 +2005,7 @@ void GCS_MAVLINK_Copter::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)) { - copter.set_home_to_current_location_and_lock(); + copter.set_home_to_current_location(true); } else { // sanity check location if (!check_latlng(packet.latitude, packet.longitude)) { @@ -2015,7 +2015,7 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg) new_home_loc.lat = packet.latitude; new_home_loc.lng = packet.longitude; new_home_loc.alt = packet.altitude / 10; - copter.set_home_and_lock(new_home_loc); + copter.set_home(new_home_loc, true); } break; } diff --git a/ArduCopter/commands.cpp b/ArduCopter/commands.cpp index 62cac6815d..23eb135bad 100644 --- a/ArduCopter/commands.cpp +++ b/ArduCopter/commands.cpp @@ -20,7 +20,7 @@ void Copter::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); } } @@ -31,37 +31,16 @@ 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); + set_home(temp_loc, false); } } // set_home_to_current_location - set home to current GPS location -bool Copter::set_home_to_current_location() { +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); - } - return false; -} - -// set_home_to_current_location_and_lock - set home to current location and lock so it cannot be moved -bool Copter::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 Copter::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; } @@ -69,7 +48,7 @@ bool Copter::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 Copter::set_home(const Location& loc) +bool Copter::set_home(const Location& loc, bool lock) { // check location is valid if (loc.lat == 0 && loc.lng == 0) { @@ -111,6 +90,11 @@ bool Copter::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/ArduCopter/commands_logic.cpp b/ArduCopter/commands_logic.cpp index 8cee333963..1abe719a4c 100644 --- a/ArduCopter/commands_logic.cpp +++ b/ArduCopter/commands_logic.cpp @@ -1102,9 +1102,9 @@ void Copter::do_change_speed(const AP_Mission::Mission_Command& cmd) void Copter::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 { - set_home(cmd.content.location); + set_home(cmd.content.location, false); } } diff --git a/ArduCopter/motors.cpp b/ArduCopter/motors.cpp index 9eb9ea474f..721f9890ae 100644 --- a/ArduCopter/motors.cpp +++ b/ArduCopter/motors.cpp @@ -175,7 +175,7 @@ bool Copter::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); } calc_distance_and_bearing();