From fd4bceaafe03fa8da4e77231d0a8d27fd4fb1cd0 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 12 Aug 2013 13:15:02 +1000 Subject: [PATCH] AP_L1_Control: avoid using float global coordinates this increases the navigation precision of rovers using L1 control down to the centimeter level --- libraries/AP_L1_Control/AP_L1_Control.cpp | 19 +++++-------------- 1 file changed, 5 insertions(+), 14 deletions(-) diff --git a/libraries/AP_L1_Control/AP_L1_Control.cpp b/libraries/AP_L1_Control/AP_L1_Control.cpp index cf25a4c6168..102c96c5ed5 100644 --- a/libraries/AP_L1_Control/AP_L1_Control.cpp +++ b/libraries/AP_L1_Control/AP_L1_Control.cpp @@ -118,23 +118,18 @@ void AP_L1_Control::update_waypoint(const struct Location &prev_WP, const struct // 0.3183099 = 1/1/pipi _L1_dist = 0.3183099f * _L1_damping * _L1_period * groundSpeed; - //Convert current location and WP positions to 2D vectors in lat and long - Vector2f A_air((_current_loc.lat*1.0e-7f), (_current_loc.lng*1.0e-7f)); - Vector2f A_v((prev_WP.lat*1.0e-7f), (prev_WP.lng*1.0e-7f)); - Vector2f B_v((next_WP.lat*1.0e-7f), (next_WP.lng*1.0e-7f)); - // Calculate the NE position of WP B relative to WP A - Vector2f AB = _geo2planar(A_v, B_v); + Vector2f AB = location_diff(prev_WP, next_WP); // Check for AB zero length and track directly to the destination // if too small if (AB.length() < 1.0e-6f) { - AB = _geo2planar(A_air, B_v); + AB = location_diff(_current_loc, next_WP); } AB.normalize(); // Calculate the NE position of the aircraft relative to WP A - A_air = _geo2planar(A_v, A_air); + Vector2f A_air = location_diff(prev_WP, _current_loc); // calculate distance to target track, for reporting _crosstrack_error = AB % A_air; @@ -212,15 +207,11 @@ void AP_L1_Control::update_loiter(const struct Location ¢er_WP, float radius // 0.3183099 = 1/pi _L1_dist = 0.3183099f * _L1_damping * _L1_period * groundSpeed; - //Convert current location and WP positionsto 2D vectors in lat and long - Vector2f A_air((_current_loc.lat*1.0e-7f), (_current_loc.lng*1.0e-7f)); - Vector2f A_v((center_WP.lat*1.0e-7f), (center_WP.lng*1.0e-7f)); - //Calculate the NE position of the aircraft relative to WP A - A_air = _geo2planar(A_v, A_air); + Vector2f A_air = location_diff(center_WP, _current_loc); //Calculate the unit vector from WP A to aircraft - Vector2f A_air_unit = (A_air).normalized(); + Vector2f A_air_unit = A_air.normalized(); //Calculate Nu to capture center_WP float xtrackVelCap = A_air_unit % _groundspeed_vector; // Velocity across line - perpendicular to radial inbound to WP