SITL: Add start time argument.

libraries/AP_HAL_SITL/SITL_cmdline.cpp

@@ -37,6 +37,8 @@
 
 #include <signal.h>
 #include <stdio.h>
+#include <time.h>
+#include <sys/time.h>
 
 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) {

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;

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();