mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-11 02:18:29 -04:00
Loiter I reset
This commit is contained in:
parent
0ace8474d7
commit
1ef7f9fe62
@ -1391,8 +1391,6 @@ static void update_navigation()
|
||||
// reset LOITER to current position
|
||||
next_WP = current_loc;
|
||||
|
||||
// clear Loiter Iterms
|
||||
//reset_nav();
|
||||
}else{
|
||||
// this is also set by GPS in update_nav
|
||||
wp_control = LOITER_MODE;
|
||||
|
@ -239,12 +239,12 @@ static void reset_nav(void)
|
||||
nav_throttle = 0;
|
||||
invalid_throttle = true;
|
||||
|
||||
//g.pi_nav_lat.reset_I();
|
||||
//g.pi_nav_lon.reset_I();
|
||||
g.pi_nav_lat.reset_I();
|
||||
g.pi_nav_lon.reset_I();
|
||||
|
||||
// considering not reseting wind control
|
||||
//g.pi_loiter_lat.reset_I();
|
||||
//g.pi_loiter_lon.reset_I();
|
||||
g.pi_loiter_lat.reset_I();
|
||||
g.pi_loiter_lon.reset_I();
|
||||
|
||||
circle_angle = 0;
|
||||
crosstrack_error = 0;
|
||||
|
Loading…
Reference in New Issue
Block a user