diff --git a/Rover/mode.cpp b/Rover/mode.cpp index 866b7a5397ce82..b5800acb340eb1 100644 --- a/Rover/mode.cpp +++ b/Rover/mode.cpp @@ -44,7 +44,8 @@ bool Mode::enter() // initialisation common to all modes if (ret) { - set_reversed(false); + // init reversed flag + init_reversed_flag(); // clear sailboat tacking flags g2.sailboat.clear_tack(); diff --git a/Rover/mode.h b/Rover/mode.h index f166843ed0d231..71715d5486c103 100644 --- a/Rover/mode.h +++ b/Rover/mode.h @@ -124,6 +124,9 @@ class Mode // execute the mission in reverse (i.e. backing up) void set_reversed(bool value); + // init reversed flag for autopilot mode + virtual void init_reversed_flag() { if (is_autopilot_mode()) { set_reversed(false); } } + // handle tacking request (from auxiliary switch) in sailboats virtual void handle_tack_request(); @@ -277,6 +280,13 @@ class ModeAuto : public Mode bool nav_script_time(uint16_t &id, uint8_t &cmd, float &arg1, float &arg2, int16_t &arg3, int16_t &arg4); void nav_script_time_done(uint16_t id); + // + void init_reversed_flag() override { + if (!mission.is_resume()) { + set_reversed(false); + } + } + AP_Mission mission{ FUNCTOR_BIND_MEMBER(&ModeAuto::start_command, bool, const AP_Mission::Mission_Command&), FUNCTOR_BIND_MEMBER(&ModeAuto::verify_command_callback, bool, const AP_Mission::Mission_Command&), diff --git a/libraries/AP_Mission/AP_Mission.h b/libraries/AP_Mission/AP_Mission.h index a06fa0012811fe..e76c3a638f5486 100644 --- a/libraries/AP_Mission/AP_Mission.h +++ b/libraries/AP_Mission/AP_Mission.h @@ -709,6 +709,9 @@ class AP_Mission _force_resume = force_resume; } + // returns true if configured to resume + bool is_resume() const { return _restart == 0 || _force_resume; } + // get a reference to the AP_Mission semaphore, allowing an external caller to lock the // storage while working with multiple waypoints HAL_Semaphore &get_semaphore(void)