Skip to content

Commit

Permalink
Plane: use enum class for VTOL approach stage
Browse files Browse the repository at this point in the history
also namespace it with the state variable which uses this type
  • Loading branch information
peterbarker authored and tridge committed Aug 7, 2024
1 parent cbbb688 commit 2632e5b
Show file tree
Hide file tree
Showing 5 changed files with 29 additions and 28 deletions.
23 changes: 12 additions & 11 deletions ArduPlane/Plane.h
Original file line number Diff line number Diff line change
Expand Up @@ -366,19 +366,20 @@ class Plane : public AP_Vehicle {
uint32_t AFS_last_valid_rc_ms;
} failsafe;

enum Landing_ApproachStage {
RTL,
LOITER_TO_ALT,
ENSURE_RADIUS,
WAIT_FOR_BREAKOUT,
APPROACH_LINE,
VTOL_LANDING,
};

#if HAL_QUADPLANE_ENABLED
// Landing
struct {
enum Landing_ApproachStage approach_stage;
class VTOLApproach {
public:
enum class Stage {
RTL,
LOITER_TO_ALT,
ENSURE_RADIUS,
WAIT_FOR_BREAKOUT,
APPROACH_LINE,
VTOL_LANDING,
};

Stage approach_stage;
float approach_direction_deg;
} vtol_approach_s;
#endif
Expand Down
24 changes: 12 additions & 12 deletions ArduPlane/commands_logic.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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

Expand Down Expand Up @@ -1016,31 +1016,31 @@ 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);
if (plane.reached_loiter_target()) {
// 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);

if (labs(loiter.sum_cd) > 1 && (loiter.reached_target_alt || loiter.unable_to_acheive_target_alt)) {
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
Expand All @@ -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);

Expand All @@ -1062,15 +1062,15 @@ 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 {
break;
}
FALLTHROUGH;
}
case APPROACH_LINE:
case VTOLApproach::Stage::APPROACH_LINE:
{
// project an apporach path
Location start = cmd.content.location;
Expand Down Expand Up @@ -1099,15 +1099,15 @@ 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 {
break;
}
FALLTHROUGH;
}
case VTOL_LANDING:
case VTOLApproach::Stage::VTOL_LANDING:
// nothing to do here, we should be into the quadplane landing code
return true;
}
Expand Down
4 changes: 2 additions & 2 deletions ArduPlane/mode_rtl.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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()) {
Expand Down Expand Up @@ -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;
Expand Down
4 changes: 2 additions & 2 deletions ArduPlane/navigation.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
2 changes: 1 addition & 1 deletion ArduPlane/quadplane.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}
Expand Down

0 comments on commit 2632e5b

Please sign in to comment.