|
|
@ -185,7 +185,7 @@ void Plane::update_loiter(uint16_t radius) |
|
|
|
} else if (loiter.start_time_ms == 0 && |
|
|
|
} else if (loiter.start_time_ms == 0 && |
|
|
|
control_mode == AUTO && |
|
|
|
control_mode == AUTO && |
|
|
|
!auto_state.no_crosstrack && |
|
|
|
!auto_state.no_crosstrack && |
|
|
|
get_distance(current_loc, next_WP_loc) > radius*2) { |
|
|
|
get_distance(current_loc, next_WP_loc) > radius*3) { |
|
|
|
// if never reached loiter point and using crosstrack and somewhat far away from loiter point
|
|
|
|
// if never reached loiter point and using crosstrack and somewhat far away from loiter point
|
|
|
|
// navigate to it like in auto-mode for normal crosstrack behavior
|
|
|
|
// navigate to it like in auto-mode for normal crosstrack behavior
|
|
|
|
nav_controller->update_waypoint(prev_WP_loc, next_WP_loc); |
|
|
|
nav_controller->update_waypoint(prev_WP_loc, next_WP_loc); |
|
|
|