|
|
|
@ -332,6 +332,10 @@ void Aircraft::update_dynamics(const Vector3f &rot_accel)
@@ -332,6 +332,10 @@ void Aircraft::update_dynamics(const Vector3f &rot_accel)
|
|
|
|
|
// 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(); |
|
|
|
|