aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Plane: fixed combination of passby and acceptance dist WP

Browse Source 
when a user sets a passby distance we should calculate the turn point
based on the extrapolated distance, not the original waypoint

also simplify the passby logic using offset_bearing()
This commit is contained in:
Andrew Tridgell 2022-07-03 15:52:16 +10:00
parent 6eb406b2d6
commit 592f33bfaf
1 changed files with 6 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
ArduPlane/commands_logic.cpp
View File

@ -615,13 +615,11 @@ bool Plane::verify_nav_wp(const AP_Mission::Mission_Command& cmd)
uint8_t cmd_acceptance_distance = LOWBYTE(cmd.p1); // radius in meters to accept reaching the wp
if (cmd_passby > 0) {
float dist = prev_WP_loc.get_distance(flex_next_WP_loc);
const float dist = prev_WP_loc.get_distance(flex_next_WP_loc);
const float bearing_deg = degrees(prev_WP_loc.get_bearing(flex_next_WP_loc));
if (!is_zero(dist)) {
float factor = (dist + cmd_passby) / dist;
flex_next_WP_loc.lat = flex_next_WP_loc.lat + (flex_next_WP_loc.lat - prev_WP_loc.lat) * (factor - 1.0f);
flex_next_WP_loc.lng = flex_next_WP_loc.lng + Location::diff_longitude(flex_next_WP_loc.lng,prev_WP_loc.lng) * (factor - 1.0f);
if (is_positive(dist)) {
flex_next_WP_loc.offset_bearing(bearing_deg, cmd_passby);
}
}
@ -651,8 +649,8 @@ bool Plane::verify_nav_wp(const AP_Mission::Mission_Command& cmd)
} else if (cmd_passby == 0) {
acceptance_distance_m = nav_controller->turn_distance(get_wp_radius(), auto_state.next_turn_angle);
}
if (auto_state.wp_distance <= acceptance_distance_m) {
const float wp_dist = current_loc.get_distance(flex_next_WP_loc);
if (wp_dist <= acceptance_distance_m) {
gcs().send_text(MAV_SEVERITY_INFO, "Reached waypoint #%i dist %um",
(unsigned)mission.get_current_nav_cmd().index,
(unsigned)current_loc.get_distance(flex_next_WP_loc));