Line data    Source code

       1             : #include "Copter.h"
       2             : 
       3             : Mode::_TakeOff Mode::takeoff;
       4             : _AutoTakeoff Mode::auto_takeoff;
       5             : 
       6             : // This file contains the high-level takeoff logic for Loiter, PosHold, AltHold, Sport modes.
       7             : //   The take-off can be initiated from a GCS NAV_TAKEOFF command which includes a takeoff altitude
       8             : //   A safe takeoff speed is calculated and used to calculate a time_ms
       9             : //   the pos_control target is then slowly increased until time_ms expires
      10             : 
      11           0 : bool Mode::do_user_takeoff_start_m(float takeoff_alt_m)
      12             : {
      13           0 :     copter.flightmode->takeoff.start_m(takeoff_alt_m);
      14           0 :     return true;
      15             : }
      16             : 
      17             : // initiate user takeoff - called when MAVLink TAKEOFF command is received
      18         147 : bool Mode::do_user_takeoff_U_m(float takeoff_alt_m, bool must_navigate)
      19             : {
      20         147 :     if (!copter.motors->armed()) {
      21           2 :         return false;
      22             :     }
      23         145 :     if (!copter.ap.land_complete) {
      24             :         // can't takeoff again!
      25           0 :         return false;
      26             :     }
      27         145 :     if (!has_user_takeoff(must_navigate)) {
      28             :         // this mode doesn't support user takeoff
      29           0 :         return false;
      30             :     }
      31         145 :     if (takeoff_alt_m <= copter.current_loc.alt * 0.01) {
      32             :         // can't takeoff downwards...
      33           0 :         return false;
      34             :     }
      35             : 
      36             :     // Vehicles using motor interlock should return false if motor interlock is disabled.
      37             :     // Interlock must be enabled to allow the controller to spool up the motor(s) for takeoff.
      38         145 :     if (!motors->get_interlock() && copter.ap.using_interlock) {
      39           0 :         return false;
      40             :     }
      41             : 
      42         145 :     if (!do_user_takeoff_start_m(takeoff_alt_m)) {
      43           0 :         return false;
      44             :     }
      45             : 
      46         145 :     copter.set_auto_armed(true);
      47         145 :     return true;
      48             : }
      49             : 
      50             : // start takeoff to specified altitude above home in meters
      51         165 : void Mode::_TakeOff::start_m(float alt_m)
      52             : {
      53             :     // initialise takeoff state
      54         165 :     _running = true;
      55         165 :     take_off_start_alt_m = copter.pos_control->get_pos_estimate_U_m();
      56         165 :     take_off_complete_alt_m  = take_off_start_alt_m + alt_m;
      57         165 : }
      58             : 
      59             : // stop takeoff
      60        1432 : void Mode::_TakeOff::stop()
      61             : {
      62        1432 :     _running = false;
      63             :     // Check if we have progressed far enough through the takeoff process that the
      64             :     // aircraft may have left the ground but not yet detected the climb.
      65        1432 :     if (copter.attitude_control->get_throttle_in() > copter.get_non_takeoff_throttle()) {
      66         755 :         copter.set_land_complete(false);
      67             :     }
      68        1432 : }
      69             : 
      70             : // do_pilot_takeoff_ms - controls the vertical position controller during the process of taking off
      71             : //  take off is complete when the vertical target reaches the take off altitude.
      72             : //  climb is cancelled if pilot_climb_rate_ms becomes negative
      73             : //  sets take off to complete when target altitude is within 1% of the take off altitude
      74       71751 : void Mode::_TakeOff::do_pilot_takeoff_ms(float pilot_climb_rate_ms)
      75             : {
      76             :     // return pilot_climb_rate if take-off inactive
      77       71751 :     if (!_running) {
      78           0 :         return;
      79             :     }
      80             : 
      81       71751 :     if (copter.ap.land_complete) {
      82             :         // send throttle to attitude controller with angle boost
      83       65824 :         float throttle_norm = constrain_float(copter.attitude_control->get_throttle_in() + copter.G_Dt / copter.g2.takeoff_throttle_slew_time, 0.0, 1.0);
      84       65824 :         copter.attitude_control->set_throttle_out(throttle_norm, true, 0.0);
      85             :         // tell position controller to reset alt target and reset I terms
      86       65824 :         copter.pos_control->D_init_controller();
      87       65824 :         if (throttle_norm >= MIN(copter.g2.takeoff_throttle_max, 0.9) || 
      88      131481 :             (copter.pos_control->get_estimated_accel_U_mss() >= 0.5 * copter.pos_control->D_get_max_accel_mss()) ||
      89      198206 :             (copter.pos_control->get_vel_estimate_U_ms() >= constrain_float(pilot_climb_rate_ms, copter.pos_control->get_max_speed_up_ms() * 0.1, copter.pos_control->get_max_speed_up_ms() * 0.5)) || 
      90       66560 :             (is_positive(take_off_complete_alt_m - take_off_start_alt_m) && copter.pos_control->get_pos_estimate_U_m() - take_off_start_alt_m > 0.5 * (take_off_complete_alt_m - take_off_start_alt_m))) {
      91             :             // throttle > 90%
      92             :             // acceleration > 50% maximum acceleration
      93             :             // velocity > 10% maximum velocity && commanded climb rate
      94             :             // velocity > 50% maximum velocity
      95             :             // altitude change greater than half complete alt from start off alt
      96         165 :             copter.set_land_complete(false);
      97             :         }
      98             :     } else {
      99        5927 :         float pos_d_m = -take_off_complete_alt_m;
     100        5927 :         float vel_d_ms = -pilot_climb_rate_ms;
     101             : 
     102             :         // command the aircraft to the take off altitude and current pilot climb rate
     103        5927 :         copter.pos_control->input_pos_vel_accel_D_m(pos_d_m, vel_d_ms, 0.0);
     104             : 
     105             :         // stop take off early and return if negative climb rate is commanded or we are within 0.1% of our take off altitude
     106       11854 :         if (is_negative(pilot_climb_rate_ms) ||
     107        5927 :             (take_off_complete_alt_m  - take_off_start_alt_m) * 0.999f < copter.pos_control->get_pos_desired_U_m() - take_off_start_alt_m) {
     108         165 :             stop();
     109             :         }
     110             :     }
     111             : }
     112             : 
     113             : // auto_takeoff_run - controls the vertical position controller during the process of taking off in auto modes
     114             : // auto_takeoff_complete set to true when target altitude is within 10% of the take off altitude and less than 50% max climb rate
     115     3150731 : void _AutoTakeoff::run()
     116             : {
     117     3150731 :     const auto &g2 = copter.g2;
     118     3150731 :     const auto &wp_nav = copter.wp_nav;
     119     3150731 :     auto *motors = copter.motors;
     120     3150731 :     auto *pos_control = copter.pos_control;
     121     3150731 :     auto *attitude_control = copter.attitude_control;
     122             : 
     123             :     // if not armed set throttle to zero and exit immediately
     124     3150731 :     if (!motors->armed() || !copter.ap.auto_armed) {
     125             :         // do not spool down tradheli when on the ground with motor interlock enabled
     126      244601 :         copter.flightmode->make_safe_ground_handling(copter.is_tradheli() && motors->get_interlock());
     127             :         // update auto_takeoff_no_nav_alt_m
     128      244601 :         no_nav_alt_m = pos_control->get_pos_estimate_U_m() + g2.wp_navalt_min_m;
     129      485617 :         return;
     130             :     }
     131             : 
     132             :     // get terrain offset
     133     2906130 :     float terrain_u_m = 0.0f;
     134     2906130 :     if (is_terrain_alt && !wp_nav->get_terrain_U_m(terrain_u_m)) {
     135             :         // trigger terrain failsafe
     136           0 :         copter.failsafe_terrain_on_event();
     137           0 :         return;
     138             :     }
     139             : 
     140             :     // set motors to full range
     141     2906130 :     motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
     142             : 
     143             :     // aircraft stays in landed state until rotor speed run up has finished
     144     2906130 :     if (motors->get_spool_state() != AP_Motors::SpoolState::THROTTLE_UNLIMITED) {
     145             :         // motors have not completed spool up yet so relax navigation and position controllers
     146      170331 :         pos_control->NE_relax_velocity_controller();
     147      170331 :         pos_control->NE_update_controller();
     148      170331 :         pos_control->D_relax_controller(0.0f);   // forces throttle output to decay to zero
     149      170331 :         pos_control->D_update_controller();
     150      170331 :         attitude_control->reset_yaw_target_and_rate();
     151      170331 :         attitude_control->reset_rate_controller_I_terms();
     152      170331 :         attitude_control->input_thrust_vector_rate_heading_rads(pos_control->get_thrust_vector(), 0.0);
     153             :         // update auto_takeoff_no_nav_alt_m
     154      170331 :         no_nav_alt_m = pos_control->get_pos_estimate_U_m() + g2.wp_navalt_min_m;
     155      170331 :         return;
     156             :     }
     157             :     
     158             :     // aircraft stays in landed state until vertical movement is detected or 90% throttle is reached
     159     2735799 :     if (copter.ap.land_complete) {
     160             :         // send throttle to attitude controller with angle boost
     161       70685 :         float throttle = constrain_float(copter.attitude_control->get_throttle_in() + copter.G_Dt / copter.g2.takeoff_throttle_slew_time, 0.0, 1.0);
     162       70685 :         copter.attitude_control->set_throttle_out(throttle, true, 0.0);
     163             :         // tell position controller to reset alt target and reset I terms
     164       70685 :         copter.pos_control->D_init_controller();
     165       70685 :         pos_control->NE_relax_velocity_controller();
     166       70685 :         pos_control->NE_update_controller();
     167       70685 :         attitude_control->reset_rate_controller_I_terms();
     168       70685 :         attitude_control->input_thrust_vector_rate_heading_rads(pos_control->get_thrust_vector(), 0.0);
     169       70685 :         if (throttle >= MIN(copter.g2.takeoff_throttle_max, 0.9) || 
     170       70685 :             (copter.pos_control->get_estimated_accel_U_mss() >= 0.5 * copter.pos_control->D_get_max_accel_mss()) ||
     171      211836 :             (copter.pos_control->get_vel_estimate_U_ms() >= 0.1 * copter.pos_control->get_max_speed_up_ms()) || 
     172       70466 :             ( no_nav_active && (pos_control->get_pos_estimate_U_m() >= no_nav_alt_m))) {
     173             :             // throttle > 90%
     174             :             // acceleration > 50% maximum acceleration
     175             :             // velocity > 10% maximum velocity
     176             :             // altitude change greater than half auto_takeoff_no_nav_alt_cm
     177         219 :             copter.set_land_complete(false);
     178             :         }
     179       70685 :         return;
     180             :     }
     181             : 
     182             :     // check if we are not navigating because of low altitude
     183     2665114 :     if (no_nav_active) {
     184             :         // check if vehicle has reached no_nav_alt threshold
     185           0 :         if (pos_control->get_pos_estimate_U_m() >= no_nav_alt_m) {
     186           0 :             no_nav_active = false;
     187             :         }
     188           0 :         pos_control->NE_relax_velocity_controller();
     189             :     } else {
     190     2665114 :         Vector2f vel_ne_zero;
     191     2665114 :         Vector2f accel_ne_zero;
     192     2665114 :         pos_control->input_vel_accel_NE_m(vel_ne_zero, accel_ne_zero);
     193             :     }
     194     2665114 :     pos_control->NE_update_controller();
     195             : 
     196             :     // command the aircraft to the take off altitude
     197     2665114 :     float pos_d_m = -(complete_alt_m + terrain_u_m);
     198     2665114 :     float vel_d_zero = 0.0;
     199     2665114 :     copter.pos_control->input_pos_vel_accel_D_m(pos_d_m, vel_d_zero, 0.0);
     200             :     
     201             :     // run the vertical position controller and set output throttle
     202     2665114 :     pos_control->D_update_controller();
     203             : 
     204             :     // call attitude controller with auto yaw
     205     2665114 :     attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), copter.flightmode->auto_yaw.get_heading());
     206             : 
     207             :     // takeoff complete when we are less than 1% of the stopping distance from the target altitude
     208             :     // and 10% our maximum climb rate
     209     2665114 :     const float vel_threshold_fraction = 0.1;
     210             :     // stopping distance from vel_threshold_fraction * max velocity
     211     2665114 :     const float stop_distance_m = 0.5 * sq(vel_threshold_fraction * copter.pos_control->get_max_speed_up_ms()) / copter.pos_control->D_get_max_accel_mss();
     212     2665114 :     bool reached_altitude = copter.pos_control->get_pos_desired_U_m() >= -pos_d_m - stop_distance_m;
     213     2665114 :     bool reached_climb_rate = copter.pos_control->get_vel_desired_U_ms() < copter.pos_control->get_max_speed_up_ms() * vel_threshold_fraction;
     214     2665114 :     complete = reached_altitude && reached_climb_rate;
     215             : 
     216             :     // calculate completion for location in case it is needed for a smooth transition to wp_nav
     217     2665114 :     if (complete) {
     218     1564115 :         no_nav_active = false;
     219     1564115 :         const Vector3p& _complete_pos_ned_m = copter.pos_control->get_pos_desired_NED_m();
     220     1564115 :         complete_pos_ned_m = Vector3p{_complete_pos_ned_m.x, _complete_pos_ned_m.y, pos_d_m};
     221             :     }
     222             : }
     223             : 
     224         238 : void _AutoTakeoff::start_m(float _complete_alt_m, bool _is_terrain_alt)
     225             : {
     226             :     // auto_takeoff_complete_alt_m is a problem if equal to auto_takeoff_start_alt_m
     227         238 :     complete_alt_m = _complete_alt_m;
     228         238 :     is_terrain_alt = _is_terrain_alt;
     229         238 :     complete = false;
     230             :     // initialise auto_takeoff_no_nav_alt_m
     231         238 :     const auto &g2 = copter.g2;
     232         238 :     no_nav_alt_m = copter.pos_control->get_pos_estimate_U_m() + g2.wp_navalt_min_m;
     233         238 :     if ((g2.wp_navalt_min_m > 0) && (copter.flightmode->is_disarmed_or_landed() || !copter.motors->get_interlock())) {
     234             :         // we are not flying, climb with no navigation to current alt-above-ekf-origin + wp_navalt_min_m
     235           0 :         no_nav_active = true;
     236             :     } else {
     237         238 :         no_nav_active = false;
     238             :     }
     239         238 : }
     240             : 
     241             : // return takeoff final target position in m from the EKF origin if takeoff has completed successfully
     242          58 : bool _AutoTakeoff::get_completion_pos_ned_m(Vector3p& pos_ned_m)
     243             : {
     244             :     // only provide location if takeoff has completed
     245          58 :     if (!complete) {
     246           1 :         return false;
     247             :     }
     248             : 
     249          57 :     pos_ned_m = complete_pos_ned_m;
     250          57 :     return true;
     251             : }
     252             : 
     253     2996931 : bool Mode::is_taking_off() const
     254             : {
     255     2996931 :     if (!has_user_takeoff(false)) {
     256     1866449 :         return false;
     257             :     }
     258     1130482 :     return takeoff.running();
     259             : }