diff --git a/libraries/AP_HAL_SITL/SITL_cmdline.cpp b/libraries/AP_HAL_SITL/SITL_cmdline.cpp index 4d5d7e12e7..a3cec5f788 100644 --- a/libraries/AP_HAL_SITL/SITL_cmdline.cpp +++ b/libraries/AP_HAL_SITL/SITL_cmdline.cpp @@ -37,6 +37,8 @@ #include #include +#include +#include extern const AP_HAL::HAL& hal; @@ -94,6 +96,7 @@ void SITL_State::_usage(void) "\t--sim-port-in PORT set port num for simulator in\n" "\t--sim-port-out PORT set port num for simulator out\n" "\t--irlock-port PORT set port num for irlock\n" + "\t--start-time TIMESTR set simulation start time in UNIX timestamp" ); } @@ -198,6 +201,12 @@ void SITL_State::_parse_command_line(int argc, char * const argv[]) uint16_t simulator_port_out = SIM_OUT_PORT; _irlock_port = IRLOCK_PORT; + // Set default start time to the real system time. + // This will be overwritten if argument provided. + static struct timeval first_tv; + gettimeofday(&first_tv, nullptr); + time_t start_time_UTC = first_tv.tv_sec; + enum long_options { CMDLINE_GIMBAL = 1, CMDLINE_FGVIEW, @@ -218,6 +227,7 @@ void SITL_State::_parse_command_line(int argc, char * const argv[]) CMDLINE_SIM_PORT_IN, CMDLINE_SIM_PORT_OUT, CMDLINE_IRLOCK_PORT, + CMDLINE_START_TIME, }; const struct GetOptLong::option options[] = { @@ -253,6 +263,7 @@ void SITL_State::_parse_command_line(int argc, char * const argv[]) {"sim-port-in", true, 0, CMDLINE_SIM_PORT_IN}, {"sim-port-out", true, 0, CMDLINE_SIM_PORT_OUT}, {"irlock-port", true, 0, CMDLINE_IRLOCK_PORT}, + {"start-time", true, 0, CMDLINE_START_TIME}, {0, false, 0, 0} }; @@ -371,6 +382,9 @@ void SITL_State::_parse_command_line(int argc, char * const argv[]) case CMDLINE_IRLOCK_PORT: _irlock_port = atoi(gopt.optarg); break; + case CMDLINE_START_TIME: + start_time_UTC = atoi(gopt.optarg); + break; default: _usage(); exit(1); @@ -415,6 +429,9 @@ void SITL_State::_parse_command_line(int argc, char * const argv[]) exit(1); } + // Set SITL start time. + AP::sitl()->start_time_UTC = start_time_UTC; + fprintf(stdout, "Starting sketch '%s'\n", SKETCH); if (strcmp(SKETCH, "ArduCopter") == 0) { diff --git a/libraries/AP_HAL_SITL/sitl_gps.cpp b/libraries/AP_HAL_SITL/sitl_gps.cpp index 95e8f65eae..8e137b80d9 100644 --- a/libraries/AP_HAL_SITL/sitl_gps.cpp +++ b/libraries/AP_HAL_SITL/sitl_gps.cpp @@ -116,7 +116,7 @@ static void simulation_timeval(struct timeval *tv) static struct timeval first_tv; if (first_usec == 0) { first_usec = now; - gettimeofday(&first_tv, nullptr); + first_tv.tv_sec = AP::sitl()->start_time_UTC; } *tv = first_tv; tv->tv_sec += now / 1000000ULL; diff --git a/libraries/SITL/SITL.h b/libraries/SITL/SITL.h index ef350ea086..c769c6a118 100644 --- a/libraries/SITL/SITL.h +++ b/libraries/SITL/SITL.h @@ -345,6 +345,8 @@ public: uint16_t irlock_port; + time_t start_time_UTC; + void simstate_send(mavlink_channel_t chan); void Log_Write_SIMSTATE();