diff --git a/ArduPlane/commands_logic.pde b/ArduPlane/commands_logic.pde index 3d72c711bba7e378a3d00b7ac00a61c88c4a7aaa..fefb87577d412e21e0f4589420248af862837908 100644 --- a/ArduPlane/commands_logic.pde +++ b/ArduPlane/commands_logic.pde @@ -514,8 +514,8 @@ static bool verify_continue_and_change_alt() return true; } - //Is the next_WP less than 200 m away? - if (get_distance(prev_WP_loc, next_WP_loc) < 200.f) { + // Is the next_WP less than 200 m away? + if (get_distance(current_loc, next_WP_loc) < 200.f) { //push another 300 m down the line int32_t next_wp_bearing_cd = get_bearing_cd(prev_WP_loc, next_WP_loc); location_update(next_WP_loc, next_wp_bearing_cd * 0.01f, 300.f);