diff --git a/ArduCopter/commands_logic.pde b/ArduCopter/commands_logic.pde index 90ec232f7f..1ad8d66560 100644 --- a/ArduCopter/commands_logic.pde +++ b/ArduCopter/commands_logic.pde @@ -290,7 +290,7 @@ static void do_loiter_turns() wp_control = CIRCLE_MODE; // reset desired location - circle_angle = 0; + if(command_nav_queue.lat == 0){ // allow user to specify just the altitude @@ -305,6 +305,10 @@ static void do_loiter_turns() loiter_total = command_nav_queue.p1 * 360; loiter_sum = 0; old_target_bearing = target_bearing; + + circle_angle = target_bearing + 18000; + circle_angle = wrap_360(circle_angle); + circle_angle *= RADX100; } static void do_loiter_time()