diff --git a/libraries/AP_Mission/AP_Mission.cpp b/libraries/AP_Mission/AP_Mission.cpp index 2e5fb77d99..ea441be219 100644 --- a/libraries/AP_Mission/AP_Mission.cpp +++ b/libraries/AP_Mission/AP_Mission.cpp @@ -95,7 +95,7 @@ void AP_Mission::init() init_jump_tracking(); // If Mission Clear bit is set then it should clear the mission, otherwise retain the mission. - if (AP_MISSION_MASK_MISSION_CLEAR & _options) { + if (option_is_set(Option::CLEAR_ON_BOOT)) { GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Clearing Mission"); clear(); } @@ -2502,7 +2502,7 @@ bool AP_Mission::is_best_land_sequence(const Location ¤t_loc) } // check if MIS_OPTIONS bit set to allow distance calculation to be done - if (!(_options & AP_MISSION_MASK_DIST_TO_LAND_CALC)) { + if (!option_is_set(Option::FAILSAFE_TO_BEST_LANDING)) { return false; } diff --git a/libraries/AP_Mission/AP_Mission.h b/libraries/AP_Mission/AP_Mission.h index 69f8fe2b46..32bb2fa77d 100644 --- a/libraries/AP_Mission/AP_Mission.h +++ b/libraries/AP_Mission/AP_Mission.h @@ -45,9 +45,6 @@ #define AP_MISSION_RESTART_DEFAULT 0 // resume the mission from the last command run by default #define AP_MISSION_OPTIONS_DEFAULT 0 // Do not clear the mission when rebooting -#define AP_MISSION_MASK_MISSION_CLEAR (1<<0) // If set then Clear the mission on boot -#define AP_MISSION_MASK_DIST_TO_LAND_CALC (1<<1) // Allow distance to best landing calculation to be run on failsafe -#define AP_MISSION_MASK_CONTINUE_AFTER_LAND (1<<2) // Allow mission to continue after land #define AP_MISSION_MAX_WP_HISTORY 7 // The maximum number of previous wp commands that will be stored from the active missions history #define LAST_WP_PASSED (AP_MISSION_MAX_WP_HISTORY-2) @@ -732,14 +729,23 @@ public: void reset_wp_history(void); /* - return true if MIS_OPTIONS is set to allow continue of mission - logic after a land and the next waypoint is a takeoff. If this + Option::FailsafeToBestLanding - continue mission + logic after a land if the next waypoint is a takeoff. If this is false then after a landing is complete the vehicle should disarm and mission logic should stop */ + enum class Option { + CLEAR_ON_BOOT = 0, // clear mission on vehicle boot + FAILSAFE_TO_BEST_LANDING = 1, // on failsafe, find fastest path along mission home + CONTINUE_AFTER_LAND = 2, // continue running mission (do not disarm) after land if takeoff is next waypoint + }; + bool option_is_set(Option option) const { + return (_options.get() & (uint16_t)option) != 0; + } + bool continue_after_land_check_for_takeoff(void); bool continue_after_land(void) const { - return (_options.get() & AP_MISSION_MASK_CONTINUE_AFTER_LAND) != 0; + return option_is_set(Option::CONTINUE_AFTER_LAND); } // user settable parameters