From 653fa5a191fda301bcb61d9102001658af793ca7 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 12 Aug 2013 13:26:40 +1000 Subject: [PATCH] AP_L1_Control: adjust position for GPS lag this should help for rovers --- libraries/AP_L1_Control/AP_L1_Control.cpp | 26 +++++++++++++++++------ 1 file changed, 19 insertions(+), 7 deletions(-) diff --git a/libraries/AP_L1_Control/AP_L1_Control.cpp b/libraries/AP_L1_Control/AP_L1_Control.cpp index 102c96c5ed..cd9a9c6c86 100644 --- a/libraries/AP_L1_Control/AP_L1_Control.cpp +++ b/libraries/AP_L1_Control/AP_L1_Control.cpp @@ -99,10 +99,15 @@ void AP_L1_Control::update_waypoint(const struct Location &prev_WP, const struct // Get current position and velocity _ahrs->get_projected_position(_current_loc); + Vector2f _groundspeed_vector = _ahrs->groundspeed_vector(); + + // update the position for lag. This helps especially for rovers + // where waypoints may be very close together + Vector2f lag_offset = _groundspeed_vector * _ahrs->get_position_lag(); + location_offset(_current_loc, lag_offset.x, lag_offset.y); + // update _target_bearing_cd _target_bearing_cd = get_bearing_cd(_current_loc, next_WP); - - Vector2f _groundspeed_vector = _ahrs->groundspeed_vector(); //Calculate groundspeed float groundSpeed = _groundspeed_vector.length(); @@ -192,15 +197,22 @@ void AP_L1_Control::update_loiter(const struct Location ¢er_WP, float radius float K_L1 = 4.0f * _L1_damping * _L1_damping; //Get current position and velocity - _ahrs->get_projected_position(_current_loc); + _ahrs->get_position(_current_loc); + + Vector2f _groundspeed_vector = _ahrs->groundspeed_vector(); + + // update the position for lag. This helps especially for rovers + // where waypoints may be very close together + Vector2f lag_offset = _groundspeed_vector * _ahrs->get_position_lag(); + location_offset(_current_loc, lag_offset.x, lag_offset.y); + + //Calculate groundspeed + float groundSpeed = max(_groundspeed_vector.length() , 1.0f); + // update _target_bearing_cd _target_bearing_cd = get_bearing_cd(_current_loc, center_WP); - Vector2f _groundspeed_vector = _ahrs->groundspeed_vector(); - - //Calculate groundspeed - float groundSpeed = _maxf(_groundspeed_vector.length() , 1.0f); // Calculate time varying control parameters // Calculate the L1 length required for specified period