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); } /*