From 73a2c99d5695ab5874ea6b83704aaf1384b05116 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 26 Mar 2016 14:23:05 +1100 Subject: [PATCH] SITL: limit rotational rate to 2000dps --- libraries/SITL/SIM_Aircraft.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/libraries/SITL/SIM_Aircraft.cpp b/libraries/SITL/SIM_Aircraft.cpp index 96ab199d4c..9b90bfbc5f 100644 --- a/libraries/SITL/SIM_Aircraft.cpp +++ b/libraries/SITL/SIM_Aircraft.cpp @@ -328,10 +328,14 @@ void Aircraft::set_speedup(float speedup) void Aircraft::update_dynamics(const Vector3f &rot_accel) { float delta_time = frame_time_us * 1.0e-6f; - + // update rotational rates in body frame gyro += rot_accel * delta_time; + gyro.x = constrain_float(gyro.x, -radians(2000), radians(2000)); + gyro.y = constrain_float(gyro.y, -radians(2000), radians(2000)); + gyro.z = constrain_float(gyro.z, -radians(2000), radians(2000)); + // update attitude dcm.rotate(gyro * delta_time); dcm.normalize();