mirror of https://github.com/ArduPilot/ardupilot
commands.pde: fixed unit error for distance check
This commit is contained in:
parent
7a5347fb1f
commit
efc74a87aa
|
@ -137,7 +137,7 @@ static void set_next_WP(struct Location *wp)
|
|||
if (next_WP.lat == 0 || command_nav_index <= 1){
|
||||
prev_WP = current_loc;
|
||||
}else{
|
||||
if (get_distance(¤t_loc, &next_WP) < 10)
|
||||
if (get_distance(¤t_loc, &next_WP) < 250)
|
||||
prev_WP = next_WP;
|
||||
else
|
||||
prev_WP = current_loc;
|
||||
|
@ -167,7 +167,6 @@ static void set_next_WP(struct Location *wp)
|
|||
// -----------------------------------
|
||||
wp_distance = get_distance(¤t_loc, &next_WP);
|
||||
target_bearing = get_bearing(&prev_WP, &next_WP);
|
||||
nav_bearing = target_bearing;
|
||||
|
||||
// calc the location error:
|
||||
calc_location_error(&next_WP);
|
||||
|
|
Loading…
Reference in New Issue