diff --git a/libraries/AP_L1_Control/AP_L1_Control.cpp b/libraries/AP_L1_Control/AP_L1_Control.cpp index fb2c609b9b..6faed10461 100644 --- a/libraries/AP_L1_Control/AP_L1_Control.cpp +++ b/libraries/AP_L1_Control/AP_L1_Control.cpp @@ -241,13 +241,13 @@ void AP_L1_Control::update_waypoint(const struct Location &prev_WP, const struct _L1_dist = MAX(0.3183099f * _L1_damping * _L1_period * groundSpeed, dist_min); // Calculate the NE position of WP B relative to WP A - Vector2f AB = location_diff(prev_WP, next_WP); + Vector2f AB = prev_WP.get_distance_NE(next_WP); float AB_length = AB.length(); // Check for AB zero length and track directly to the destination // if too small if (AB.length() < 1.0e-6f) { - AB = location_diff(_current_loc, next_WP); + AB = _current_loc.get_distance_NE(next_WP); if (AB.length() < 1.0e-6f) { AB = Vector2f(cosf(get_yaw()), sinf(get_yaw())); } @@ -255,7 +255,7 @@ void AP_L1_Control::update_waypoint(const struct Location &prev_WP, const struct AB.normalize(); // Calculate the NE position of the aircraft relative to WP A - Vector2f A_air = location_diff(prev_WP, _current_loc); + const Vector2f A_air = prev_WP.get_distance_NE(_current_loc); // calculate distance to target track, for reporting _crosstrack_error = A_air % AB; @@ -276,7 +276,7 @@ void AP_L1_Control::update_waypoint(const struct Location &prev_WP, const struct } else if (alongTrackDist > AB_length + groundSpeed*3) { // we have passed point B by 3 seconds. Head towards B // Calc Nu to fly To WP B - Vector2f B_air = location_diff(next_WP, _current_loc); + const Vector2f B_air = next_WP.get_distance_NE(_current_loc); Vector2f B_air_unit = (B_air).normalized(); // Unit vector from WP B to aircraft xtrackVel = _groundspeed_vector % (-B_air_unit); // Velocity across line ltrackVel = _groundspeed_vector * (-B_air_unit); // Velocity along line @@ -369,7 +369,7 @@ void AP_L1_Control::update_loiter(const struct Location ¢er_WP, float radius _L1_dist = 0.3183099f * _L1_damping * _L1_period * groundSpeed; //Calculate the NE position of the aircraft relative to WP A - Vector2f A_air = location_diff(center_WP, _current_loc); + const Vector2f A_air = center_WP.get_distance_NE(_current_loc); // Calculate the unit vector from WP A to aircraft // protect against being on the waypoint and having zero velocity