Replay: added -A option to set arming time

Tools/Replay/Replay.pde

@@ -85,6 +85,7 @@ static bool done_parameters;
 static bool done_baro_init;
 static bool done_home_init;
 static uint16_t update_rate;
+static uint32_t arm_time_ms;
 
 static uint8_t num_user_parameters;
 static struct {
@@ -100,6 +101,7 @@ static void usage(void)
     ::printf(" -pNAME=VALUE set parameter NAME to VALUE\n");
     ::printf(" -aMASK     set accel mask (1=accel1 only, 2=accel2 only, 3=both)\n");
     ::printf(" -gMASK     set gyro mask (1=gyro1 only, 2=gyro2 only, 3=both)\n");
+    ::printf(" -A time    arm at time milliseconds)\n");
 }
 
 void setup()
@@ -113,7 +115,7 @@ void setup()
 
     hal.util->commandline_arguments(argc, argv);
 
-	while ((opt = getopt(argc, argv, "r:p:ha:g:")) != -1) {
+	while ((opt = getopt(argc, argv, "r:p:ha:g:A:")) != -1) {
 		switch (opt) {
         case 'h':
             usage();
@@ -131,6 +133,10 @@ void setup()
             LogReader.set_accel_mask(strtol(optarg, NULL, 0));
             break;
 
+        case 'A':
+            arm_time_ms = strtoul(optarg, NULL, 0);
+            break;
+
         case 'p':
             char *eq = strchr(optarg, '=');
             if (eq == NULL) {
@@ -177,7 +183,11 @@ void setup()
     ahrs.set_fly_forward(true);
     ahrs.set_wind_estimation(true);
     ahrs.set_correct_centrifugal(true);
-    
+
+    if (arm_time_ms != 0) {
+        ahrs.set_armed(false);
+    }
+
     barometer.init();
     barometer.setHIL(0);
     barometer.read();
@@ -301,6 +311,14 @@ void loop()
 {
     while (true) {
         uint8_t type;
+
+        if (arm_time_ms != 0 && hal.scheduler->millis() > arm_time_ms) {
+            if (!ahrs.get_armed()) {
+                ahrs.set_armed(true);
+                ::printf("Arming at %u ms\n", (unsigned)hal.scheduler->millis());
+            }
+        }
+
         if (!LogReader.update(type)) {
             ::printf("End of log at %.1f seconds\n", hal.scheduler->millis()*0.001f);
             fclose(plotf);