mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-02-11 02:13:57 -04:00
ArduPlane: mode AUTOLAND enhancements
Co-authored-by: Andrew Tridgell <andrew@tridgell.net> Co-authored-by: Pete Hall <iampete@hotmail.co.uk>
This commit is contained in:
Henry Wurzburg
Andrew Tridgell
parent
2bc167d2ff
commit
010e6ba0b0
@ -321,6 +321,10 @@ bool AP_Arming_Plane::arm(const AP_Arming::Method method, const bool do_arming_c
|
||||
// rising edge of delay_arming oneshot
|
||||
delay_arming = true;
|
||||
|
||||
#if MODE_AUTOLAND_ENABLED
|
||||
plane.mode_autoland.arm_check();
|
||||
#endif
|
||||
|
||||
send_arm_disarm_statustext("Throttle armed");
|
||||
|
||||
return true;
|
||||
|
||||
@ -514,6 +514,10 @@ void Plane::update_control_mode(void)
|
||||
update_fly_forward();
|
||||
|
||||
control_mode->update();
|
||||
|
||||
#if MODE_AUTOLAND_ENABLED
|
||||
mode_autoland.check_takeoff_direction();
|
||||
#endif
|
||||
}
|
||||
|
||||
|
||||
|
||||
@ -1333,6 +1333,11 @@ public:
|
||||
|
||||
#endif // AP_SCRIPTING_ENABLED
|
||||
|
||||
bool tkoff_option_is_set(AP_FixedWing::TakeoffOption option) const {
|
||||
return (aparm.takeoff_options & int32_t(option)) != 0;
|
||||
}
|
||||
|
||||
|
||||
};
|
||||
|
||||
extern Plane plane;
|
||||
|
||||
@ -26,15 +26,18 @@ bool Plane::start_command(const AP_Mission::Mission_Command& cmd)
|
||||
// reset loiter start time. New command is a new loiter
|
||||
loiter.start_time_ms = 0;
|
||||
|
||||
AP_Mission::Mission_Command next_nav_cmd;
|
||||
const uint16_t next_index = mission.get_current_nav_index() + 1;
|
||||
const bool have_next_cmd = mission.get_next_nav_cmd(next_index, next_nav_cmd);
|
||||
auto_state.wp_is_land_approach = have_next_cmd && (next_nav_cmd.id == MAV_CMD_NAV_LAND);
|
||||
// Mission lookahead is only valid in auto
|
||||
if (control_mode == &mode_auto) {
|
||||
AP_Mission::Mission_Command next_nav_cmd;
|
||||
const uint16_t next_index = mission.get_current_nav_index() + 1;
|
||||
const bool have_next_cmd = mission.get_next_nav_cmd(next_index, next_nav_cmd);
|
||||
auto_state.wp_is_land_approach = have_next_cmd && (next_nav_cmd.id == MAV_CMD_NAV_LAND);
|
||||
#if HAL_QUADPLANE_ENABLED
|
||||
if (have_next_cmd && quadplane.is_vtol_land(next_nav_cmd.id)) {
|
||||
auto_state.wp_is_land_approach = false;
|
||||
}
|
||||
if (have_next_cmd && quadplane.is_vtol_land(next_nav_cmd.id)) {
|
||||
auto_state.wp_is_land_approach = false;
|
||||
}
|
||||
#endif
|
||||
}
|
||||
}
|
||||
|
||||
switch(cmd.id) {
|
||||
@ -382,9 +385,6 @@ 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();
|
||||
}
|
||||
|
||||
@ -559,11 +559,6 @@ bool Plane::verify_takeoff()
|
||||
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) {
|
||||
@ -1173,6 +1168,13 @@ bool Plane::verify_loiter_heading(bool init)
|
||||
}
|
||||
#endif
|
||||
|
||||
#if MODE_AUTOLAND_ENABLED
|
||||
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,
|
||||
|
||||
@ -156,6 +156,11 @@ public:
|
||||
|
||||
// true if voltage correction should be applied to throttle
|
||||
virtual bool use_battery_compensation() const;
|
||||
|
||||
#if MODE_AUTOLAND_ENABLED
|
||||
// true if mode allows landing direction to be set on first takeoff after arm in this mode
|
||||
virtual bool allows_autoland_direction_capture() const { return false; }
|
||||
#endif
|
||||
|
||||
#if AP_QUICKTUNE_ENABLED
|
||||
// does this mode support VTOL quicktune?
|
||||
@ -212,6 +217,11 @@ public:
|
||||
|
||||
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
|
||||
@ -262,6 +272,11 @@ public:
|
||||
|
||||
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
|
||||
|
||||
#if AP_PLANE_GLIDER_PULLUP_ENABLED
|
||||
bool in_pullup() const { return pullup.in_pullup(); }
|
||||
#endif
|
||||
@ -308,6 +323,11 @@ public:
|
||||
|
||||
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;
|
||||
@ -449,6 +469,11 @@ public:
|
||||
// true if voltage correction should be applied to throttle
|
||||
bool use_battery_compensation() const override { return false; }
|
||||
|
||||
#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
|
||||
|
||||
};
|
||||
|
||||
|
||||
@ -495,6 +520,11 @@ public:
|
||||
|
||||
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
|
||||
|
||||
private:
|
||||
void stabilize_stick_mixing_direct();
|
||||
|
||||
@ -513,6 +543,10 @@ public:
|
||||
|
||||
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
|
||||
@ -552,6 +586,11 @@ public:
|
||||
|
||||
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 ModeFBWB : public Mode
|
||||
@ -844,6 +883,11 @@ public:
|
||||
|
||||
bool does_auto_throttle() 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
|
||||
|
||||
// var_info for holding parameter information
|
||||
static const struct AP_Param::GroupInfo var_info[];
|
||||
|
||||
@ -887,6 +931,13 @@ public:
|
||||
|
||||
bool does_auto_throttle() const override { return true; }
|
||||
|
||||
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[];
|
||||
@ -894,11 +945,29 @@ public:
|
||||
AP_Int16 final_wp_alt;
|
||||
AP_Int16 final_wp_dist;
|
||||
AP_Int16 landing_dir_off;
|
||||
AP_Int8 options;
|
||||
|
||||
// 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,
|
||||
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;
|
||||
AP_Mission::Mission_Command cmd_loiter;
|
||||
AP_Mission::Mission_Command cmd_land;
|
||||
Location land_start;
|
||||
AutoLandStage stage;
|
||||
void set_autoland_direction(const float heading);
|
||||
};
|
||||
#endif
|
||||
#if HAL_SOARING_ENABLED
|
||||
|
||||
@ -28,13 +28,20 @@ const AP_Param::GroupInfo ModeAutoLand::var_info[] = {
|
||||
|
||||
// @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.
|
||||
// @Description: The captured takeoff direction after ground course is established in autotakeoffsis offset by this amount to create a different landing direction and approach.However,if TKOFF_OPTION bit1 is set, the takeoff(landing) direction is captured immediately via compass heading upon arming, then this offset is NOT applied.
|
||||
// @Range: -360 360
|
||||
// @Increment: 1
|
||||
// @Units: deg
|
||||
// @User: Standard
|
||||
AP_GROUPINFO("DIR_OFF", 3, ModeAutoLand, landing_dir_off, 0),
|
||||
|
||||
// @Param: OPTIONS
|
||||
// @DisplayName: Autoland mode options
|
||||
// @Description: Enables optional autoland mode behaviors
|
||||
// @Bitmask: 1: When set if there is a healthy compass in use the compass heading will be captured at arming and used for the AUTOLAND mode's initial takeoff direction instead of capturing ground course in NAV_TAKEOFF or Mode TAKEOFF or other modes.
|
||||
// @User: Standard
|
||||
AP_GROUPINFO("OPTIONS", 4, ModeAutoLand, options, 0),
|
||||
|
||||
AP_GROUPEND
|
||||
};
|
||||
|
||||
@ -45,81 +52,100 @@ ModeAutoLand::ModeAutoLand() :
|
||||
}
|
||||
|
||||
bool ModeAutoLand::_enter()
|
||||
{
|
||||
{
|
||||
//must be flying to enter
|
||||
if (!plane.is_flying()) {
|
||||
gcs().send_text(MAV_SEVERITY_WARNING, "Must already be flying!");
|
||||
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
|
||||
|
||||
// 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");
|
||||
return false;
|
||||
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");
|
||||
gcs().send_text(MAV_SEVERITY_WARNING, "Takeoff initial direction not set");
|
||||
return false;
|
||||
}
|
||||
|
||||
plane.prev_WP_loc = plane.current_loc;
|
||||
plane.set_target_altitude_current();
|
||||
plane.auto_state.next_wp_crosstrack = true;
|
||||
|
||||
plane.prev_WP_loc = plane.current_loc;
|
||||
|
||||
// In flight stage normal for approach
|
||||
plane.set_flight_stage(AP_FixedWing::FlightStage::NORMAL);
|
||||
|
||||
const Location &home = ahrs.get_home();
|
||||
|
||||
#ifndef HAL_LANDING_DEEPSTALL_ENABLED
|
||||
if (plane.landing.get_type() == AP_Landing::TYPE_DEEPSTALL) {
|
||||
// Deep stall landings require only a landing location, they do there own loiter to alt and approach
|
||||
cmd_land.id = MAV_CMD_NAV_LAND;
|
||||
cmd_land.content.location = home;
|
||||
|
||||
// p1 gives the altitude from which to start the deep stall above the location alt
|
||||
cmd_land.p1 = final_wp_alt;
|
||||
plane.start_command(cmd_land);
|
||||
|
||||
stage = AutoLandStage::LANDING;
|
||||
return true;
|
||||
}
|
||||
#endif // HAL_LANDING_DEEPSTALL_ENABLED
|
||||
|
||||
/*
|
||||
landing is in 3 steps:
|
||||
1) a base leg waypoint
|
||||
2) a land start WP, with crosstrack
|
||||
3) the landing WP, with crosstrack
|
||||
Glide slope landing is in 3 steps:
|
||||
1) a loitering to alt waypoint centered on base leg
|
||||
2) exiting and proceeeing to a final approach land start WP, with crosstrack
|
||||
3) the landing WP at home, with crosstrack
|
||||
|
||||
the base leg point is 90 degrees off from the landing leg
|
||||
*/
|
||||
const Location &home = ahrs.get_home();
|
||||
|
||||
/*
|
||||
first calculate the starting waypoint we will use when doing the
|
||||
NAV_LAND. This is offset by final_wp_dist from home, in a
|
||||
direction 180 degrees from takeoff direction
|
||||
*/
|
||||
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);
|
||||
land_start = home;
|
||||
land_start.offset_bearing(plane.takeoff_state.initial_direction.heading, -final_wp_dist);
|
||||
land_start.set_alt_m(final_wp_alt, Location::AltFrame::ABOVE_HOME);
|
||||
land_start.change_alt_frame(Location::AltFrame::ABSOLUTE);
|
||||
|
||||
/*
|
||||
now create the initial target waypoint for the base leg. We
|
||||
now create the initial target waypoint for the loitering to alt centered on base leg waypoint. We
|
||||
choose if we will do a right or left turn onto the landing based
|
||||
on where we are when we enter the landing mode
|
||||
*/
|
||||
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 = final_wp_dist * 0.333;
|
||||
cmd[0].id = MAV_CMD_NAV_WAYPOINT;
|
||||
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);
|
||||
// set a 1m acceptance radius, we want to fly all the way to this waypoint
|
||||
cmd[0].p1 = 1;
|
||||
const float bearing_to_current_deg = degrees(land_start.get_bearing(plane.current_loc));
|
||||
const float bearing_err_deg = wrap_180(plane.takeoff_state.initial_direction.heading - bearing_to_current_deg);
|
||||
const float bearing_offset_deg = (bearing_err_deg > 0) ? -90 : 90;
|
||||
|
||||
/*
|
||||
create the WP for the start of the landing
|
||||
*/
|
||||
cmd[1].content.location = land_start_WP;
|
||||
cmd[1].id = MAV_CMD_NAV_WAYPOINT;
|
||||
// Try and minimize loiter radius by using the smaller of the waypoint loiter radius or 1/3 of the final WP distance
|
||||
const float loiter_radius = MIN(final_wp_dist * 0.333, fabsf(plane.aparm.loiter_radius));
|
||||
|
||||
// and the land WP
|
||||
cmd[2].id = MAV_CMD_NAV_LAND;
|
||||
cmd[2].content.location = home;
|
||||
// corrected_loiter_radius is the radius the vehicle will actually fly, this gets larger as altitude increases.
|
||||
// Strictly this gets the loiter radius at the current altitude, really we want the loiter radius at final_wp_alt.
|
||||
const float corrected_loiter_radius = plane.nav_controller->loiter_radius(loiter_radius);
|
||||
|
||||
// start first leg
|
||||
stage = 0;
|
||||
plane.start_command(cmd[0]);
|
||||
cmd_loiter.id = MAV_CMD_NAV_LOITER_TO_ALT;
|
||||
cmd_loiter.p1 = loiter_radius;
|
||||
cmd_loiter.content.location = land_start;
|
||||
cmd_loiter.content.location.offset_bearing(plane.takeoff_state.initial_direction.heading + bearing_offset_deg, corrected_loiter_radius);
|
||||
cmd_loiter.content.location.loiter_ccw = bearing_err_deg>0? 1 :0;
|
||||
|
||||
// don't crosstrack initially
|
||||
plane.auto_state.crosstrack = false;
|
||||
plane.auto_state.next_wp_crosstrack = true;
|
||||
// land WP at home
|
||||
cmd_land.id = MAV_CMD_NAV_LAND;
|
||||
cmd_land.content.location = home;
|
||||
|
||||
// start first leg toward the base leg loiter to alt point
|
||||
stage = AutoLandStage::LOITER;
|
||||
plane.start_command(cmd_loiter);
|
||||
return true;
|
||||
}
|
||||
|
||||
@ -129,30 +155,87 @@ void ModeAutoLand::update()
|
||||
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);
|
||||
// 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();
|
||||
plane.calc_throttle();
|
||||
}
|
||||
}
|
||||
|
||||
void ModeAutoLand::navigate()
|
||||
{
|
||||
if (stage == 2) {
|
||||
// we are on final landing leg
|
||||
plane.set_flight_stage(AP_FixedWing::FlightStage::LAND);
|
||||
plane.verify_command(cmd[stage]);
|
||||
return;
|
||||
}
|
||||
switch (stage) {
|
||||
case AutoLandStage::LOITER:
|
||||
// check if we have arrived and completed loiter at base leg waypoint
|
||||
if (plane.verify_loiter_to_alt(cmd_loiter)) {
|
||||
stage = AutoLandStage::LANDING;
|
||||
plane.start_command(cmd_land);
|
||||
// Crosstrack from the land start location
|
||||
plane.prev_WP_loc = land_start;
|
||||
|
||||
// see if we have completed the leg
|
||||
if (plane.verify_nav_wp(cmd[stage])) {
|
||||
stage++;
|
||||
plane.prev_WP_loc = plane.next_WP_loc;
|
||||
plane.auto_state.next_turn_angle = 90;
|
||||
plane.start_command(cmd[stage]);
|
||||
}
|
||||
break;
|
||||
|
||||
case AutoLandStage::LANDING:
|
||||
plane.set_flight_stage(AP_FixedWing::FlightStage::LAND);
|
||||
plane.verify_command(cmd_land);
|
||||
// make sure we line up
|
||||
plane.auto_state.crosstrack = true;
|
||||
plane.auto_state.next_wp_crosstrack = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
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.is_flying() &&
|
||||
hal.util->get_soft_armed() &&
|
||||
plane.gps.ground_speed() > GPS_GND_CRS_MIN_SPD) {
|
||||
set_autoland_direction(plane.gps.ground_course() + landing_dir_off);
|
||||
}
|
||||
}
|
||||
|
||||
// Sets autoland direction using ground course + offest parameter
|
||||
void ModeAutoLand::set_autoland_direction(const float heading)
|
||||
{
|
||||
plane.takeoff_state.initial_direction.heading = wrap_360(heading);
|
||||
plane.takeoff_state.initial_direction.initialized = true;
|
||||
gcs().send_text(MAV_SEVERITY_INFO, "Autoland direction= %u",int(plane.takeoff_state.initial_direction.heading));
|
||||
}
|
||||
|
||||
/*
|
||||
return true when the LOITER_TO_ALT is lined up ready for the landing, used in commands_logic verify loiter to alt
|
||||
*/
|
||||
bool ModeAutoLand::landing_lined_up(void)
|
||||
{
|
||||
// use the line between the center of the LOITER_TO_ALT on the base leg and the
|
||||
// start of the landing leg (land_start_WP)
|
||||
return plane.mode_loiter.isHeadingLinedUp(cmd_loiter.content.location, cmd_land.content.location);
|
||||
}
|
||||
|
||||
// possibly capture heading on arming 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)) {
|
||||
set_autoland_direction(plane.ahrs.yaw_sensor * 0.01);
|
||||
}
|
||||
}
|
||||
|
||||
#endif // MODE_AUTOLAND_ENABLED
|
||||
|
||||
|
||||
@ -134,9 +134,6 @@ 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
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -145,16 +142,6 @@ 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.
|
||||
|
||||
Loading…
Reference in New Issue
Block a user