Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

SITL: limit rotational rate to 2000dps

This commit is contained in:
Andrew Tridgell 2016-03-26 14:23:05 +11:00
parent 4d01cb1716
commit 73a2c99d56
1 changed files with 5 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/SITL/SIM_Aircraft.cpp
View File

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