ardupilot/ArduCopter/mode_land.cpp

169 lines
5.6 KiB
C++
Raw Normal View History

#include "Copter.h"
// land_init - initialise land controller
bool ModeLand::init(bool ignore_checks)
{
// check if we have GPS and decide which LAND we're going to do
control_position = copter.position_ok();
2022-02-22 09:23:00 -04:00
// set horizontal speed and acceleration limits
pos_control->set_max_speed_accel_xy(wp_nav->get_default_speed_xy(), wp_nav->get_wp_acceleration());
pos_control->set_correction_speed_accel_xy(wp_nav->get_default_speed_xy(), wp_nav->get_wp_acceleration());
// initialise the horizontal position controller
if (control_position && !pos_control->is_active_xy()) {
pos_control->init_xy_controller();
}
2014-02-15 04:37:24 -04:00
2021-05-19 11:07:38 -03:00
// set vertical speed and acceleration limits
2021-05-11 01:42:02 -03:00
pos_control->set_max_speed_accel_z(wp_nav->get_default_speed_down(), wp_nav->get_default_speed_up(), wp_nav->get_accel_z());
pos_control->set_correction_speed_accel_z(wp_nav->get_default_speed_down(), wp_nav->get_default_speed_up(), wp_nav->get_accel_z());
2021-05-19 11:07:38 -03:00
// initialise the vertical position controller
if (!pos_control->is_active_z()) {
2021-05-11 01:42:02 -03:00
pos_control->init_z_controller();
}
2018-10-24 22:23:57 -03:00
land_start_time = millis();
land_pause = false;
// reset flag indicating if pilot has applied roll or pitch inputs during landing
copter.ap.land_repo_active = false;
// this will be set true if prec land is later active
copter.ap.prec_land_active = false;
2019-09-10 01:38:32 -03:00
// initialise yaw
2022-09-24 11:12:33 -03:00
auto_yaw.set_mode(AutoYaw::Mode::HOLD);
2019-09-10 01:38:32 -03:00
#if AP_LANDINGGEAR_ENABLED
// optionally deploy landing gear
copter.landinggear.deploy_for_landing();
#endif
#if AP_FENCE_ENABLED
// disable the fence on landing
copter.fence.auto_disable_fence_for_landing();
#endif
#if AC_PRECLAND_ENABLED
// initialise precland state machine
copter.precland_statemachine.init();
#endif
return true;
}
// land_run - runs the land controller
// should be called at 100hz or more
void ModeLand::run()
{
if (control_position) {
gps_run();
2018-10-24 22:23:57 -03:00
} else {
nogps_run();
}
}
// land_gps_run - runs the land controller
// horizontal position controlled with loiter controller
// should be called at 100hz or more
void ModeLand::gps_run()
{
// disarm when the landing detector says we've landed
if (copter.ap.land_complete && motors->get_spool_state() == AP_Motors::SpoolState::GROUND_IDLE) {
2020-02-21 09:09:57 -04:00
copter.arming.disarm(AP_Arming::Method::LANDED);
}
2018-10-24 22:23:57 -03:00
// Land State Machine Determination
2019-04-08 05:15:57 -03:00
if (is_disarmed_or_landed()) {
make_safe_ground_handling();
} else {
// set motors to full range
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
2018-10-24 22:23:57 -03:00
// pause before beginning land descent
2021-05-12 01:40:33 -03:00
if (land_pause && millis()-land_start_time >= LAND_WITH_DELAY_MS) {
land_pause = false;
}
// run normal landing or precision landing (if enabled)
land_run_normal_or_precland(land_pause);
}
}
// land_nogps_run - runs the land controller
// pilot controls roll and pitch angles
// should be called at 100hz or more
void ModeLand::nogps_run()
{
float target_roll = 0.0f, target_pitch = 0.0f;
// process pilot inputs
if (!copter.failsafe.radio) {
if ((g.throttle_behavior & THR_BEHAVE_HIGH_THROTTLE_CANCELS_LAND) != 0 && copter.rc_throttle_control_in_filter.get() > LAND_CANCEL_TRIGGER_THR){
AP::logger().Write_Event(LogEvent::LAND_CANCELLED_BY_PILOT);
// exit land if throttle is high
copter.set_mode(Mode::Number::ALT_HOLD, ModeReason::THROTTLE_LAND_ESCAPE);
}
if (g.land_repositioning) {
// apply SIMPLE mode transform to pilot inputs
update_simple_mode();
// get pilot desired lean angles
get_pilot_desired_lean_angles(target_roll, target_pitch, copter.aparm.angle_max, attitude_control->get_althold_lean_angle_max_cd());
}
}
// disarm when the landing detector says we've landed
if (copter.ap.land_complete && motors->get_spool_state() == AP_Motors::SpoolState::GROUND_IDLE) {
2020-02-21 09:09:57 -04:00
copter.arming.disarm(AP_Arming::Method::LANDED);
}
// Land State Machine Determination
2019-04-08 05:15:57 -03:00
if (is_disarmed_or_landed()) {
make_safe_ground_handling();
} else {
// set motors to full range
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
2016-01-13 03:11:01 -04:00
// pause before beginning land descent
2021-05-12 01:40:33 -03:00
if (land_pause && millis()-land_start_time >= LAND_WITH_DELAY_MS) {
land_pause = false;
}
land_run_vertical_control(land_pause);
}
// call attitude controller
2022-09-24 11:12:33 -03:00
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, auto_yaw.get_heading().yaw_rate_cds);
}
// do_not_use_GPS - forces land-mode to not use the GPS but instead rely on pilot input for roll and pitch
// called during GPS failsafe to ensure that if we were already in LAND mode that we do not use the GPS
// has no effect if we are not already in LAND mode
void ModeLand::do_not_use_GPS()
{
control_position = false;
}
// set_mode_land_with_pause - sets mode to LAND and triggers 4 second delay before descent starts
// this is always called from a failsafe so we trigger notification to pilot
void Copter::set_mode_land_with_pause(ModeReason reason)
{
set_mode(Mode::Number::LAND, reason);
mode_land.set_land_pause(true);
// alert pilot to mode change
AP_Notify::events.failsafe_mode_change = 1;
}
// landing_with_GPS - returns true if vehicle is landing using GPS
2018-03-31 02:15:45 -03:00
bool Copter::landing_with_GPS()
{
return (flightmode->mode_number() == Mode::Number::LAND &&
mode_land.controlling_position());
}