From 77f007ab27a9d28fe2aac3e9d692c49ba79d30d2 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Sat, 2 May 2015 21:41:33 +1000
Subject: [PATCH] SITL: support speedup setting on the command line

---
 libraries/SITL/SIM_Aircraft.cpp    | 14 ++++++++++++--
 libraries/SITL/SIM_Aircraft.h      |  6 +++++-
 libraries/SITL/SIM_Multicopter.cpp |  2 --
 3 files changed, 17 insertions(+), 5 deletions(-)

diff --git a/libraries/SITL/SIM_Aircraft.cpp b/libraries/SITL/SIM_Aircraft.cpp
index a522f9a901..8f922bafba 100644
--- a/libraries/SITL/SIM_Aircraft.cpp
+++ b/libraries/SITL/SIM_Aircraft.cpp
@@ -38,11 +38,11 @@ Aircraft::Aircraft(const char *home_str) :
     velocity_ef(),
     velocity_body(),
     mass(0),
-    update_frequency(50),
     accel_body(0, 0, -GRAVITY_MSS),
     time_now_us(0),
     gyro_noise(radians(0.1f)),
-    accel_noise(0.3)
+    accel_noise(0.3),
+    rate_hz(400)
 {
     char *saveptr=NULL;
     char *s = strdup(home_str);
@@ -60,6 +60,8 @@ Aircraft::Aircraft(const char *home_str) :
 
     dcm.from_euler(0, 0, atof(yaw_s));
     free(s);
+
+    set_speedup(1);
 }
 
 /* 
@@ -235,3 +237,11 @@ uint64_t Aircraft::get_wall_time_us() const
 	gettimeofday(&tp,NULL);
 	return tp.tv_sec*1.0e6 + tp.tv_usec;
 }
+
+/*
+  set simulation speedup
+ */
+void Aircraft::set_speedup(float speedup)
+{
+    setup_frame_time(rate_hz, speedup);
+}
diff --git a/libraries/SITL/SIM_Aircraft.h b/libraries/SITL/SIM_Aircraft.h
index 61e4d22a29..71ff5abd0f 100644
--- a/libraries/SITL/SIM_Aircraft.h
+++ b/libraries/SITL/SIM_Aircraft.h
@@ -40,6 +40,11 @@ public:
         uint16_t servos[16];
     };
 
+    /*
+      set simulation speedup
+     */
+    void set_speedup(float speedup);
+
     /*
       step the FDM by one time step
      */
@@ -60,7 +65,6 @@ protected:
     Vector3f velocity_body; // m/s, body frame
     Vector3f position; // meters, NED from origin
     float mass; // kg
-    float update_frequency;
     Vector3f accel_body; // m/s/s NED, body frame
 
     uint64_t time_now_us;
diff --git a/libraries/SITL/SIM_Multicopter.cpp b/libraries/SITL/SIM_Multicopter.cpp
index 1994957521..704670f579 100644
--- a/libraries/SITL/SIM_Multicopter.cpp
+++ b/libraries/SITL/SIM_Multicopter.cpp
@@ -122,8 +122,6 @@ MultiCopter::MultiCopter(const char *home_str, const char *frame_str) :
     thrust_scale = (mass * GRAVITY_MSS) / (frame->num_motors * hover_throttle);
 
     frame_height = 0.1;
-
-    setup_frame_time(400, 3);
 }
 
 /*