|
|
@ -329,10 +329,10 @@ void AC_AttitudeControl_Multi::rate_controller_run() |
|
|
|
|
|
|
|
|
|
|
|
Vector3f gyro_latest = _ahrs.get_gyro_latest(); |
|
|
|
Vector3f gyro_latest = _ahrs.get_gyro_latest(); |
|
|
|
|
|
|
|
|
|
|
|
_motors.set_roll(get_rate_roll_pid().update_all(_rate_target_ang_vel.x, gyro_latest.x, _motors.limit.roll_pitch)); |
|
|
|
_motors.set_roll(get_rate_roll_pid().update_all(_rate_target_ang_vel.x, gyro_latest.x, _motors.limit.roll)); |
|
|
|
_motors.set_roll_ff(get_rate_roll_pid().get_ff()); |
|
|
|
_motors.set_roll_ff(get_rate_roll_pid().get_ff()); |
|
|
|
|
|
|
|
|
|
|
|
_motors.set_pitch(get_rate_pitch_pid().update_all(_rate_target_ang_vel.y, gyro_latest.y, _motors.limit.roll_pitch)); |
|
|
|
_motors.set_pitch(get_rate_pitch_pid().update_all(_rate_target_ang_vel.y, gyro_latest.y, _motors.limit.pitch)); |
|
|
|
_motors.set_pitch_ff(get_rate_pitch_pid().get_ff()); |
|
|
|
_motors.set_pitch_ff(get_rate_pitch_pid().get_ff()); |
|
|
|
|
|
|
|
|
|
|
|
_motors.set_yaw(get_rate_yaw_pid().update_all(_rate_target_ang_vel.z, gyro_latest.z, _motors.limit.yaw)); |
|
|
|
_motors.set_yaw(get_rate_yaw_pid().update_all(_rate_target_ang_vel.z, gyro_latest.z, _motors.limit.yaw)); |
|
|
|