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

Plane: fixed check for fixed wing approach QRTL start

Browse Source 
ensure we are lined up, or we are at less than 0.5 of radius before we
switch to QRTL. This fixes the case where the stopping distance is
greater than the radius, prevening us from switching to QRTL while not
lined up with the landing point
This commit is contained in:
Andrew Tridgell 2022-06-29 10:10:51 +10:00 committed by Randy Mackay
parent 3c3a55201d
commit a6f6d12934
1 changed files with 20 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

39
ArduPlane/commands_logic.cpp
View File

@ -986,6 +986,10 @@ void Plane::exit_mission_callback()
#if HAL_QUADPLANE_ENABLED
bool Plane::verify_landing_vtol_approach(const AP_Mission::Mission_Command &cmd)
{
const float radius = is_zero(quadplane.fw_land_approach_radius)? aparm.loiter_radius : quadplane.fw_land_approach_radius;
const int8_t direction = is_negative(radius) ? -1 : 1;
const float abs_radius = fabsf(radius);
switch (vtol_approach_s.approach_stage) {
case RTL:
{
@ -1013,21 +1017,12 @@ bool Plane::verify_landing_vtol_approach(const AP_Mission::Mission_Command &cmd)
}
case ENSURE_RADIUS:
{
float radius;
if (is_zero(quadplane.fw_land_approach_radius)) {
radius = aparm.loiter_radius;
} else {
radius = quadplane.fw_land_approach_radius;
}
const int8_t direction = is_negative(radius) ? -1 : 1;
radius = fabsf(radius);
// validate that the vehicle is at least the expected distance away from the loiter point
// require an angle total of at least 2 centidegrees, due to special casing of 1 centidegree
if (((fabsf(cmd.content.location.get_distance(current_loc) - radius) > 5.0f) &&
(cmd.content.location.get_distance(current_loc) < radius)) ||
if (((fabsf(cmd.content.location.get_distance(current_loc) - abs_radius) > 5.0f) &&
(cmd.content.location.get_distance(current_loc) < abs_radius)) ||
(loiter.sum_cd < 2)) {
nav_controller->update_loiter(cmd.content.location, radius, direction);
nav_controller->update_loiter(cmd.content.location, abs_radius, direction);
break;
}
vtol_approach_s.approach_stage = WAIT_FOR_BREAKOUT;
@ -1035,12 +1030,6 @@ bool Plane::verify_landing_vtol_approach(const AP_Mission::Mission_Command &cmd)
}
case WAIT_FOR_BREAKOUT:
{
float radius = quadplane.fw_land_approach_radius;
if (is_zero(radius)) {
radius = aparm.loiter_radius;
}
const int8_t direction = is_negative(radius) ? -1 : 1;
nav_controller->update_loiter(cmd.content.location, radius, direction);
const float breakout_direction_rad = radians(wrap_180(vtol_approach_s.approach_direction_deg + (direction > 0 ? 270 : 90)));
@ -1071,7 +1060,19 @@ bool Plane::verify_landing_vtol_approach(const AP_Mission::Mission_Command &cmd)
// check if we should move on to the next waypoint
Location breakout_loc = cmd.content.location;
breakout_loc.offset_bearing(vtol_approach_s.approach_direction_deg + 180, quadplane.stopping_distance());
if(current_loc.past_interval_finish_line(start, breakout_loc)) {
const bool past_finish_line = current_loc.past_interval_finish_line(start, breakout_loc);
const bool half_radius = current_loc.get_distance(cmd.content.location) < 0.5 * abs_radius;
bool lined_up = true;
Vector3f vel_NED;
if (ahrs.get_velocity_NED(vel_NED)) {
const float ground_course_deg = wrap_180(degrees(vel_NED.xy().angle()));
const float approach_deg = wrap_180(vtol_approach_s.approach_direction_deg);
const float angle_err = fabsf(wrap_180(ground_course_deg - approach_deg));
lined_up = (angle_err < 30);
}
if (past_finish_line && (lined_up || half_radius)) {
vtol_approach_s.approach_stage = VTOL_LANDING;
quadplane.do_vtol_land(cmd);
// fallthrough