diff --git a/ArduSub/commands.cpp b/ArduSub/commands.cpp index b03213a3b7..6a984655d8 100644 --- a/ArduSub/commands.cpp +++ b/ArduSub/commands.cpp @@ -1,12 +1,5 @@ #include "Sub.h" -/* - * the home_state has a number of possible values (see enum HomeState in defines.h's) - * HOME_UNSET = home is not set, no GPS positions yet received - * HOME_SET_NOT_LOCKED = home is set to EKF origin or armed location (can be moved) - * HOME_SET_AND_LOCKED = home has been set by user, cannot be moved except by user initiated do-set-home command - */ - // checks if we should update ahrs/RTL home position from the EKF void Sub::update_home_from_EKF() { @@ -69,15 +62,16 @@ bool Sub::set_home(const Location& loc, bool lock) return false; } + const bool home_was_set = ahrs.home_is_set(); + // set ahrs home (used for RTL) ahrs.set_home(loc); // init inav and compass declination - if (!ahrs.home_is_set()) { + if (!home_was_set) { // update navigation scalers. used to offset the shrinking longitude as we go towards the poles scaleLongDown = longitude_scale(loc); // record home is set - ahrs.set_home_status(HOME_SET_NOT_LOCKED); Log_Write_Event(DATA_SET_HOME); // log new home position which mission library will pull from ahrs @@ -91,7 +85,7 @@ bool Sub::set_home(const Location& loc, bool lock) // lock home position if (lock) { - ahrs.set_home_status(HOME_SET_AND_LOCKED); + ahrs.lock_home(); } // log ahrs home and ekf origin dataflash diff --git a/ArduSub/motors.cpp b/ArduSub/motors.cpp index 7c0b2849c5..584f90138d 100644 --- a/ArduSub/motors.cpp +++ b/ArduSub/motors.cpp @@ -50,7 +50,7 @@ bool Sub::init_arm_motors(bool arming_from_gcs) // Always use absolute altitude for ROV // ahrs.resetHeightDatum(); // Log_Write_Event(DATA_EKF_ALT_RESET); - } else if (ahrs.home_status() == HOME_SET_NOT_LOCKED) { + } else if (ahrs.home_is_set() && !ahrs.home_is_locked()) { // Reset home position if it has already been set before (but not locked) set_home_to_current_location(false); }