Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

SITL: limit rotational rate to 2000dps

Browse Source
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) void Aircraft::update_dynamics(const Vector3f &rot_accel)
{ {
float delta_time = frame_time_us * 1.0e-6f; float delta_time = frame_time_us * 1.0e-6f;
// update rotational rates in body frame // update rotational rates in body frame
gyro += rot_accel * delta_time; 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 // update attitude
dcm.rotate(gyro * delta_time); dcm.rotate(gyro * delta_time);
dcm.normalize(); dcm.normalize();