AP_NavEKF: Improved control of fusion update intervals