mirror of https://github.com/ArduPilot/ardupilot
reversed circle_WP calculation so the next_WP refers to the destination around the circle and circle_WP refers to the center. The intent is to make it easier to see via ground station.
This commit is contained in:
parent
56335e81e4
commit
5754f97433
|
@ -2226,8 +2226,8 @@ static void update_nav_wp()
|
||||||
if (circle_angle > 6.28318531)
|
if (circle_angle > 6.28318531)
|
||||||
circle_angle -= 6.28318531;
|
circle_angle -= 6.28318531;
|
||||||
|
|
||||||
circle_WP.lng = next_WP.lng + (g.loiter_radius * 100 * cos(1.57 - circle_angle) * scaleLongUp);
|
next_WP.lng = circle_WP.lng + (g.loiter_radius * 100 * cos(1.57 - circle_angle) * scaleLongUp);
|
||||||
circle_WP.lat = next_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(&circle_WP);
|
||||||
|
|
|
@ -453,6 +453,7 @@ static void set_mode(byte mode)
|
||||||
roll_pitch_mode = CIRCLE_RP;
|
roll_pitch_mode = CIRCLE_RP;
|
||||||
throttle_mode = CIRCLE_THR;
|
throttle_mode = CIRCLE_THR;
|
||||||
set_next_WP(¤t_loc);
|
set_next_WP(¤t_loc);
|
||||||
|
circle_WP = next_WP;
|
||||||
circle_angle = 0;
|
circle_angle = 0;
|
||||||
break;
|
break;
|
||||||
|
|
||||||
|
|
Loading…
Reference in New Issue