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

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
ArduPlane/Plane.h
View File

@ -366,19 +366,20 @@ private:
uint32_t AFS_last_valid_rc_ms; uint32_t AFS_last_valid_rc_ms;
} failsafe; } failsafe;
enum Landing_ApproachStage {
RTL,
LOITER_TO_ALT,
ENSURE_RADIUS,
WAIT_FOR_BREAKOUT,
APPROACH_LINE,
VTOL_LANDING,
};
#if HAL_QUADPLANE_ENABLED #if HAL_QUADPLANE_ENABLED
// Landing // Landing
struct { class VTOLApproach {
enum Landing_ApproachStage approach_stage; public:
enum class Stage {
RTL,
LOITER_TO_ALT,
ENSURE_RADIUS,
WAIT_FOR_BREAKOUT,
APPROACH_LINE,
VTOL_LANDING,
};
Stage approach_stage;
float approach_direction_deg; float approach_direction_deg;
} vtol_approach_s; } vtol_approach_s;
#endif #endif

24
ArduPlane/commands_logic.cpp
View File

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

4
ArduPlane/mode_rtl.cpp
View File

@ -7,7 +7,7 @@ bool ModeRTL::_enter()
plane.do_RTL(plane.get_RTL_altitude_cm()); plane.do_RTL(plane.get_RTL_altitude_cm());
plane.rtl.done_climb = false; plane.rtl.done_climb = false;
#if HAL_QUADPLANE_ENABLED #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 // Quadplane specific checks
if (plane.quadplane.available()) { if (plane.quadplane.available()) {
@ -83,7 +83,7 @@ void ModeRTL::navigate()
AP_Mission::Mission_Command cmd; AP_Mission::Mission_Command cmd;
cmd.content.location = plane.next_WP_loc; cmd.content.location = plane.next_WP_loc;
plane.verify_landing_vtol_approach(cmd); 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); plane.set_mode(plane.mode_qrtl, ModeReason::RTL_COMPLETE_SWITCHING_TO_VTOL_LAND_RTL);
} }
return; return;

4
ArduPlane/navigation.cpp
View File

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

2
ArduPlane/quadplane.cpp
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 (id == MAV_CMD_NAV_VTOL_LAND || id == MAV_CMD_NAV_PAYLOAD_PLACE) {
if (landing_with_fixed_wing_spiral_approach()) { 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 { } else {
return true; return true;
} }