mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-12 02:48:28 -04:00
made the crosstrack use the previous WP instead of the current loc to get a better line in AP.
This commit is contained in:
parent
82adf1df6b
commit
62ed256fca
@ -146,7 +146,17 @@ static void set_next_WP(struct Location *wp)
|
|||||||
|
|
||||||
// copy the current WP into the OldWP slot
|
// copy the current WP into the OldWP slot
|
||||||
// ---------------------------------------
|
// ---------------------------------------
|
||||||
prev_WP = next_WP;
|
if (next_WP.lat == 0 || command_nav_index <= 1){
|
||||||
|
prev_WP = current_loc;
|
||||||
|
}else{
|
||||||
|
if (get_distance(¤t_loc, &next_WP) < 10)
|
||||||
|
prev_WP = next_WP;
|
||||||
|
else
|
||||||
|
prev_WP = current_loc;
|
||||||
|
}
|
||||||
|
|
||||||
|
//Serial.printf("set_next_WP #%d, ", command_nav_index);
|
||||||
|
//print_wp(&prev_WP, command_nav_index -1);
|
||||||
|
|
||||||
// Load the next_WP slot
|
// Load the next_WP slot
|
||||||
// ---------------------
|
// ---------------------
|
||||||
@ -165,7 +175,7 @@ static void set_next_WP(struct Location *wp)
|
|||||||
// -----------------------------------
|
// -----------------------------------
|
||||||
wp_totalDistance = get_distance(¤t_loc, &next_WP);
|
wp_totalDistance = get_distance(¤t_loc, &next_WP);
|
||||||
wp_distance = wp_totalDistance;
|
wp_distance = wp_totalDistance;
|
||||||
target_bearing = get_bearing(¤t_loc, &next_WP);
|
target_bearing = get_bearing(&prev_WP, &next_WP);
|
||||||
|
|
||||||
// to check if we have missed the WP
|
// to check if we have missed the WP
|
||||||
// ---------------------------------
|
// ---------------------------------
|
||||||
|
Loading…
Reference in New Issue
Block a user