aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

ArduPlane: add AutoLand fixed-wing mode

Browse Source
This commit is contained in:
Henry Wurzburg 2024-12-22 13:39:44 -06:00 committed by Peter Barker
parent a7f7b37c80
commit 32f5afb22a
17 changed files with 269 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
ArduPlane/AP_Arming.cpp
View File

@ -378,8 +378,11 @@ bool AP_Arming_Plane::disarm(const AP_Arming::Method method, bool do_disarm_chec
// DO_CHANGE_SPEED commands
plane.new_airspeed_cm = -1;
#if MODE_AUTOLAND_ENABLED
// takeoff direction always cleared on disarm
plane.takeoff_state.initial_direction.initialized = false;
#endif
send_arm_disarm_statustext("Throttle disarmed");
return true;
}

6
ArduPlane/GCS_MAVLink_Plane.cpp
View File

@ -59,6 +59,9 @@ MAV_MODE GCS_MAVLINK_Plane::base_mode() const
case Mode::Number::GUIDED:
case Mode::Number::CIRCLE:
case Mode::Number::TAKEOFF:
#if MODE_AUTOLAND_ENABLED
case Mode::Number::AUTOLAND:
#endif
#if HAL_QUADPLANE_ENABLED
case Mode::Number::QRTL:
case Mode::Number::LOITER_ALT_QLAND:
@ -1588,6 +1591,9 @@ uint8_t GCS_MAVLINK_Plane::send_available_mode(uint8_t index) const
&plane.mode_takeoff,
#if HAL_SOARING_ENABLED
&plane.mode_thermal,
#endif
#if MODE_AUTOLAND_ENABLED
&plane.mode_autoland,
#endif
};

3
ArduPlane/GCS_Plane.cpp
View File

@ -70,6 +70,9 @@ void GCS_Plane::update_vehicle_sensor_status_flags(void)
case Mode::Number::GUIDED:
case Mode::Number::CIRCLE:
case Mode::Number::TAKEOFF:
#if MODE_AUTOLAND_ENABLED
case Mode::Number::AUTOLAND:
#endif
#if HAL_QUADPLANE_ENABLED
case Mode::Number::QRTL:
case Mode::Number::LOITER_ALT_QLAND:

6
ArduPlane/Parameters.cpp
View File

@ -1028,6 +1028,12 @@ const AP_Param::Info Plane::var_info[] = {
// @Path: mode_takeoff.cpp
GOBJECT(mode_takeoff, "TKOFF_", ModeTakeoff),
#if MODE_AUTOLAND_ENABLED
// @Group: AUTOLAND_
// @Path: mode_autoland.cpp
GOBJECT(mode_autoland, "AUTOLAND_", ModeAutoLand),
#endif
#if AP_PLANE_GLIDER_PULLUP_ENABLED
// @Group: PUP_
// @Path: pullup.cpp

2
ArduPlane/Parameters.h
View File

@ -363,6 +363,8 @@ public:
k_param_pullup = 270,
k_param_quicktune,
k_param_mode_autoland,
};
AP_Int16 format_version;

6
ArduPlane/Plane.cpp
View File

@ -690,7 +690,11 @@ void Plane::update_flight_stage(void)
}
#endif
set_flight_stage(AP_FixedWing::FlightStage::NORMAL);
} else if (control_mode != &mode_takeoff) {
} else if ((control_mode != &mode_takeoff)
#if MODE_AUTOLAND_ENABLED
&& (control_mode != &mode_autoland)
#endif
) {
// If not in AUTO then assume normal operation for normal TECS operation.
// This prevents TECS from being stuck in the wrong stage if you switch from
// AUTO to, say, FBWB during a landing, an aborted landing or takeoff.

14
ArduPlane/Plane.h
View File

@ -172,7 +172,9 @@ public:
friend class ModeTakeoff;
friend class ModeThermal;
friend class ModeLoiterAltQLand;
#if MODE_AUTOLAND_ENABLED
friend class ModeAutoLand;
#endif
#if AP_EXTERNAL_CONTROL_ENABLED
friend class AP_ExternalControl_Plane;
#endif
@ -326,6 +328,9 @@ private:
#endif // QAUTOTUNE_ENABLED
#endif // HAL_QUADPLANE_ENABLED
ModeTakeoff mode_takeoff;
#if MODE_AUTOLAND_ENABLED
ModeAutoLand mode_autoland;
#endif
#if HAL_SOARING_ENABLED
ModeThermal mode_thermal;
#endif
@ -454,6 +459,13 @@ private:
float throttle_lim_min;
uint32_t throttle_max_timer_ms;
uint32_t level_off_start_time_ms;
// Good candidate for keeping the initial time for TKOFF_THR_MAX_T.
#if MODE_AUTOLAND_ENABLED
struct {
float heading; // deg
bool initialized;
} initial_direction;
#endif
} takeoff_state;
// ground steering controller state

10
ArduPlane/commands_logic.cpp
View File

@ -382,6 +382,9 @@ void Plane::do_takeoff(const AP_Mission::Mission_Command& cmd)
// zero locked course
steer_state.locked_course_err = 0;
steer_state.hold_course_cd = -1;
#if MODE_AUTOLAND_ENABLED
takeoff_state.initial_direction.initialized = false;
#endif
auto_state.baro_takeoff_alt = barometer.get_altitude();
}
@ -550,12 +553,17 @@ bool Plane::verify_takeoff()
trust_ahrs_yaw |= ahrs.dcm_yaw_initialised();
#endif
if (trust_ahrs_yaw && steer_state.hold_course_cd == -1) {
const float min_gps_speed = 5;
const float min_gps_speed = GPS_GND_CRS_MIN_SPD;
if (auto_state.takeoff_speed_time_ms == 0 &&
gps.status() >= AP_GPS::GPS_OK_FIX_3D &&
gps.ground_speed() > min_gps_speed &&
hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_DISARMED) {
auto_state.takeoff_speed_time_ms = millis();
#if MODE_AUTOLAND_ENABLED
plane.takeoff_state.initial_direction.heading = wrap_360(plane.gps.ground_course() + plane.mode_autoland.landing_dir_off);
takeoff_state.initial_direction.initialized = true;
gcs().send_text(MAV_SEVERITY_INFO, "Autoland direction= %u",int(takeoff_state.initial_direction.heading));
#endif
}
if (auto_state.takeoff_speed_time_ms != 0 &&
millis() - auto_state.takeoff_speed_time_ms >= 2000) {

5
ArduPlane/control_modes.cpp
View File

@ -83,6 +83,11 @@ Mode *Plane::mode_from_mode_num(const enum Mode::Number num)
case Mode::Number::TAKEOFF:
ret = &mode_takeoff;
break;
#if MODE_AUTOLAND_ENABLED
case Mode::Number::AUTOLAND:
ret = &mode_autoland;
break;
#endif //MODE_AUTOLAND_ENABLED
case Mode::Number::THERMAL:
#if HAL_SOARING_ENABLED
ret = &mode_thermal;

3
ArduPlane/defines.h
View File

@ -10,6 +10,8 @@
#define TAKEOFF_RUDDER_WARNING_TIMEOUT 3000 //ms that GCS warning about not returning arming rudder to neutral repeats
#define GPS_GND_CRS_MIN_SPD 5 // m/s, used to set when intial_direction.heading is captured in NAV_TAKEOFF and Mode TAKEOFF
// failsafe
// ----------------------
enum failsafe_state {
@ -45,6 +47,7 @@ enum failsafe_action_long {
FS_ACTION_LONG_GLIDE = 2,
FS_ACTION_LONG_PARACHUTE = 3,
FS_ACTION_LONG_AUTO = 4,
FS_ACTION_LONG_AUTOLAND = 5,
};
// type of stick mixing enabled

26
ArduPlane/events.cpp
View File

@ -65,7 +65,11 @@ void Plane::failsafe_short_on_event(enum failsafe_state fstype, ModeReason reaso
break;
#endif // HAL_QUADPLANE_ENABLED
case Mode::Number::AUTO: {
case Mode::Number::AUTO:
#if MODE_AUTOLAND_ENABLED
case Mode::Number::AUTOLAND:
#endif
{
if (failsafe_in_landing_sequence()) {
// don't failsafe in a landing sequence
break;
@ -145,6 +149,12 @@ void Plane::failsafe_long_on_event(enum failsafe_state fstype, ModeReason reason
set_mode(mode_fbwa, reason);
} else if (g.fs_action_long == FS_ACTION_LONG_AUTO) {
set_mode(mode_auto, reason);
#if MODE_AUTOLAND_ENABLED
} else if (g.fs_action_long == FS_ACTION_LONG_AUTOLAND) {
if (!set_mode(mode_autoland, reason)) {
set_mode(mode_rtl, reason);
}
#endif
} else {
set_mode(mode_rtl, reason);
}
@ -194,14 +204,23 @@ void Plane::failsafe_long_on_event(enum failsafe_state fstype, ModeReason reason
set_mode(mode_fbwa, reason);
} else if (g.fs_action_long == FS_ACTION_LONG_AUTO) {
set_mode(mode_auto, reason);
#if MODE_AUTOLAND_ENABLED
} else if (g.fs_action_long == FS_ACTION_LONG_AUTOLAND) {
if (!set_mode(mode_autoland, reason)) {
set_mode(mode_rtl, reason);
}
#endif
} else if (g.fs_action_long == FS_ACTION_LONG_RTL) {
set_mode(mode_rtl, reason);
}
break;
case Mode::Number::RTL:
if (g.fs_action_long == FS_ACTION_LONG_AUTO) {
set_mode(mode_auto, reason);
#if MODE_AUTOLAND_ENABLED
} else if (g.fs_action_long == FS_ACTION_LONG_AUTOLAND) {
set_mode(mode_autoland, reason);
#endif
}
break;
#if HAL_QUADPLANE_ENABLED
@ -210,6 +229,9 @@ void Plane::failsafe_long_on_event(enum failsafe_state fstype, ModeReason reason
case Mode::Number::LOITER_ALT_QLAND:
#endif
case Mode::Number::INITIALISING:
#if MODE_AUTOLAND_ENABLED
case Mode::Number::AUTOLAND:
#endif
break;
}
gcs().send_text(MAV_SEVERITY_WARNING, "%s Failsafe On: %s", (reason == ModeReason:: GCS_FAILSAFE) ? "GCS" : "RC Long", control_mode->name());

4
ArduPlane/mode.cpp
View File

@ -142,6 +142,10 @@ bool Mode::enter()
// Make sure the flight stage is correct for the new mode
plane.update_flight_stage();
// reset landing state
plane.landing.reset();
#if HAL_QUADPLANE_ENABLED
if (quadplane.enabled()) {
float aspeed;

41
ArduPlane/mode.h
View File

@ -15,6 +15,10 @@
#define AP_QUICKTUNE_ENABLED HAL_QUADPLANE_ENABLED
#endif
#ifndef MODE_AUTOLAND_ENABLED
#define MODE_AUTOLAND_ENABLED 1
#endif
#include <AP_Quicktune/AP_Quicktune.h>
class AC_PosControl;
@ -61,6 +65,9 @@ public:
#if HAL_QUADPLANE_ENABLED
LOITER_ALT_QLAND = 25,
#endif
#if MODE_AUTOLAND_ENABLED
AUTOLAND = 26,
#endif
// Mode number 30 reserved for "offboard" for external/lua control.
};
@ -859,7 +866,41 @@ private:
bool have_autoenabled_fences;
};
#if MODE_AUTOLAND_ENABLED
class ModeAutoLand: public Mode
{
public:
ModeAutoLand();
Number mode_number() const override { return Number::AUTOLAND; }
const char *name() const override { return "AUTOLAND"; }
const char *name4() const override { return "ALND"; }
// methods that affect movement of the vehicle in this mode
void update() override;
void navigate() override;
bool allows_throttle_nudging() const override { return true; }
bool does_auto_navigation() const override { return true; }
bool does_auto_throttle() const override { return true; }
// 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;
protected:
bool _enter() override;
AP_Mission::Mission_Command cmd;
bool land_started;
};
#endif
#if HAL_SOARING_ENABLED
class ModeThermal: public Mode

118
ArduPlane/mode_autoland.cpp Normal file
View File

@ -0,0 +1,118 @@
#include "mode.h"
#include "Plane.h"
#include <GCS_MAVLink/GCS.h>
#if MODE_AUTOLAND_ENABLED
/*
mode AutoLand parameters
*/
const AP_Param::GroupInfo ModeAutoLand::var_info[] = {
// @Param: WP_ALT
// @DisplayName: Final approach WP altitude
// @Description: This is the target altitude above HOME for final approach waypoint
// @Range: 0 200
// @Increment: 1
// @Units: m
// @User: Standard
AP_GROUPINFO("WP_ALT", 1, ModeAutoLand, final_wp_alt, 55),
// @Param: WP_DIST
// @DisplayName: Final approach WP distance
// @Description: This is the distance from Home that the final approach waypoint is set. The waypoint point will be in the opposite direction of takeoff (the direction the plane is facing when the plane sets its takeoff heading)
// @Range: 0 700
// @Increment: 1
// @Units: m
// @User: Standard
AP_GROUPINFO("WP_DIST", 2, ModeAutoLand, final_wp_dist, 400),
// @Param: DIR_OFF
// @DisplayName: Landing direction offset from takeoff
// @Description: The captured takeoff direction (at arming,if TKOFF_OPTION bit1 is set, or after ground course is established in autotakeoffs)is offset by this amount to create a different landing direction and approach.
// @Range: -360 360
// @Increment: 1
// @Units: deg
// @User: Standard
AP_GROUPINFO("DIR_OFF", 3, ModeAutoLand, landing_dir_off, 0),
AP_GROUPEND
};
ModeAutoLand::ModeAutoLand() :
Mode()
{
AP_Param::setup_object_defaults(this, var_info);
}
bool ModeAutoLand::_enter()
{
//must be flying to enter
if (!plane.is_flying()) {
gcs().send_text(MAV_SEVERITY_WARNING, "Must already be flying!");
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
#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");
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;
}
//setup final approach waypoint
plane.prev_WP_loc = plane.current_loc;
const Location &home = ahrs.get_home();
plane.set_target_altitude_current();
plane.next_WP_loc = home;
const float bearing = wrap_360(plane.takeoff_state.initial_direction.heading + 180);
plane.next_WP_loc.offset_bearing(bearing, final_wp_dist);
plane.next_WP_loc.set_alt_m(final_wp_alt, Location::AltFrame::ABOVE_HOME);
// create a command to fly to final approach waypoint and start it
cmd.id = MAV_CMD_NAV_WAYPOINT;
cmd.content.location = plane.next_WP_loc;
plane.start_command(cmd);
land_started = false;
return true;
}
void ModeAutoLand::update()
{
plane.calc_nav_roll();
plane.calc_nav_pitch();
plane.set_offset_altitude_location(plane.prev_WP_loc, plane.next_WP_loc);
if (plane.landing.is_throttle_suppressed()) {
// if landing is considered complete throttle is never allowed, regardless of landing type
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0.0);
} else {
plane.calc_throttle();
}
}
void ModeAutoLand::navigate()
{
// check to see if if we have reached final approach waypoint, switch to NAV_LAND command and start it once if so
if (!land_started){
if (plane.verify_nav_wp(cmd)){
const Location &home_loc = ahrs.get_home();
cmd.id = MAV_CMD_NAV_LAND;
cmd.content.location = home_loc;
land_started = true;
plane.prev_WP_loc = plane.current_loc;
plane.next_WP_loc = home_loc;
plane.start_command(cmd);
plane.set_flight_stage(AP_FixedWing::FlightStage::LAND);
}
return;
//otherwise keep flying the current command
} else {
plane.verify_command(cmd);
}
}
#endif

14
ArduPlane/mode_takeoff.cpp
View File

@ -134,6 +134,9 @@ void ModeTakeoff::update()
plane.takeoff_state.throttle_max_timer_ms = millis();
takeoff_mode_setup = true;
plane.steer_state.hold_course_cd = wrap_360_cd(direction*100); // Necessary to allow Plane::takeoff_calc_roll() to function.
#if MODE_AUTOLAND_ENABLED
plane.takeoff_state.initial_direction.initialized = false;
#endif
}
}
}
@ -142,7 +145,16 @@ void ModeTakeoff::update()
plane.set_flight_stage(AP_FixedWing::FlightStage::NORMAL);
takeoff_mode_setup = false;
}
#if MODE_AUTOLAND_ENABLED
// set initial_direction.heading
const float min_gps_speed = GPS_GND_CRS_MIN_SPD;
if (!(plane.takeoff_state.initial_direction.initialized) && (plane.gps.ground_speed() > min_gps_speed)
&& (plane.flight_stage == AP_FixedWing::FlightStage::TAKEOFF)) {
plane.takeoff_state.initial_direction.heading = wrap_360(plane.gps.ground_course() + plane.mode_autoland.landing_dir_off);
plane.takeoff_state.initial_direction.initialized = true;
gcs().send_text(MAV_SEVERITY_INFO, "Autoland direction= %u",int(plane.takeoff_state.initial_direction.heading));
}
#endif
// We update the waypoint to follow once we're past TKOFF_LVL_ALT or we
// pass the target location. The check for target location prevents us
// flying towards a wrong location if we can't climb.

1
ArduPlane/quadplane.h
View File

@ -62,6 +62,7 @@ public:
friend class ModeQAutotune;
friend class ModeQAcro;
friend class ModeLoiterAltQLand;
friend class ModeAutoLand;
QuadPlane(AP_AHRS &_ahrs);

9
ArduPlane/takeoff.cpp
View File

@ -22,12 +22,17 @@ bool Plane::auto_takeoff_check(void)
return false;
}
// Reset states if process has been interrupted
// Reset states if process has been interrupted, except initial_direction.initialized if set
#if MODE_AUTOLAND_ENABLED
bool takeoff_dir_initialized = takeoff_state.initial_direction.initialized;
#endif
if (takeoff_state.last_check_ms && (now - takeoff_state.last_check_ms) > 200) {
memset(&takeoff_state, 0, sizeof(takeoff_state));
#if MODE_AUTOLAND_ENABLED
takeoff_state.initial_direction.initialized = takeoff_dir_initialized; //restore dir init state
#endif
return false;
}
takeoff_state.last_check_ms = now;
//check if waiting for rudder neutral after rudder arm