From 48919b625354ff6283944759c4959429ef709637 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 11 May 2014 21:39:06 +1000 Subject: [PATCH] Rover: use EKF velocity for high rate ground_speed updates this may give smoother throttle response when the EKF is enabled --- APMrover2/APMrover2.pde | 13 ++++++++++++- 1 file changed, 12 insertions(+), 1 deletion(-) diff --git a/APMrover2/APMrover2.pde b/APMrover2/APMrover2.pde index 5f16a423cc..487a7796f8 100644 --- a/APMrover2/APMrover2.pde +++ b/APMrover2/APMrover2.pde @@ -623,6 +623,12 @@ static void ahrs_update() ahrs.update(); + // if using the EKF get a speed update now (from accelerometers) + Vector3f velocity; + if (ahrs.get_velocity_NED(velocity)) { + ground_speed = pythagorous2(velocity.x, velocity.y); + } + if (should_log(MASK_LOG_ATTITUDE_FAST)) Log_Write_Attitude(); @@ -824,7 +830,12 @@ static void update_GPS_10Hz(void) ground_start_count = 0; } } - ground_speed = gps.ground_speed(); + Vector3f velocity; + if (ahrs.get_velocity_NED(velocity)) { + ground_speed = pythagorous2(velocity.x, velocity.y); + } else { + ground_speed = gps.ground_speed(); + } #if CAMERA == ENABLED if (camera.update_location(current_loc) == true) {