diff --git a/libraries/AP_HAL_SITL/HAL_SITL_Class.cpp b/libraries/AP_HAL_SITL/HAL_SITL_Class.cpp index 8c75ed2735..8d7cd1e21a 100644 --- a/libraries/AP_HAL_SITL/HAL_SITL_Class.cpp +++ b/libraries/AP_HAL_SITL/HAL_SITL_Class.cpp @@ -193,6 +193,13 @@ uint8_t HAL_SITL::get_instance() const return _sitl_state->get_instance(); } +#if defined(HAL_BUILD_AP_PERIPH) +bool HAL_SITL::run_in_maintenance_mode() const +{ + return _sitl_state->run_in_maintenance_mode(); +} +#endif + void HAL_SITL::run(int argc, char * const argv[], Callbacks* callbacks) const { assert(callbacks); diff --git a/libraries/AP_HAL_SITL/HAL_SITL_Class.h b/libraries/AP_HAL_SITL/HAL_SITL_Class.h index eaa18f58ac..2f3a890b64 100644 --- a/libraries/AP_HAL_SITL/HAL_SITL_Class.h +++ b/libraries/AP_HAL_SITL/HAL_SITL_Class.h @@ -37,6 +37,10 @@ public: uint8_t get_instance() const; +#if defined(HAL_BUILD_AP_PERIPH) + bool run_in_maintenance_mode() const; +#endif + private: HALSITL::SITL_State *_sitl_state; diff --git a/libraries/AP_HAL_SITL/SITL_Periph_State.cpp b/libraries/AP_HAL_SITL/SITL_Periph_State.cpp index 521bf17b00..ce0a51809b 100644 --- a/libraries/AP_HAL_SITL/SITL_Periph_State.cpp +++ b/libraries/AP_HAL_SITL/SITL_Periph_State.cpp @@ -29,12 +29,13 @@ void SITL_State::init(int argc, char * const argv[]) { const struct GetOptLong::option options[] = { {"help", false, 0, 'h'}, {"instance", true, 0, 'I'}, + {"maintenance", false, 0, 'M'}, }; setvbuf(stdout, (char *)0, _IONBF, 0); setvbuf(stderr, (char *)0, _IONBF, 0); - GetOptLong gopt(argc, argv, "hI:", + GetOptLong gopt(argc, argv, "hI:M", options); while((opt = gopt.getoption()) != -1) { @@ -42,10 +43,15 @@ void SITL_State::init(int argc, char * const argv[]) { case 'I': _instance = atoi(gopt.optarg); break; + case 'M': + printf("Running in Maintenance Mode\n"); + _maintenance = true; + break; default: printf("Options:\n" "\t--help|-h display this help information\n" - "\t--instance|-I N set instance of SITL Periph\n"); + "\t--instance|-I N set instance of SITL Periph\n" + "\t--maintenance|-M run in maintenance mode\n"); exit(1); } } diff --git a/libraries/AP_HAL_SITL/SITL_Periph_State.h b/libraries/AP_HAL_SITL/SITL_Periph_State.h index 0c9a790053..222abf54d4 100644 --- a/libraries/AP_HAL_SITL/SITL_Periph_State.h +++ b/libraries/AP_HAL_SITL/SITL_Periph_State.h @@ -61,6 +61,8 @@ public: uint8_t get_instance() const { return _instance; } + bool run_in_maintenance_mode() const { return _maintenance; } + SITL::SerialDevice *create_serial_sim(const char *name, const char *arg) { return nullptr; } @@ -74,6 +76,7 @@ private: const char *defaults_path = HAL_PARAM_DEFAULTS_PATH; uint8_t _instance; + bool _maintenance; }; #endif