From b9128a932f60e69b9e4590657034dd56ea49e776 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 2 Jan 2014 22:05:41 +1100 Subject: [PATCH] AP_L1_Control: no need to project position for lag now handled by AHRS --- libraries/AP_L1_Control/AP_L1_Control.cpp | 10 ---------- 1 file changed, 10 deletions(-) diff --git a/libraries/AP_L1_Control/AP_L1_Control.cpp b/libraries/AP_L1_Control/AP_L1_Control.cpp index d10c9077f6..6466f1169d 100644 --- a/libraries/AP_L1_Control/AP_L1_Control.cpp +++ b/libraries/AP_L1_Control/AP_L1_Control.cpp @@ -124,11 +124,6 @@ void AP_L1_Control::update_waypoint(const struct Location &prev_WP, const struct 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); @@ -229,11 +224,6 @@ void AP_L1_Control::update_loiter(const struct Location ¢er_WP, float radius 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);