Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_L1_Control: use get_distance_NE instead of location_diff

This commit is contained in:
Pierre Kancir 2019-04-08 15:16:19 +02:00 committed by Tom Pittenger
parent 4ea10ce5f5
commit 25507d4d7b
1 changed files with 5 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
libraries/AP_L1_Control/AP_L1_Control.cpp
View File

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