diff --git a/libraries/AP_HAL_AVR_SITL/SITL_State.cpp b/libraries/AP_HAL_AVR_SITL/SITL_State.cpp index 094fec63eb..643f26465f 100644 --- a/libraries/AP_HAL_AVR_SITL/SITL_State.cpp +++ b/libraries/AP_HAL_AVR_SITL/SITL_State.cpp @@ -103,7 +103,7 @@ void SITL_State::_parse_command_line(int argc, char * const argv[]) setvbuf(stdout, (char *)0, _IONBF, 0); setvbuf(stderr, (char *)0, _IONBF, 0); - while ((opt = getopt(argc, argv, "swhr:H:CI:")) != -1) { + while ((opt = getopt(argc, argv, "swhr:H:CI:P:")) != -1) { switch (opt) { case 'w': AP_Param::erase_all(); @@ -125,6 +125,9 @@ void SITL_State::_parse_command_line(int argc, char * const argv[]) _simin_port += instance * 10; } break; + case 'P': + _set_param_default(optarg); + break; default: _usage(); exit(1); @@ -156,6 +159,37 @@ void SITL_State::_parse_command_line(int argc, char * const argv[]) } +void SITL_State::_set_param_default(char *parm) +{ + char *p = strchr(parm, '='); + if (p == NULL) { + printf("Please specify parameter as NAME=VALUE"); + exit(1); + } + float value = atof(p+1); + *p = 0; + enum ap_var_type var_type; + AP_Param *vp = AP_Param::find(parm, &var_type); + if (vp == NULL) { + printf("Unknown parameter %s\n", parm); + exit(1); + } + if (var_type == AP_PARAM_FLOAT) { + ((AP_Float *)vp)->set_and_save(value); + } else if (var_type == AP_PARAM_INT32) { + ((AP_Int32 *)vp)->set_and_save(value); + } else if (var_type == AP_PARAM_INT16) { + ((AP_Int16 *)vp)->set_and_save(value); + } else if (var_type == AP_PARAM_INT8) { + ((AP_Int8 *)vp)->set_and_save(value); + } else { + printf("Unable to set parameter %s\n", parm); + exit(1); + } + printf("Set parameter %s to %f\n", parm, value); +} + + /* setup for SITL handling */ diff --git a/libraries/AP_HAL_AVR_SITL/SITL_State.h b/libraries/AP_HAL_AVR_SITL/SITL_State.h index a35f2050e4..aa2d146693 100644 --- a/libraries/AP_HAL_AVR_SITL/SITL_State.h +++ b/libraries/AP_HAL_AVR_SITL/SITL_State.h @@ -51,6 +51,7 @@ public: private: void _parse_command_line(int argc, char * const argv[]); + void _set_param_default(char *parm); void _usage(void); void _sitl_setup(void); void _setup_fdm(void);