Copter: run rate controllers before AHRS/EKF update

This reduces lag between IMU updates and motor outputs by 0.6 milliseconds
This commit is contained in:
Randy Mackay 2017-03-02 20:50:29 +09:00
parent 67097c8d59
commit ca4cbaeb39
1 changed files with 2 additions and 3 deletions

View File

@ -255,14 +255,13 @@ void Copter::loop()
// Main loop - 400hz
void Copter::fast_loop()
{
// run low level rate controllers that only require IMU data
attitude_control->rate_controller_run();
// IMU DCM Algorithm
// --------------------
read_AHRS();
// run low level rate controllers that only require IMU data
attitude_control->rate_controller_run();
#if FRAME_CONFIG == HELI_FRAME
update_heli_control_dynamics();
#endif //HELI_FRAME