mirror of https://github.com/ArduPilot/ardupilot
Circle_WP distance check fix
This commit is contained in:
parent
d448ea23e6
commit
60cdbe771c
|
@ -2294,7 +2294,7 @@ static void update_nav_wp()
|
||||||
next_WP.lat = circle_WP.lat + (g.loiter_radius * 100 * sin(1.57 - circle_angle));
|
next_WP.lat = circle_WP.lat + (g.loiter_radius * 100 * sin(1.57 - circle_angle));
|
||||||
|
|
||||||
// calc the lat and long error to the target
|
// calc the lat and long error to the target
|
||||||
calc_location_error(&circle_WP);
|
calc_location_error(&next_WP);
|
||||||
|
|
||||||
// use error as the desired rate towards the target
|
// use error as the desired rate towards the target
|
||||||
// nav_lon, nav_lat is calculated
|
// nav_lon, nav_lat is calculated
|
||||||
|
|
Loading…
Reference in New Issue