ArduCopter/mode_land.cpp

   1 #include "Copter.h"
   2
   3 // land_init - initialise land controller
   4 bool ModeLand::init(bool ignore_checks)
   5 {
   6     // check if we have GPS and decide which LAND we're going to do
   7     control_position = copter.position_ok();
   8
   9     // set horizontal speed and acceleration limits
  10     pos_control->set_max_speed_accel_xy(wp_nav->get_default_speed_xy(), wp_nav->get_wp_acceleration());
  11     pos_control->set_correction_speed_accel_xy(wp_nav->get_default_speed_xy(), wp_nav->get_wp_acceleration());
  12
  13     // initialise the horizontal position controller
  14     if (control_position && !pos_control->is_active_xy()) {
  15         pos_control->init_xy_controller();
  16     }
  17
  18     // set vertical speed and acceleration limits
  19     pos_control->set_max_speed_accel_z(wp_nav->get_default_speed_down(), wp_nav->get_default_speed_up(), wp_nav->get_accel_z());
  20     pos_control->set_correction_speed_accel_z(wp_nav->get_default_speed_down(), wp_nav->get_default_speed_up(), wp_nav->get_accel_z());
  21
  22     // initialise the vertical position controller
  23     if (!pos_control->is_active_z()) {
  24         pos_control->init_z_controller();
  25     }
  26
  27     land_start_time = millis();
  28     land_pause = false;
  29
  30     // reset flag indicating if pilot has applied roll or pitch inputs during landing
  31     copter.ap.land_repo_active = false;
  32
  33     // this will be set true if prec land is later active
  34     copter.ap.prec_land_active = false;
  35
  36     // initialise yaw
  37     auto_yaw.set_mode(AutoYaw::Mode::HOLD);
  38
  39 #if AP_LANDINGGEAR_ENABLED
  40     // optionally deploy landing gear
  41     copter.landinggear.deploy_for_landing();
  42 #endif
  43
  44 #if AC_PRECLAND_ENABLED
  45     // initialise precland state machine
  46     copter.precland_statemachine.init();
  47 #endif
  48
  49     return true;
  50 }
  51
  52 // land_run - runs the land controller
  53 // should be called at 100hz or more
  54 void ModeLand::run()
  55 {
  56     if (control_position) {
  57         gps_run();
  58     } else {
  59         nogps_run();
  60     }
  61 }
  62
  63 // land_gps_run - runs the land controller
  64 //      horizontal position controlled with loiter controller
  65 //      should be called at 100hz or more
  66 void ModeLand::gps_run()
  67 {
  68     // disarm when the landing detector says we've landed
  69     if (copter.ap.land_complete && motors->get_spool_state() == AP_Motors::SpoolState::GROUND_IDLE) {
  70         copter.arming.disarm(AP_Arming::Method::LANDED);
  71     }
  72
  73     // Land State Machine Determination
  74     if (is_disarmed_or_landed()) {
  75         make_safe_ground_handling();
  76     } else {
  77         // set motors to full range
  78         motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
  79
  80         // pause before beginning land descent
  81         if (land_pause && millis()-land_start_time >= LAND_WITH_DELAY_MS) {
  82             land_pause = false;
  83         }
  84
  85         // run normal landing or precision landing (if enabled)
  86         land_run_normal_or_precland(land_pause);
  87     }
  88 }
  89
  90 // land_nogps_run - runs the land controller
  91 //      pilot controls roll and pitch angles
  92 //      should be called at 100hz or more
  93 void ModeLand::nogps_run()
  94 {
  95     float target_roll = 0.0f, target_pitch = 0.0f;
  96
  97     // process pilot inputs
  98     if (!copter.failsafe.radio) {
  99         if ((g.throttle_behavior & THR_BEHAVE_HIGH_THROTTLE_CANCELS_LAND) != 0 && copter.rc_throttle_control_in_filter.get() > LAND_CANCEL_TRIGGER_THR){
 100             LOGGER_WRITE_EVENT(LogEvent::LAND_CANCELLED_BY_PILOT);
 101             // exit land if throttle is high
 102             copter.set_mode(Mode::Number::ALT_HOLD, ModeReason::THROTTLE_LAND_ESCAPE);
 103         }
 104
 105         if (g.land_repositioning) {
 106             // apply SIMPLE mode transform to pilot inputs
 107             update_simple_mode();
 108
 109             // get pilot desired lean angles
 110             get_pilot_desired_lean_angles(target_roll, target_pitch, copter.aparm.angle_max, attitude_control->get_althold_lean_angle_max_cd());
 111         }
 112
 113     }
 114
 115     // disarm when the landing detector says we've landed
 116     if (copter.ap.land_complete && motors->get_spool_state() == AP_Motors::SpoolState::GROUND_IDLE) {
 117         copter.arming.disarm(AP_Arming::Method::LANDED);
 118     }
 119
 120     // Land State Machine Determination
 121     if (is_disarmed_or_landed()) {
 122         make_safe_ground_handling();
 123     } else {
 124         // set motors to full range
 125         motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
 126
 127         // pause before beginning land descent
 128         if (land_pause && millis()-land_start_time >= LAND_WITH_DELAY_MS) {
 129             land_pause = false;
 130         }
 131
 132         land_run_vertical_control(land_pause);
 133     }
 134
 135     // call attitude controller
 136     attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, auto_yaw.get_heading().yaw_rate_cds);
 137 }
 138
 139 // do_not_use_GPS - forces land-mode to not use the GPS but instead rely on pilot input for roll and pitch
 140 //  called during GPS failsafe to ensure that if we were already in LAND mode that we do not use the GPS
 141 //  has no effect if we are not already in LAND mode
 142 void ModeLand::do_not_use_GPS()
 143 {
 144     control_position = false;
 145 }
 146
 147 // set_mode_land_with_pause - sets mode to LAND and triggers 4 second delay before descent starts
 148 //  this is always called from a failsafe so we trigger notification to pilot
 149 void Copter::set_mode_land_with_pause(ModeReason reason)
 150 {
 151     set_mode(Mode::Number::LAND, reason);
 152     mode_land.set_land_pause(true);
 153
 154     // alert pilot to mode change
 155     AP_Notify::events.failsafe_mode_change = 1;
 156 }
 157
 158 // landing_with_GPS - returns true if vehicle is landing using GPS
 159 bool Copter::landing_with_GPS()
 160 {
 161     return (flightmode->mode_number() == Mode::Number::LAND &&
 162             mode_land.controlling_position());
 163 }