Skip to content

Commit

Permalink
Plane: neaten up the autoland logic
Browse files Browse the repository at this point in the history
  • Loading branch information
@tridge
tridge committed Jan 12, 2025
1 parent ceff3d0 commit a3e5475
Show file tree
Hide file tree
Showing 4 changed files with 108 additions and 48 deletions.
7 changes: 1 addition & 6 deletions ArduPlane/AP_Arming.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -322,12 +322,7 @@ bool AP_Arming_Plane::arm(const AP_Arming::Method method, const bool do_arming_c
delay_arming = true;

#if MODE_AUTOLAND_ENABLED
//capture heading for autoland mode if option is set and using a compass
if (plane.ahrs.use_compass() && plane.mode_autoland.autoland_option_is_set(ModeAutoLand::AutoLandOption::AUTOLAND_DIR_ON_ARM)) {
plane.takeoff_state.initial_direction.heading = wrap_360(plane.ahrs.yaw_sensor * 0.01f);
plane.takeoff_state.initial_direction.initialized = true;
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Autoland direction= %u",int(plane.takeoff_state.initial_direction.heading));
}
plane.mode_autoland.arm_check();
#endif

send_arm_disarm_statustext("Throttle armed");
Expand Down
19 changes: 9 additions & 10 deletions ArduPlane/commands_logic.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1164,14 +1164,18 @@ bool Plane::verify_loiter_heading(bool init)
return true;
}
#endif
//Get the lat/lon of next Nav waypoint after this one:
AP_Mission::Mission_Command next_nav_cmd;
bool in_autoland_mode = false;

#if MODE_AUTOLAND_ENABLED
in_autoland_mode = plane.control_mode-> mode_number() == Mode::Number::AUTOLAND;
if (control_mode == &mode_autoland) {
// autoland mode has its own lineup criterion
return mode_autoland.landing_lined_up();
}
#endif

//Get the lat/lon of next Nav waypoint after this one:
AP_Mission::Mission_Command next_nav_cmd;
if (! mission.get_next_nav_cmd(mission.get_current_nav_index() + 1,
next_nav_cmd) && !in_autoland_mode) {
next_nav_cmd)) {
//no next waypoint to shoot for -- go ahead and break out of loiter
return true;
}
Expand All @@ -1180,11 +1184,6 @@ bool Plane::verify_loiter_heading(bool init)
loiter.sum_cd = 0;
}

#if MODE_AUTOLAND_ENABLED
if (plane.control_mode-> mode_number() == Mode::Number::AUTOLAND){
return plane.mode_loiter.isHeadingLinedUp( plane.mode_autoland.loiter_WP,plane.mode_autoland.land_start_WP);
}
#endif
return plane.mode_loiter.isHeadingLinedUp(next_WP_loc, next_nav_cmd.content.location);
}

Expand Down
37 changes: 28 additions & 9 deletions ArduPlane/mode.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -217,6 +217,11 @@ friend class ModeQAcro;

void stabilize_quaternion();

#if MODE_AUTOLAND_ENABLED
// true if mode allows landing direction to be set on first takeoff after arm in this mode
bool allows_autoland_direction_capture() const override { return true; }
#endif

protected:

// ACRO controller state
Expand Down Expand Up @@ -318,6 +323,11 @@ class ModeAutoTune : public Mode

void run() override;

#if MODE_AUTOLAND_ENABLED
// true if mode allows landing direction to be set on first takeoff after arm in this mode
bool allows_autoland_direction_capture() const override { return true; }
#endif

protected:

bool _enter() override;
Expand Down Expand Up @@ -533,6 +543,10 @@ class ModeTraining : public Mode

void run() override;

#if MODE_AUTOLAND_ENABLED
// true if mode allows landing direction to be set on first takeoff after arm in this mode
bool allows_autoland_direction_capture() const override { return true; }
#endif
};

class ModeInitializing : public Mode
Expand Down Expand Up @@ -813,11 +827,6 @@ class ModeQAcro : public Mode
bool is_vtol_man_throttle() const override { return true; }
virtual bool is_vtol_man_mode() const override { return true; }

#if MODE_AUTOLAND_ENABLED
// true if mode allows landing direction to be set on first takeoff after arm in this mode
bool allows_autoland_direction_capture() const override { return true; }
#endif

// methods that affect movement of the vehicle in this mode
void update() override;

Expand Down Expand Up @@ -924,29 +933,39 @@ class ModeAutoLand: public Mode

void check_takeoff_direction(void);

// return true when lined up correctly from the LOITER_TO_ALT
bool landing_lined_up(void);

// see if we should capture the direction
void arm_check(void);

// var_info for holding parameter information
static const struct AP_Param::GroupInfo var_info[];

AP_Int16 final_wp_alt;
AP_Int16 final_wp_dist;
AP_Int16 landing_dir_off;
AP_Int8 options;
Location land_start_WP;
Location loiter_WP;


// Bitfields of AUTOLAND_OPTIONS
enum class AutoLandOption {
AUTOLAND_DIR_ON_ARM = (1U << 0), // set dir for autoland on arm if compass in use.
};

enum class AutoLandStage {
LOITER,
BASE_LEG,
LANDING
};

bool autoland_option_is_set(AutoLandOption option) const {
return (options & int8_t(option)) != 0;
}

protected:
bool _enter() override;
AP_Mission::Mission_Command cmd[3];
uint8_t stage;
AutoLandStage stage;
void set_autoland_direction(void);
};
#endif
Expand Down
93 changes: 70 additions & 23 deletions ArduPlane/mode_autoland.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -59,13 +59,15 @@ bool ModeAutoLand::_enter()
return false;
}

//takeoff direction must be set and must not be a quadplane, otherwise since flying switch to RTL so this can be used as FS action
// autoland not available for quadplanes as capture of takeoff direction
// doesn't make sense
#if HAL_QUADPLANE_ENABLED
if (quadplane.available() && !quadplane.option_is_set(QuadPlane::OPTION::ALLOW_FW_LAND)) {
gcs().send_text(MAV_SEVERITY_WARNING, "Option not set to allow fixed wing autoland");
if (quadplane.available()) {
gcs().send_text(MAV_SEVERITY_WARNING, "autoland not available");
return false;
}
#endif

if (!plane.takeoff_state.initial_direction.initialized) {
gcs().send_text(MAV_SEVERITY_WARNING, "Takeoff initial direction not set,must use autotakeoff");
return false;
Expand All @@ -74,6 +76,8 @@ bool ModeAutoLand::_enter()
plane.set_target_altitude_current();
plane.auto_state.next_wp_crosstrack = true; // not really needed since previous/next_WP is not used by loiter cmd navigation

plane.prev_WP_loc = plane.current_loc;

/*
landing is in 3 steps:
1) a base leg loitering to alt waypoint
Expand All @@ -89,7 +93,7 @@ bool ModeAutoLand::_enter()
NAV_LAND. This is offset by final_wp_dist from home, in a
direction 180 degrees from takeoff direction
*/
land_start_WP = home;
Location land_start_WP = home;
land_start_WP.offset_bearing(wrap_360(plane.takeoff_state.initial_direction.heading + 180), final_wp_dist);
land_start_WP.set_alt_m(final_wp_alt, Location::AltFrame::ABOVE_HOME);
land_start_WP.change_alt_frame(Location::AltFrame::ABSOLUTE);
Expand All @@ -102,11 +106,14 @@ bool ModeAutoLand::_enter()
const float bearing_to_current_deg = wrap_180(degrees(land_start_WP.get_bearing(plane.current_loc)));
const float bearing_err_deg = wrap_180(wrap_180(plane.takeoff_state.initial_direction.heading) - bearing_to_current_deg);
const float bearing_offset_deg = bearing_err_deg > 0? -90 : 90;
const float base_leg_length = plane.aparm.loiter_radius; //make the "base leg" the loiter radius so its a tangent exit always

// make the "base leg" the smaller of the loiter radius and 1/3 of the final WP distance
// this results in a neat landing no matter the loiter radius
const float base_leg_length = MIN(final_wp_dist * 0.333, fabsf(plane.aparm.loiter_radius));

cmd[0].id = MAV_CMD_NAV_LOITER_TO_ALT;
cmd[0].content.location = land_start_WP;
cmd[0].content.location.offset_bearing(plane.takeoff_state.initial_direction.heading + bearing_offset_deg, base_leg_length);
loiter_WP = cmd[0].content.location;
cmd[0].content.location.loiter_ccw = bearing_err_deg>0? 1 :0;

/*
Expand All @@ -120,7 +127,7 @@ bool ModeAutoLand::_enter()
cmd[2].content.location = home;

// start first leg
stage = 0;
stage = AutoLandStage::LOITER;
plane.start_command(cmd[0]);

return true;
Expand All @@ -142,36 +149,54 @@ void ModeAutoLand::update()
void ModeAutoLand::navigate()
{
switch (stage) {
case 0:
case AutoLandStage::LOITER:
if (plane.verify_loiter_to_alt(cmd[0])) {
break;
stage = AutoLandStage::BASE_LEG;
plane.start_command(cmd[1]);
// setup a predictable turn angle for the base leg
plane.auto_state.next_turn_angle = 90;
plane.auto_state.crosstrack = false;
}
return;
case 1:
if (plane.verify_nav_wp(cmd[stage])) {
break;
break;

case AutoLandStage::BASE_LEG:
if (plane.verify_nav_wp(cmd[1])) {
stage = AutoLandStage::LANDING;
plane.start_command(cmd[2]);
}
return;
case 2:
break;

case AutoLandStage::LANDING:
plane.set_flight_stage(AP_FixedWing::FlightStage::LAND);
plane.verify_command(cmd[stage]);
return;
plane.verify_command(cmd[2]);
// make sure we line up
plane.auto_state.crosstrack = true;
break;
}
stage++;
plane.start_command(cmd[stage]);
return;
}

/*
Takeoff direction is initialized after arm when sufficient altitude and ground speed is obtained, then captured takeoff direction + offset used as landing direction in AUTOLAND
Takeoff direction is initialized after arm when sufficient altitude
and ground speed is obtained, then captured takeoff direction +
offset used as landing direction in AUTOLAND
*/
void ModeAutoLand::check_takeoff_direction()
{
#if HAL_QUADPLANE_ENABLED
// we don't allow fixed wing autoland for quadplanes
if (quadplane.available()) {
return;
}
#endif

if (plane.takeoff_state.initial_direction.initialized) {
return;
}
//set autoland direction to GPS course over ground
if (plane.control_mode->allows_autoland_direction_capture() && (plane.gps.ground_speed() > GPS_GND_CRS_MIN_SPD)) {
if (plane.control_mode->allows_autoland_direction_capture() &&
plane.is_flying() &&
hal.util->get_soft_armed() &&
plane.gps.ground_speed() > GPS_GND_CRS_MIN_SPD) {
set_autoland_direction();
}
}
Expand All @@ -183,4 +208,26 @@ void ModeAutoLand::set_autoland_direction()
plane.takeoff_state.initial_direction.initialized = true;
gcs().send_text(MAV_SEVERITY_INFO, "Autoland direction= %u",int(plane.takeoff_state.initial_direction.heading));
}
#endif

/*
return true when the LOITER_TO_ALT is lined up ready for the landing
*/
bool ModeAutoLand::landing_lined_up(void)
{
// use the line between the center of the LOITER_TO_ALT and the
// start of the landing leg
return plane.mode_loiter.isHeadingLinedUp_cd(cmd[0].content.location.get_bearing(cmd[1].content.location)*100);
}

// possibly capture heading for autoland mode if option is set and using a compass
void ModeAutoLand::arm_check(void)
{
if (plane.ahrs.use_compass() && autoland_option_is_set(ModeAutoLand::AutoLandOption::AUTOLAND_DIR_ON_ARM)) {
plane.takeoff_state.initial_direction.heading = wrap_360(plane.ahrs.yaw_sensor * 0.01f);
plane.takeoff_state.initial_direction.initialized = true;
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Autoland direction= %u",int(plane.takeoff_state.initial_direction.heading));
}
}

#endif // MODE_AUTOLAND_ENABLED

0 comments on commit a3e5475

Please sign in to comment.