From b4d9397821e0e04c8d0199f859e7edf73462dabb Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 3 Apr 2017 11:05:14 +1000 Subject: [PATCH] Copter: re-arrange fast_loop for minimum latency this makes motor outputs as responsive as possible to gyros --- ArduCopter/ArduCopter.cpp | 14 +++++++++----- 1 file changed, 9 insertions(+), 5 deletions(-) diff --git a/ArduCopter/ArduCopter.cpp b/ArduCopter/ArduCopter.cpp index a289090a4f..ffa1e13cfc 100644 --- a/ArduCopter/ArduCopter.cpp +++ b/ArduCopter/ArduCopter.cpp @@ -255,10 +255,16 @@ void Copter::loop() // Main loop - 400hz void Copter::fast_loop() { + // update INS immediately to get current gyro data populated + ins.update(); + // run low level rate controllers that only require IMU data attitude_control->rate_controller_run(); - // IMU DCM Algorithm + // send outputs to the motors library immediately + motors_output(); + + // run EKF state estimator (expensive) // -------------------- read_AHRS(); @@ -266,9 +272,6 @@ void Copter::fast_loop() update_heli_control_dynamics(); #endif //HELI_FRAME - // send outputs to the motors library - motors_output(); - // Inertial Nav // -------------------- read_inertia(); @@ -626,7 +629,8 @@ void Copter::read_AHRS(void) gcs_check_input(); #endif - ahrs.update(); + // we tell AHRS to skip INS update as we have already done it in fast_loop() + ahrs.update(true); } // read baro and rangefinder altitude at 10hz