Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

Plane: used correct WP radius for VTOL flight

Browse Source
This commit is contained in:
Iampete1 2021-08-30 19:25:45 +01:00 committed by Andrew Tridgell
parent 7ec7b478f0
commit 478bf37246
2 changed files with 14 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
ArduPlane/Plane.h
View File

@ -929,6 +929,7 @@ private:
void do_set_home(const AP_Mission::Mission_Command& cmd);
bool start_command_callback(const AP_Mission::Mission_Command &cmd);
bool verify_command_callback(const AP_Mission::Mission_Command& cmd);
float get_wp_radius() const;
// commands.cpp
void set_guided_WP(void);

16
ArduPlane/commands_logic.cpp
View File

@ -639,9 +639,7 @@ bool Plane::verify_nav_wp(const AP_Mission::Mission_Command& cmd)
// allow user to override acceptance radius
acceptance_distance_m = cmd_acceptance_distance;
} else if (cmd_passby == 0) {
acceptance_distance_m = nav_controller->turn_distance(g.waypoint_radius, auto_state.next_turn_angle);
} else {
acceptance_distance_m = nav_controller->turn_distance(get_wp_radius(), auto_state.next_turn_angle);
}
if (auto_state.wp_distance <= acceptance_distance_m) {
@ -768,7 +766,7 @@ bool Plane::verify_RTL()
loiter.direction = 1;
}
update_loiter(abs(g.rtl_radius));
if (auto_state.wp_distance <= (uint32_t)MAX(g.waypoint_radius,0) ||
if (auto_state.wp_distance <= (uint32_t)MAX(get_wp_radius(),0) ||
reached_loiter_target()) {
gcs().send_text(MAV_SEVERITY_INFO,"Reached RTL location");
return true;
@ -1098,3 +1096,13 @@ bool Plane::verify_loiter_heading(bool init)
return plane.mode_loiter.isHeadingLinedUp(next_WP_loc, next_nav_cmd.content.location);
}
float Plane::get_wp_radius() const
{
#if HAL_QUADPLANE_ENABLED
if (plane.quadplane.in_vtol_mode()) {
return plane.quadplane.wp_nav->get_wp_radius_cm() * 0.01;
}
#endif
return g.waypoint_radius;
}