From a80e72ff82589ae3d4cf4f2c8dfbc55877e303a8 Mon Sep 17 00:00:00 2001 From: Andrew Chapman Date: Tue, 15 Apr 2014 09:26:45 -0700 Subject: [PATCH] AP_Mission: added MIS_AUTORESTART parameter - added MIS_AUTORESTART parameter, defaults to 0 - added start_or_resume() function to either start or resume a mission based on that parameter value --- libraries/AP_Mission/AP_Mission.cpp | 15 +++++++++++++++ libraries/AP_Mission/AP_Mission.h | 6 +++++- 2 files changed, 20 insertions(+), 1 deletion(-) diff --git a/libraries/AP_Mission/AP_Mission.cpp b/libraries/AP_Mission/AP_Mission.cpp index b2b928749f..88c1d497ce 100644 --- a/libraries/AP_Mission/AP_Mission.cpp +++ b/libraries/AP_Mission/AP_Mission.cpp @@ -15,6 +15,12 @@ const AP_Param::GroupInfo AP_Mission::var_info[] PROGMEM = { // @User: Advanced AP_GROUPINFO("TOTAL", 0, AP_Mission, _cmd_total, 0), + // @Param: AUTORESET + // @DisplayName: Controls whether to reset mission when switching to auto + // @Description: When set to 0 it will continue a previous auto mission, when set to 1 it will restart from the first waypoint + // @Values: 0:Continue Mission, 1:Reset Mission + AP_GROUPINFO("AUTORESET", 1, AP_Mission, _auto_reset, 0), + AP_GROUPEND }; @@ -100,6 +106,15 @@ void AP_Mission::resume() } } +void AP_Mission::start_or_resume() +{ + if (_auto_reset) { + start(); + } else { + resume(); + } +} + /// clear - clears out mission /// returns true if mission was running so it could not be cleared bool AP_Mission::clear() diff --git a/libraries/AP_Mission/AP_Mission.h b/libraries/AP_Mission/AP_Mission.h index b815679761..b0faa3d0fb 100644 --- a/libraries/AP_Mission/AP_Mission.h +++ b/libraries/AP_Mission/AP_Mission.h @@ -213,6 +213,9 @@ public: /// previous running commands will be re-initialised void resume(); + /// start_or_resume - if MIS_AUTORESTART=0 this will call resume(), otherwise it will call start() + void start_or_resume(); + /// clear - clears out mission /// returns true if mission was running so it could not be cleared bool clear(); @@ -355,7 +358,8 @@ private: const AP_AHRS& _ahrs; // used only for home position // parameters - AP_Int16 _cmd_total; // total number of commands in the mission + AP_Int16 _cmd_total; // total number of commands in the mission + AP_Int8 _auto_reset; // when true the mission will reset to the start when initiated // pointer to main program functions mission_cmd_fn_t _cmd_start_fn; // pointer to function which will be called when a new command is started