Line data    Source code

       1             : #include "Copter.h"
       2             : 
       3             : // table of user settable parameters
       4             : const AP_Param::GroupInfo ModeLand::var_info[] = {
       5             : 
       6             :     // @Param: SPD_MS
       7             :     // @DisplayName: Land speed
       8             :     // @Description: The descent speed for the final stage of landing in m/s
       9             :     // @Units: m/s
      10             :     // @Range: 0.3 2
      11             :     // @Increment: 0.1
      12             :     // @User: Standard
      13             :     AP_GROUPINFO("SPD_MS", 1, ModeLand, land_speed_ms, LAND_SPD_MS_DEFAULT),
      14             : 
      15             :     // @Param: SPD_HIGH_MS
      16             :     // @DisplayName: Land speed high
      17             :     // @Description: The descent speed for the first stage of landing in m/s. If this is zero then WP_SPD_DN is used
      18             :     // @Units: m/s
      19             :     // @Range: 0 5
      20             :     // @Increment: 0.1
      21             :     // @User: Standard
      22             :     AP_GROUPINFO("SPD_HIGH_MS", 2, ModeLand, land_speed_high_ms, 0),
      23             : 
      24             :     // @Param: ALT_LOW_M
      25             :     // @DisplayName: Land alt low
      26             :     // @Description: Altitude during Landing at which vehicle slows to LAND_SPD_MS
      27             :     // @Units: m
      28             :     // @Range: 1 100
      29             :     // @Increment: 0.1
      30             :     // @User: Advanced
      31             :     AP_GROUPINFO("ALT_LOW_M", 3, ModeLand, land_alt_low_m, 10),
      32             : 
      33             :     AP_GROUPEND
      34             : };
      35             : 
      36             : // constructor
      37        1001 : ModeLand::ModeLand() : Mode()
      38             : {
      39             :     // load parameter defaults
      40        1001 :     AP_Param::setup_object_defaults(this, var_info);
      41        1001 : }
      42             : 
      43             : // convert parameters
      44        1001 : void ModeLand::convert_params()
      45             : {
      46             :     // PARAMETER_CONVERSION - Added: Jan-2026 for ArduPilot-4.7
      47             : 
      48             :     // return immediately if parameter conversion has already been performed
      49        1001 :     if (land_speed_ms.configured() || land_speed_high_ms.configured() || land_alt_low_m.configured()) {
      50           9 :         return;
      51             :     }
      52             : 
      53             :     static const AP_Param::ConversionInfo conversion_info[] = {
      54             :         { Parameters::k_param_land_speed_cms, 0, AP_PARAM_INT16, "LAND_SPD_MS" },     // LAND_SPEED moved to LAND_SPD_MS
      55             :         { Parameters::k_param_land_speed_high_cms, 0, AP_PARAM_INT16, "LAND_SPD_HIGH_MS" },   // LAND_SPEED_HIGH moved to LAND_SPD_HIGH_MS
      56             :         { Parameters::k_param_g2, 25, AP_PARAM_INT16, "LAND_ALT_LOW_M" },  // LAND_ALT_LOW moved to LAND_ALT_LOW_M
      57             :     };
      58         992 :     AP_Param::convert_old_parameters_scaled(conversion_info, ARRAY_SIZE(conversion_info), 0.01, 0);
      59             : }
      60             : 
      61             : // land_init - initialise land controller
      62         102 : bool ModeLand::init(bool ignore_checks)
      63             : {
      64             :     // check if we have GPS and decide which LAND we're going to do
      65         102 :     control_position = copter.position_ok();
      66             : 
      67             :     // set horizontal speed and acceleration limits
      68         102 :     pos_control->NE_set_max_speed_accel_m(wp_nav->get_default_speed_NE_ms(), wp_nav->get_wp_acceleration_mss());
      69         102 :     pos_control->NE_set_correction_speed_accel_m(wp_nav->get_default_speed_NE_ms(), wp_nav->get_wp_acceleration_mss());
      70             : 
      71             :     // initialise the horizontal position controller
      72         102 :     if (control_position && !pos_control->NE_is_active()) {
      73          22 :         pos_control->NE_init_controller();
      74             :     }
      75             : 
      76             :     // set vertical speed and acceleration limits
      77         102 :     pos_control->D_set_max_speed_accel_m(wp_nav->get_default_speed_down_ms(), wp_nav->get_default_speed_up_ms(), wp_nav->get_accel_D_mss());
      78         102 :     pos_control->D_set_correction_speed_accel_m(wp_nav->get_default_speed_down_ms(), wp_nav->get_default_speed_up_ms(), wp_nav->get_accel_D_mss());
      79             : 
      80             :     // initialise the vertical position controller
      81         102 :     if (!pos_control->D_is_active()) {
      82          10 :         pos_control->D_init_controller();
      83             :     }
      84             : 
      85         102 :     land_start_time = millis();
      86         102 :     land_pause = false;
      87             : 
      88             :     // reset flag indicating if pilot has applied roll or pitch inputs during landing
      89         102 :     copter.ap.land_repo_active = false;
      90             : 
      91             :     // this will be set true if prec land is later active
      92         102 :     copter.ap.prec_land_active = false;
      93             : 
      94             :     // initialise yaw
      95         102 :     auto_yaw.set_mode(AutoYaw::Mode::HOLD);
      96             : 
      97             : #if AP_LANDINGGEAR_ENABLED
      98             :     // optionally deploy landing gear
      99         102 :     copter.landinggear.deploy_for_landing();
     100             : #endif
     101             : 
     102             : #if AC_PRECLAND_ENABLED
     103             :     // initialise precland state machine
     104         102 :     copter.precland_statemachine.init();
     105             : #endif
     106             : 
     107         102 :     return true;
     108             : }
     109             : 
     110             : // land_run - runs the land controller
     111             : // should be called at 100hz or more
     112     1101496 : void ModeLand::run()
     113             : {
     114     1101496 :     if (control_position) {
     115     1000133 :         gps_run();
     116             :     } else {
     117      101363 :         nogps_run();
     118             :     }
     119     1101496 : }
     120             : 
     121             : // land_gps_run - runs the land controller
     122             : //      horizontal position controlled with loiter controller
     123             : //      should be called at 100hz or more
     124     1000133 : void ModeLand::gps_run()
     125             : {
     126             :     // disarm when the landing detector says we've landed
     127     1000133 :     if (copter.ap.land_complete && motors->get_spool_state() == AP_Motors::SpoolState::GROUND_IDLE) {
     128          86 :         copter.arming.disarm(AP_Arming::Method::LANDED);
     129             :     }
     130             : 
     131             :     // Land State Machine Determination
     132     1000133 :     if (is_disarmed_or_landed()) {
     133      133866 :         make_safe_ground_handling();
     134             :     } else {
     135             :         // set motors to full range
     136      866267 :         motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
     137             : 
     138             :         // pause before beginning land descent
     139      866267 :         if (land_pause && millis()-land_start_time >= LAND_WITH_DELAY_MS) {
     140          11 :             land_pause = false;
     141             :         }
     142             : 
     143             :         // run normal landing or precision landing (if enabled)
     144      866267 :         land_run_normal_or_precland(land_pause);
     145             :     }
     146     1000133 : }
     147             : 
     148             : // land_nogps_run - runs the land controller
     149             : //      pilot controls roll and pitch angles
     150             : //      should be called at 100hz or more
     151      101363 : void ModeLand::nogps_run()
     152             : {
     153      101363 :     float target_roll_rad = 0.0f, target_pitch_rad = 0.0f;
     154             : 
     155             :     // process pilot inputs
     156      101363 :     if (rc().has_valid_input()) {
     157             : #if MODE_ALTHOLD_ENABLED
     158       31793 :         if ((g.throttle_behavior & THR_BEHAVE_HIGH_THROTTLE_CANCELS_LAND) != 0 && copter.rc_throttle_control_in_filter.get() > LAND_CANCEL_TRIGGER_THR){
     159           0 :             LOGGER_WRITE_EVENT(LogEvent::LAND_CANCELLED_BY_PILOT);
     160             :             // exit land if throttle is high
     161           0 :             copter.set_mode(Mode::Number::ALT_HOLD, ModeReason::THROTTLE_LAND_ESCAPE);
     162             :         }
     163             : #endif
     164             : 
     165       31793 :         if (g.land_repositioning) {
     166             :             // get pilot desired lean angles
     167       31793 :             get_pilot_desired_lean_angles_rad(target_roll_rad, target_pitch_rad, attitude_control->lean_angle_max_rad(), attitude_control->get_althold_lean_angle_max_rad());
     168             :         }
     169             :     }
     170             : 
     171             :     // disarm when the landing detector says we've landed
     172      101363 :     if (copter.ap.land_complete && motors->get_spool_state() == AP_Motors::SpoolState::GROUND_IDLE) {
     173           4 :         copter.arming.disarm(AP_Arming::Method::LANDED);
     174             :     }
     175             : 
     176             :     // Land State Machine Determination
     177      101363 :     if (is_disarmed_or_landed()) {
     178       12423 :         make_safe_ground_handling();
     179             :     } else {
     180             :         // set motors to full range
     181       88940 :         motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
     182             : 
     183             :         // pause before beginning land descent
     184       88940 :         if (land_pause && millis()-land_start_time >= LAND_WITH_DELAY_MS) {
     185           5 :             land_pause = false;
     186             :         }
     187             : 
     188       88940 :         land_run_vertical_control(land_pause);
     189             :     }
     190             : 
     191             :     // call attitude controller
     192      101363 :     attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw_rad(target_roll_rad, target_pitch_rad, auto_yaw.get_heading().yaw_rate_rads);
     193      101363 : }
     194             : 
     195             : // do_not_use_GPS - forces land-mode to not use the GPS but instead rely on pilot input for roll and pitch
     196             : //  called during GPS failsafe to ensure that if we were already in LAND mode that we do not use the GPS
     197             : //  has no effect if we are not already in LAND mode
     198           0 : void ModeLand::do_not_use_GPS()
     199             : {
     200           0 :     control_position = false;
     201           0 : }
     202             : 
     203             : // returns true if pilot's yaw input should be used to adjust vehicle's heading
     204     4299913 : bool ModeLand::use_pilot_yaw() const
     205             : {
     206             :     // only accept yaw input if repositioning is enabled
     207     4299913 :     return g.land_repositioning;
     208             : }
     209             : 
     210             : // set_mode_land_with_pause - sets mode to LAND and triggers 4 second delay before descent starts
     211             : //  this is always called from a failsafe so we trigger notification to pilot
     212          17 : void Copter::set_mode_land_with_pause(ModeReason reason)
     213             : {
     214          17 :     set_mode(Mode::Number::LAND, reason);
     215          17 :     mode_land.set_land_pause(true);
     216             : 
     217             :     // alert pilot to mode change
     218          17 :     AP_Notify::events.failsafe_mode_change = 1;
     219          17 : }
     220             : 
     221             : // landing_with_GPS - returns true if vehicle is landing using GPS
     222      715778 : bool Copter::landing_with_GPS()
     223             : {
     224      770806 :     return (flightmode->mode_number() == Mode::Number::LAND &&
     225      770806 :             mode_land.controlling_position());
     226             : }