Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Plane: fixed check for fixed wing approach QRTL start 

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 Peter Barker
parent 0a7757a720
commit 5f5f70d20a
1 changed files with 19 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

38
ArduPlane/commands_logic.cpp
View File

@ -1018,6 +1018,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:
{
@ -1045,21 +1049,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;
@ -1067,12 +1062,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)));
@ -1103,7 +1092,18 @@ 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 Vector2f target_vec = current_loc.get_distance_NE(cmd.content.location);
const float angle_err = fabsf(wrap_180(degrees(vel_NED.xy().angle(target_vec))));
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