Plane: use enum class for VTOL approach stage

also namespace it with the state variable which uses this type
This commit is contained in:
Peter Barker 2024-08-07 10:58:28 +10:00 committed by Andrew Tridgell
parent cbbb6881e2
commit 2632e5b8a6
5 changed files with 29 additions and 28 deletions

View File

@ -366,7 +366,11 @@ private:
uint32_t AFS_last_valid_rc_ms;
} failsafe;
enum Landing_ApproachStage {
#if HAL_QUADPLANE_ENABLED
// Landing
class VTOLApproach {
public:
enum class Stage {
RTL,
LOITER_TO_ALT,
ENSURE_RADIUS,
@ -375,10 +379,7 @@ private:
VTOL_LANDING,
};
#if HAL_QUADPLANE_ENABLED
// Landing
struct {
enum Landing_ApproachStage approach_stage;
Stage approach_stage;
float approach_direction_deg;
} vtol_approach_s;
#endif

View File

@ -429,7 +429,7 @@ void Plane::do_landing_vtol_approach(const AP_Mission::Mission_Command& cmd)
loc.sanitize(current_loc);
set_next_WP(loc);
vtol_approach_s.approach_stage = LOITER_TO_ALT;
vtol_approach_s.approach_stage = VTOLApproach::Stage::LOITER_TO_ALT;
}
#endif
@ -1016,7 +1016,7 @@ bool Plane::verify_landing_vtol_approach(const AP_Mission::Mission_Command &cmd)
loiter.direction = direction;
switch (vtol_approach_s.approach_stage) {
case RTL:
case VTOLApproach::Stage::RTL:
{
// fly home and loiter at RTL alt
nav_controller->update_loiter(cmd.content.location, abs_radius, direction);
@ -1024,11 +1024,11 @@ bool Plane::verify_landing_vtol_approach(const AP_Mission::Mission_Command &cmd)
// decend to Q RTL alt
plane.do_RTL(plane.home.alt + plane.quadplane.qrtl_alt*100UL);
plane.loiter_angle_reset();
vtol_approach_s.approach_stage = LOITER_TO_ALT;
vtol_approach_s.approach_stage = VTOLApproach::Stage::LOITER_TO_ALT;
}
break;
}
case LOITER_TO_ALT:
case VTOLApproach::Stage::LOITER_TO_ALT:
{
nav_controller->update_loiter(cmd.content.location, abs_radius, direction);
@ -1036,11 +1036,11 @@ bool Plane::verify_landing_vtol_approach(const AP_Mission::Mission_Command &cmd)
Vector3f wind = ahrs.wind_estimate();
vtol_approach_s.approach_direction_deg = degrees(atan2f(-wind.y, -wind.x));
gcs().send_text(MAV_SEVERITY_INFO, "Selected an approach path of %.1f", (double)vtol_approach_s.approach_direction_deg);
vtol_approach_s.approach_stage = ENSURE_RADIUS;
vtol_approach_s.approach_stage = VTOLApproach::Stage::ENSURE_RADIUS;
}
break;
}
case ENSURE_RADIUS:
case VTOLApproach::Stage::ENSURE_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
@ -1050,10 +1050,10 @@ bool Plane::verify_landing_vtol_approach(const AP_Mission::Mission_Command &cmd)
nav_controller->update_loiter(cmd.content.location, abs_radius, direction);
break;
}
vtol_approach_s.approach_stage = WAIT_FOR_BREAKOUT;
vtol_approach_s.approach_stage = VTOLApproach::Stage::WAIT_FOR_BREAKOUT;
FALLTHROUGH;
}
case WAIT_FOR_BREAKOUT:
case VTOLApproach::Stage::WAIT_FOR_BREAKOUT:
{
nav_controller->update_loiter(cmd.content.location, radius, direction);
@ -1062,7 +1062,7 @@ bool Plane::verify_landing_vtol_approach(const AP_Mission::Mission_Command &cmd)
// breakout when within 5 degrees of the opposite direction
if (fabsF(wrap_PI(ahrs.get_yaw() - breakout_direction_rad)) < radians(5.0f)) {
gcs().send_text(MAV_SEVERITY_INFO, "Starting VTOL land approach path");
vtol_approach_s.approach_stage = APPROACH_LINE;
vtol_approach_s.approach_stage = VTOLApproach::Stage::APPROACH_LINE;
set_next_WP(cmd.content.location);
// fallthrough
} else {
@ -1070,7 +1070,7 @@ bool Plane::verify_landing_vtol_approach(const AP_Mission::Mission_Command &cmd)
}
FALLTHROUGH;
}
case APPROACH_LINE:
case VTOLApproach::Stage::APPROACH_LINE:
{
// project an apporach path
Location start = cmd.content.location;
@ -1099,7 +1099,7 @@ bool Plane::verify_landing_vtol_approach(const AP_Mission::Mission_Command &cmd)
}
if (past_finish_line && (lined_up || half_radius)) {
vtol_approach_s.approach_stage = VTOL_LANDING;
vtol_approach_s.approach_stage = VTOLApproach::Stage::VTOL_LANDING;
quadplane.do_vtol_land(cmd);
// fallthrough
} else {
@ -1107,7 +1107,7 @@ bool Plane::verify_landing_vtol_approach(const AP_Mission::Mission_Command &cmd)
}
FALLTHROUGH;
}
case VTOL_LANDING:
case VTOLApproach::Stage::VTOL_LANDING:
// nothing to do here, we should be into the quadplane landing code
return true;
}

View File

@ -7,7 +7,7 @@ bool ModeRTL::_enter()
plane.do_RTL(plane.get_RTL_altitude_cm());
plane.rtl.done_climb = false;
#if HAL_QUADPLANE_ENABLED
plane.vtol_approach_s.approach_stage = Plane::Landing_ApproachStage::RTL;
plane.vtol_approach_s.approach_stage = Plane::VTOLApproach::Stage::RTL;
// Quadplane specific checks
if (plane.quadplane.available()) {
@ -83,7 +83,7 @@ void ModeRTL::navigate()
AP_Mission::Mission_Command cmd;
cmd.content.location = plane.next_WP_loc;
plane.verify_landing_vtol_approach(cmd);
if (plane.vtol_approach_s.approach_stage == Plane::Landing_ApproachStage::VTOL_LANDING) {
if (plane.vtol_approach_s.approach_stage == Plane::VTOLApproach::Stage::VTOL_LANDING) {
plane.set_mode(plane.mode_qrtl, ModeReason::RTL_COMPLETE_SWITCHING_TO_VTOL_LAND_RTL);
}
return;

View File

@ -116,8 +116,8 @@ float Plane::mode_auto_target_airspeed_cm()
{
#if HAL_QUADPLANE_ENABLED
if (quadplane.landing_with_fixed_wing_spiral_approach() &&
((vtol_approach_s.approach_stage == Landing_ApproachStage::APPROACH_LINE) ||
(vtol_approach_s.approach_stage == Landing_ApproachStage::VTOL_LANDING))) {
((vtol_approach_s.approach_stage == VTOLApproach::Stage::APPROACH_LINE) ||
(vtol_approach_s.approach_stage == VTOLApproach::Stage::VTOL_LANDING))) {
const float land_airspeed = TECS_controller.get_land_airspeed();
if (is_positive(land_airspeed)) {
return land_airspeed * 100;

View File

@ -3954,7 +3954,7 @@ bool QuadPlane::is_vtol_land(uint16_t id) const
{
if (id == MAV_CMD_NAV_VTOL_LAND || id == MAV_CMD_NAV_PAYLOAD_PLACE) {
if (landing_with_fixed_wing_spiral_approach()) {
return plane.vtol_approach_s.approach_stage == Plane::Landing_ApproachStage::VTOL_LANDING;
return plane.vtol_approach_s.approach_stage == Plane::VTOLApproach::Stage::VTOL_LANDING;
} else {
return true;
}