2015-05-29 23:12:49 -03:00
|
|
|
#include "Copter.h"
|
|
|
|
|
2014-01-24 02:47:42 -04:00
|
|
|
// land_init - initialise land controller
|
2019-05-09 23:18:49 -03:00
|
|
|
bool ModeLand::init(bool ignore_checks)
|
2014-01-24 02:47:42 -04:00
|
|
|
{
|
|
|
|
// check if we have GPS and decide which LAND we're going to do
|
2020-05-01 00:59:07 -03:00
|
|
|
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-01-24 02:47:42 -04:00
|
|
|
}
|
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());
|
2021-07-08 01:17:41 -03:00
|
|
|
pos_control->set_correction_speed_accel_z(wp_nav->get_default_speed_down(), wp_nav->get_default_speed_up(), wp_nav->get_accel_z());
|
2014-04-30 04:29:32 -03:00
|
|
|
|
2021-05-19 11:07:38 -03:00
|
|
|
// initialise the vertical position controller
|
2017-01-09 03:31:26 -04:00
|
|
|
if (!pos_control->is_active_z()) {
|
2021-05-11 01:42:02 -03:00
|
|
|
pos_control->init_z_controller();
|
2016-10-14 09:28:32 -03:00
|
|
|
}
|
2014-07-06 06:16:27 -03:00
|
|
|
|
2018-10-24 22:23:57 -03:00
|
|
|
land_start_time = millis();
|
2014-07-06 06:16:27 -03:00
|
|
|
land_pause = false;
|
|
|
|
|
2015-08-28 05:14:40 -03:00
|
|
|
// reset flag indicating if pilot has applied roll or pitch inputs during landing
|
2019-05-09 23:18:49 -03:00
|
|
|
copter.ap.land_repo_active = false;
|
2015-08-28 05:14:40 -03:00
|
|
|
|
2021-09-05 17:07:57 -03:00
|
|
|
// 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
|
|
|
|
2021-02-06 23:29:33 -04:00
|
|
|
#if LANDING_GEAR_ENABLED == ENABLED
|
2020-02-20 23:08:26 -04:00
|
|
|
// optionally deploy landing gear
|
|
|
|
copter.landinggear.deploy_for_landing();
|
2021-02-06 23:29:33 -04:00
|
|
|
#endif
|
2020-02-20 23:08:26 -04:00
|
|
|
|
2022-07-19 08:33:13 -03:00
|
|
|
#if AP_FENCE_ENABLED
|
2021-01-10 22:56:52 -04:00
|
|
|
// disable the fence on landing
|
2021-01-13 19:37:25 -04:00
|
|
|
copter.fence.auto_disable_fence_for_landing();
|
|
|
|
#endif
|
2021-01-10 22:56:52 -04:00
|
|
|
|
2021-07-21 16:33:37 -03:00
|
|
|
#if PRECISION_LANDING == ENABLED
|
|
|
|
// initialise precland state machine
|
|
|
|
copter.precland_statemachine.init();
|
|
|
|
#endif
|
|
|
|
|
2014-01-24 02:47:42 -04:00
|
|
|
return true;
|
|
|
|
}
|
|
|
|
|
|
|
|
// land_run - runs the land controller
|
|
|
|
// should be called at 100hz or more
|
2019-05-09 23:18:49 -03:00
|
|
|
void ModeLand::run()
|
2014-01-24 02:47:42 -04:00
|
|
|
{
|
2020-05-01 00:59:07 -03:00
|
|
|
if (control_position) {
|
2016-03-22 00:24:56 -03:00
|
|
|
gps_run();
|
2018-10-24 22:23:57 -03:00
|
|
|
} else {
|
2016-03-22 00:24:56 -03:00
|
|
|
nogps_run();
|
2014-01-24 03:23:33 -04:00
|
|
|
}
|
|
|
|
}
|
2014-01-24 02:47:42 -04:00
|
|
|
|
2017-11-15 11:19:47 -04:00
|
|
|
// land_gps_run - runs the land controller
|
2014-01-24 03:23:33 -04:00
|
|
|
// horizontal position controlled with loiter controller
|
|
|
|
// should be called at 100hz or more
|
2019-05-09 23:18:49 -03:00
|
|
|
void ModeLand::gps_run()
|
2014-01-24 03:23:33 -04:00
|
|
|
{
|
2019-02-28 05:03:23 -04:00
|
|
|
// disarm when the landing detector says we've landed
|
2019-05-09 23:18:49 -03:00
|
|
|
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);
|
2014-01-24 02:47:42 -04:00
|
|
|
}
|
2018-10-24 22:23:57 -03:00
|
|
|
|
2019-02-28 05:03:23 -04:00
|
|
|
// Land State Machine Determination
|
2019-04-08 05:15:57 -03:00
|
|
|
if (is_disarmed_or_landed()) {
|
2020-10-13 20:19:42 -03:00
|
|
|
make_safe_ground_handling();
|
2019-02-28 05:03:23 -04:00
|
|
|
} else {
|
|
|
|
// set motors to full range
|
2019-04-09 09:16:58 -03:00
|
|
|
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
|
2018-10-24 22:23:57 -03:00
|
|
|
|
2019-02-28 05:03:23 -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) {
|
2019-02-28 05:03:23 -04:00
|
|
|
land_pause = false;
|
|
|
|
}
|
2021-08-23 06:00:09 -03:00
|
|
|
|
|
|
|
// run normal landing or precision landing (if enabled)
|
2021-08-25 02:53:56 -03:00
|
|
|
land_run_normal_or_precland(land_pause);
|
2019-02-28 05:03:23 -04:00
|
|
|
}
|
2014-01-24 03:23:33 -04:00
|
|
|
}
|
|
|
|
|
|
|
|
// land_nogps_run - runs the land controller
|
|
|
|
// pilot controls roll and pitch angles
|
|
|
|
// should be called at 100hz or more
|
2019-05-09 23:18:49 -03:00
|
|
|
void ModeLand::nogps_run()
|
2014-01-24 03:23:33 -04:00
|
|
|
{
|
2014-12-03 21:25:42 -04:00
|
|
|
float target_roll = 0.0f, target_pitch = 0.0f;
|
2014-01-24 03:23:33 -04:00
|
|
|
|
2015-07-01 15:38:32 -03:00
|
|
|
// process pilot inputs
|
2018-02-07 22:21:09 -04:00
|
|
|
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){
|
2019-10-25 03:06:05 -03:00
|
|
|
AP::logger().Write_Event(LogEvent::LAND_CANCELLED_BY_PILOT);
|
2016-01-06 17:39:36 -04:00
|
|
|
// exit land if throttle is high
|
2019-10-17 00:49:22 -03:00
|
|
|
copter.set_mode(Mode::Number::ALT_HOLD, ModeReason::THROTTLE_LAND_ESCAPE);
|
2016-01-06 17:39:36 -04:00
|
|
|
}
|
|
|
|
|
2015-07-01 15:38:32 -03:00
|
|
|
if (g.land_repositioning) {
|
|
|
|
// apply SIMPLE mode transform to pilot inputs
|
|
|
|
update_simple_mode();
|
|
|
|
|
|
|
|
// get pilot desired lean angles
|
2021-09-06 06:44:18 -03:00
|
|
|
get_pilot_desired_lean_angles(target_roll, target_pitch, copter.aparm.angle_max, attitude_control->get_althold_lean_angle_max_cd());
|
2015-07-01 15:38:32 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
}
|
|
|
|
|
2019-02-28 05:03:23 -04:00
|
|
|
// disarm when the landing detector says we've landed
|
2019-05-09 23:18:49 -03:00
|
|
|
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);
|
2014-01-24 03:23:33 -04:00
|
|
|
}
|
|
|
|
|
2019-02-28 05:03:23 -04:00
|
|
|
// Land State Machine Determination
|
2019-04-08 05:15:57 -03:00
|
|
|
if (is_disarmed_or_landed()) {
|
2020-10-13 20:19:42 -03:00
|
|
|
make_safe_ground_handling();
|
2019-02-28 05:03:23 -04:00
|
|
|
} else {
|
|
|
|
// set motors to full range
|
2019-04-09 09:16:58 -03:00
|
|
|
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
|
2016-01-13 03:11:01 -04:00
|
|
|
|
2019-02-28 05:03:23 -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) {
|
2019-02-28 05:03:23 -04:00
|
|
|
land_pause = false;
|
|
|
|
}
|
2014-01-24 03:23:33 -04:00
|
|
|
|
2019-02-28 05:03:23 -04:00
|
|
|
land_run_vertical_control(land_pause);
|
2016-07-07 21:41:38 -03:00
|
|
|
}
|
|
|
|
|
2019-02-28 05:03:23 -04:00
|
|
|
// 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);
|
2018-03-19 14:26:35 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
// 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
|
2019-05-09 23:18:49 -03:00
|
|
|
void ModeLand::do_not_use_GPS()
|
2018-03-19 14:26:35 -03:00
|
|
|
{
|
2020-05-01 00:59:07 -03:00
|
|
|
control_position = false;
|
2016-07-07 21:41:38 -03:00
|
|
|
}
|
|
|
|
|
2014-07-06 06:16:27 -03:00
|
|
|
// set_mode_land_with_pause - sets mode to LAND and triggers 4 second delay before descent starts
|
2014-12-14 23:48:54 -04:00
|
|
|
// this is always called from a failsafe so we trigger notification to pilot
|
2019-10-17 00:49:22 -03:00
|
|
|
void Copter::set_mode_land_with_pause(ModeReason reason)
|
2014-07-06 06:16:27 -03:00
|
|
|
{
|
2019-09-07 20:33:56 -03:00
|
|
|
set_mode(Mode::Number::LAND, reason);
|
2020-05-01 00:59:07 -03:00
|
|
|
mode_land.set_land_pause(true);
|
2014-12-14 23:48:54 -04:00
|
|
|
|
|
|
|
// alert pilot to mode change
|
|
|
|
AP_Notify::events.failsafe_mode_change = 1;
|
2014-07-06 06:16:27 -03:00
|
|
|
}
|
2014-09-15 21:16:41 -03:00
|
|
|
|
|
|
|
// landing_with_GPS - returns true if vehicle is landing using GPS
|
2018-03-31 02:15:45 -03:00
|
|
|
bool Copter::landing_with_GPS()
|
|
|
|
{
|
2020-05-01 00:59:07 -03:00
|
|
|
return (flightmode->mode_number() == Mode::Number::LAND &&
|
|
|
|
mode_land.controlling_position());
|
2014-09-15 21:16:41 -03:00
|
|
|
}
|