Line data    Source code

       1             : #include "Plane.h"
       2             : 
       3             : /*   Check for automatic takeoff conditions being met using the following sequence:
       4             :  *   1) Check for adequate GPS lock - if not return false
       5             :  *   2) Check the gravity compensated longitudinal acceleration against the threshold and start the timer if true
       6             :  *   3) Wait until the timer has reached the specified value (increments of 0.1 sec) and then check the GPS speed against the threshold
       7             :  *   4) If the GPS speed is above the threshold and the attitude is within limits then return true and reset the timer
       8             :  *   5) If the GPS speed and attitude within limits has not been achieved after 2.5 seconds, return false and reset the timer
       9             :  *   6) If the time lapsed since the last timecheck is greater than 0.2 seconds, return false and reset the timer
      10             :  *   NOTE : This function relies on the TECS 50Hz processing for its acceleration measure.
      11             :  */
      12       36894 : bool Plane::auto_takeoff_check(void)
      13             : {
      14             :     // this is a more advanced check that relies on TECS
      15       36894 :     uint32_t now = millis();
      16       36894 :     uint16_t wait_time_ms = MIN(uint16_t(g.takeoff_throttle_delay)*100,12700);
      17             : 
      18             :     // reset all takeoff state if disarmed
      19       36894 :     if (!arming.is_armed_and_safety_off()) {
      20       34789 :         memset(&takeoff_state, 0, sizeof(takeoff_state));
      21       34789 :         auto_state.baro_takeoff_alt = barometer.get_altitude();
      22       34789 :         return false;
      23             :     }
      24             : 
      25             :     // Reset states if process has been interrupted, except initial_direction.initialized if set
      26             : #if MODE_AUTOLAND_ENABLED
      27        2105 :     bool takeoff_dir_initialized = takeoff_state.initial_direction.initialized;
      28        2105 :     float takeoff_dir = takeoff_state.initial_direction.heading;
      29             : #endif
      30        2105 :      if (takeoff_state.last_check_ms && (now - takeoff_state.last_check_ms) > 200) {
      31           7 :          memset(&takeoff_state, 0, sizeof(takeoff_state));
      32             : #if MODE_AUTOLAND_ENABLED
      33           7 :          takeoff_state.initial_direction.initialized = takeoff_dir_initialized; //restore dir init state
      34           7 :          takeoff_state.initial_direction.heading = takeoff_dir;
      35             : #endif
      36           7 :          return false;
      37             :      }
      38        2098 :     takeoff_state.last_check_ms = now;
      39             :     
      40             :     //check if waiting for rudder neutral after rudder arm
      41        2666 :     if (plane.arming.last_arm_method() == AP_Arming::Method::RUDDER &&
      42         568 :         !rc().seen_neutral_rudder()) {
      43             :         // we were armed with rudder but have not seen rudder neutral yet
      44         545 :         takeoff_state.waiting_for_rudder_neutral = true;
      45             :         // warn if we have been waiting a long time
      46         545 :         if (now - takeoff_state.rudder_takeoff_warn_ms > TAKEOFF_RUDDER_WARNING_TIMEOUT) {
      47           3 :             gcs().send_text(MAV_SEVERITY_WARNING, "Takeoff waiting for rudder release");
      48           3 :             takeoff_state.rudder_takeoff_warn_ms = now;
      49             :         }
      50             :         // since we are still waiting, dont takeoff
      51         545 :         return false;
      52             :     } else {
      53             :        // we did not arm by rudder or rudder has returned to neutral
      54             :        // make sure we dont indicate we are in the waiting state with servo position indicator
      55        1553 :        takeoff_state.waiting_for_rudder_neutral = false;
      56             :     }  
      57             : 
      58             :     // Check for bad GPS
      59        1553 :     if (gps.status() < AP_GPS_FixType::FIX_3D) {
      60             :         // no auto takeoff without GPS lock
      61           0 :         return false;
      62             :     }
      63             : 
      64        1553 :     bool do_takeoff_attitude_check = !(flight_option_enabled(FlightOptions::DISABLE_TOFF_ATTITUDE_CHK));
      65             : #if HAL_QUADPLANE_ENABLED
      66             :     // disable attitude check on tailsitters
      67        1553 :     do_takeoff_attitude_check &= !quadplane.tailsitter.enabled();
      68             : #endif
      69             : 
      70        1553 :     if (!takeoff_state.launchTimerStarted && !is_zero(g.takeoff_throttle_min_accel)) {
      71             :         // we are requiring an X acceleration event to launch
      72         280 :         float xaccel = TECS_controller.get_VXdot();
      73         280 :         if (g2.takeoff_throttle_accel_count <= 1) {
      74         280 :             if (xaccel < g.takeoff_throttle_min_accel) {
      75         272 :                 goto no_launch;
      76             :             }
      77             :         } else {
      78             :             // we need multiple accel events
      79           0 :             if (now - takeoff_state.accel_event_ms > 500) {
      80           0 :                 takeoff_state.accel_event_counter = 0;
      81             :             }
      82           0 :             bool odd_event = ((takeoff_state.accel_event_counter & 1) != 0);
      83           0 :             bool got_event = (odd_event?xaccel < -g.takeoff_throttle_min_accel : xaccel > g.takeoff_throttle_min_accel);
      84           0 :             if (got_event) {
      85           0 :                 takeoff_state.accel_event_counter++;
      86           0 :                 takeoff_state.accel_event_ms = now;
      87             :             }
      88           0 :             if (takeoff_state.accel_event_counter < g2.takeoff_throttle_accel_count) {
      89           0 :                 goto no_launch;
      90             :             }
      91             :         }
      92             :     }
      93             : 
      94             :     // we've reached the acceleration threshold, so start the timer
      95        1281 :     if (!takeoff_state.launchTimerStarted) {
      96         128 :         takeoff_state.launchTimerStarted = true;
      97         128 :         takeoff_state.last_tkoff_arm_time = now;
      98         128 :         if (now - takeoff_state.last_report_ms > 2000) {
      99         206 :             gcs().send_text(MAV_SEVERITY_INFO, "Armed AUTO, xaccel = %.1f m/s/s, waiting %.1f sec",
     100         103 :                               (double)TECS_controller.get_VXdot(), (double)(wait_time_ms*0.001f));
     101         103 :             takeoff_state.last_report_ms = now;
     102             :         }
     103             :     }
     104             : 
     105             :     // Only perform velocity check if not timed out
     106        1281 :     if ((now - takeoff_state.last_tkoff_arm_time) > wait_time_ms+100U) {
     107           5 :         if (now - takeoff_state.last_report_ms > 2000) {
     108           0 :             gcs().send_text(MAV_SEVERITY_WARNING, "Timeout AUTO");
     109           0 :             takeoff_state.last_report_ms = now;
     110             :         }
     111           5 :         goto no_launch;
     112             :     }
     113             : 
     114        1276 :     if (do_takeoff_attitude_check) {
     115             :         // Check aircraft attitude for bad launch
     116        2552 :         if (ahrs.pitch_sensor <= -3000 || ahrs.pitch_sensor >= 4500 ||
     117        1276 :             (!fly_inverted() && labs(ahrs.roll_sensor) > 3000)) {
     118          21 :             gcs().send_text(MAV_SEVERITY_WARNING, "Bad launch AUTO");
     119          21 :             takeoff_state.accel_event_counter = 0;
     120          21 :             goto no_launch;
     121             :         }
     122             :     }
     123             : 
     124             :     // Check ground speed and time delay
     125        2426 :     if (((gps.ground_speed() > g.takeoff_throttle_min_speed || is_zero(g.takeoff_throttle_min_speed))) &&
     126        1171 :         ((now - takeoff_state.last_tkoff_arm_time) >= wait_time_ms)) {
     127         102 :         gcs().send_text(MAV_SEVERITY_INFO, "Triggered AUTO. GPS speed = %.1f", (double)gps.ground_speed());
     128         102 :         takeoff_state.launchTimerStarted = false;
     129         102 :         takeoff_state.last_tkoff_arm_time = 0;
     130         102 :         takeoff_state.start_time_ms = now;
     131         102 :         takeoff_state.level_off_start_time_ms = 0;
     132         102 :         takeoff_state.throttle_max_timer_ms = now;
     133         102 :         steer_state.locked_course_err = 0; // use current heading without any error offset
     134         102 :         return true;
     135             :     }
     136             : 
     137             :     // we're not launching yet, but the timer is still going
     138        1153 :     return false;
     139             : 
     140         298 : no_launch:
     141         298 :     takeoff_state.launchTimerStarted = false;
     142         298 :     takeoff_state.last_tkoff_arm_time = 0;
     143         298 :     return false;
     144             : }
     145             : 
     146             : /*
     147             :   calculate desired bank angle during takeoff, setting nav_roll_cd
     148             :  */
     149       71460 : void Plane::takeoff_calc_roll(void)
     150             : {
     151       71460 :     if (steer_state.hold_course_cd == -1) {
     152             :         // we don't yet have a heading to hold - just level
     153             :         // the wings until we get up enough speed to get a GPS heading
     154       35475 :         nav_roll_cd = 0;
     155       35475 :         return;
     156             :     }
     157             : 
     158       35985 :     calc_nav_roll();
     159             : 
     160             :     // during takeoff use the level flight roll limit to prevent large
     161             :     // wing strike. Slowly allow for more roll as we get higher above
     162             :     // the takeoff altitude
     163       35985 :     int32_t takeoff_roll_limit_cd = roll_limit_cd;
     164             : 
     165       35985 :     if (auto_state.highest_airspeed < g.takeoff_rotate_speed) {
     166             :         // before Vrotate (aka, on the ground)
     167         383 :         takeoff_roll_limit_cd = g.level_roll_limit * 100;
     168             :     } else {
     169             :         // lim1 - below altitude TKOFF_LVL_ALT, restrict roll to LEVEL_ROLL_LIMIT
     170             :         // lim2 - above altitude (TKOFF_LVL_ALT * 3) allow full flight envelope of ROLL_LIMIT_DEG
     171             :         // In between lim1 and lim2 use a scaled roll limit.
     172             :         // The *3 scheme should scale reasonably with both small and large aircraft
     173       35602 :         const float lim1 = MAX(mode_takeoff.level_alt, 0);
     174       35602 :         const float lim2 = MIN(mode_takeoff.level_alt*3, mode_takeoff.target_alt);
     175       35602 :         const float current_baro_alt = barometer.get_altitude();
     176             : 
     177       35602 :         takeoff_roll_limit_cd = linear_interpolate(g.level_roll_limit*100, roll_limit_cd,
     178             :                                         current_baro_alt,
     179       35602 :                                         auto_state.baro_takeoff_alt+lim1, auto_state.baro_takeoff_alt+lim2);
     180             :     }
     181             : 
     182       35985 :     nav_roll_cd = constrain_int32(nav_roll_cd, -takeoff_roll_limit_cd, takeoff_roll_limit_cd);
     183             : }
     184             : 
     185             :         
     186             : /*
     187             :   calculate desired pitch angle during takeoff, setting nav_pitch_cd
     188             :  */
     189       71460 : void Plane::takeoff_calc_pitch(void)
     190             : {
     191             :     // First see if TKOFF_ROTATE_SPD applies.
     192             :     // This will set the pitch for the first portion of the takeoff, up until cruise speed is reached.
     193       71460 :     if (!auto_state.rotation_complete && g.takeoff_rotate_speed > 0) {
     194             :         // A non-zero rotate speed is recommended for ground takeoffs.
     195        2739 :         if (auto_state.highest_airspeed < g.takeoff_rotate_speed) {
     196             :             // We have not reached rotate speed, use the specified takeoff target pitch angle.
     197        2420 :             nav_pitch_cd = int32_t(100.0f * mode_takeoff.ground_pitch);
     198        2420 :             TECS_controller.set_pitch_min(0.01f*nav_pitch_cd);
     199        2420 :             TECS_controller.set_pitch_max(0.01f*nav_pitch_cd);
     200        2420 :             return;
     201         319 :         } else if (gps.ground_speed() <= (float)aparm.airspeed_cruise) {
     202             :             // If rotate speed applied, gradually transition from TKOFF_GND_PITCH to the climb angle.
     203             :             // This is recommended for ground takeoffs, so delay rotation until ground speed indicates adequate airspeed.
     204         314 :             const uint16_t min_pitch_cd = 500; // Set a minimum of 5 deg climb angle.
     205         314 :             nav_pitch_cd = (gps.ground_speed() / (float)aparm.airspeed_cruise) * auto_state.takeoff_pitch_cd;
     206         314 :             nav_pitch_cd = constrain_int32(nav_pitch_cd, min_pitch_cd, auto_state.takeoff_pitch_cd); 
     207         314 :             TECS_controller.set_pitch_min(0.01f*nav_pitch_cd);
     208         314 :             TECS_controller.set_pitch_max(0.01f*nav_pitch_cd);
     209         314 :             return;
     210             :         }
     211             :     }
     212       68726 :     auto_state.rotation_complete = true;
     213             : 
     214             :     // We are now past the rotation.
     215             :     // Initialize pitch limits for TECS.
     216       68726 :     int16_t pitch_min_cd = get_takeoff_pitch_min_cd();
     217       68726 :     bool pitch_clipped_max = false;
     218             : 
     219             :     // If we're using an airspeed sensor, we consult TECS.
     220       68726 :     if (ahrs.using_airspeed_sensor()) {
     221       63698 :         calc_nav_pitch();
     222             :         // At any rate, we don't want to go lower than the minimum pitch bound.
     223       63698 :         if (nav_pitch_cd < pitch_min_cd) {
     224       28788 :             nav_pitch_cd = pitch_min_cd;
     225             :         }
     226             :     } else {
     227             :         // If not, we will use the minimum allowed angle.
     228        5028 :         nav_pitch_cd = pitch_min_cd;
     229             : 
     230        5028 :         pitch_clipped_max = true;
     231             :     }
     232             : 
     233             :     // Check if we have trouble with roll control.
     234       68726 :     if (aparm.stall_prevention != 0) {
     235             :         // during takeoff we want to prioritise roll control over
     236             :         // pitch. Apply a reduction in pitch demand if our roll is
     237             :         // significantly off. The aim of this change is to
     238             :         // increase the robustness of hand launches, particularly
     239             :         // in cross-winds. If we start to roll over then we reduce
     240             :         // pitch demand until the roll recovers
     241       68726 :         float roll_error_rad = radians(constrain_float(fabsf(rollController.get_angle_error_deg()), 0.0, 90.0));
     242       68726 :         float reduction = sq(cosf(roll_error_rad));
     243       68726 :         nav_pitch_cd *= reduction;
     244             : 
     245       68726 :         if (nav_pitch_cd < pitch_min_cd) {
     246       36132 :             pitch_min_cd = nav_pitch_cd;
     247             :         }
     248             :     }
     249             :     // Notify TECS about the external pitch setting, for the next iteration.
     250       68726 :     TECS_controller.set_pitch_min(0.01f*pitch_min_cd);
     251       68726 :     if (pitch_clipped_max) {TECS_controller.set_pitch_max(0.01f*nav_pitch_cd);}
     252             : }
     253             : 
     254             : /*
     255             :  * Calculate the throttle limits to run at during a takeoff.
     256             :  * These limits are meant to be used exclusively by Plane::apply_throttle_limits().
     257             :  */
     258       71460 : void Plane::takeoff_calc_throttle() {
     259             :     // Initialize the maximum throttle limit.
     260       71460 :     if (aparm.takeoff_throttle_max != 0) {
     261        8450 :         takeoff_state.throttle_lim_max = aparm.takeoff_throttle_max;
     262             :     } else {
     263       63010 :         takeoff_state.throttle_lim_max = aparm.throttle_max;
     264             :     }
     265             : 
     266             :     // Initialize the minimum throttle limit.
     267       71460 :     if (aparm.takeoff_throttle_min != 0) {
     268           0 :         takeoff_state.throttle_lim_min = aparm.takeoff_throttle_min;
     269             :     } else {
     270       71460 :         takeoff_state.throttle_lim_min = aparm.throttle_cruise;
     271             :     }
     272             : 
     273             :     // Raise min to force max throttle for TKOFF_THR_MAX_T after a takeoff.
     274             :     // It only applies if the timer has been started externally.
     275       71460 :     if (takeoff_state.throttle_max_timer_ms != 0) {
     276       23514 :         const uint32_t dt = AP_HAL::millis() - takeoff_state.throttle_max_timer_ms;
     277       23514 :         if (dt*0.001 < aparm.takeoff_throttle_max_t) {
     278       23423 :             takeoff_state.throttle_lim_min = takeoff_state.throttle_lim_max;
     279             :         } else {
     280             :             // Reset the timer for future use.
     281          91 :             takeoff_state.throttle_max_timer_ms = 0;
     282             :         }
     283             :     }
     284             : 
     285             :     // Enact the TKOFF_OPTIONS logic.
     286       71460 :     const float current_baro_alt = barometer.get_altitude();
     287       71460 :     const bool below_lvl_alt = current_baro_alt < auto_state.baro_takeoff_alt + mode_takeoff.level_alt;
     288             :     // Set the minimum throttle limit.
     289       71460 :     const bool use_throttle_range = tkoff_option_is_set(AP_FixedWing::TakeoffOption::THROTTLE_RANGE);
     290      142920 :     if (!use_throttle_range // We don't want to employ a throttle range.
     291        2124 :         || !ahrs.using_airspeed_sensor() // We don't have an airspeed sensor.
     292       73584 :         || below_lvl_alt // We are below TKOFF_LVL_ALT.
     293             :         ) { // Traditional takeoff throttle limit.
     294       70833 :         takeoff_state.throttle_lim_min = takeoff_state.throttle_lim_max;
     295             :     }
     296             : 
     297       71460 :     calc_throttle();
     298       71460 : }
     299             : 
     300             : /* get the pitch min used during takeoff. This matches the mission pitch until near the end where it allows it to levels off
     301             :  */
     302      382619 : int16_t Plane::get_takeoff_pitch_min_cd(void)
     303             : {
     304      382619 :     if (flight_stage != AP_FixedWing::FlightStage::TAKEOFF) {
     305      315715 :         return auto_state.takeoff_pitch_cd;
     306             :     }
     307             : 
     308       66904 :     int32_t relative_alt_cm = adjusted_relative_altitude_cm();
     309       66904 :     int32_t remaining_height_to_target_cm = (auto_state.takeoff_altitude_rel_cm - relative_alt_cm);
     310             : 
     311             :     // seconds to target alt method
     312       66904 :     if (g.takeoff_pitch_limit_reduction_sec > 0) {
     313             :         // if height-below-target has been initialized then use it to create and apply a scaler to the pitch_min
     314       66761 :         if (auto_state.height_below_takeoff_to_level_off_cm != 0) {
     315        8016 :             float scalar = remaining_height_to_target_cm / (float)auto_state.height_below_takeoff_to_level_off_cm;
     316        8016 :             return auto_state.takeoff_pitch_cd * scalar;
     317             :         }
     318             : 
     319             :         // are we entering the region where we want to start levelling off before we reach takeoff alt?
     320       58745 :         if (auto_state.sink_rate < -0.1f) {
     321       27498 :             float sec_to_target = (remaining_height_to_target_cm * 0.01f) / (-auto_state.sink_rate);
     322       27487 :             if (sec_to_target > 0 &&
     323       54985 :                 relative_alt_cm >= 1000 &&
     324       17744 :                 sec_to_target <= g.takeoff_pitch_limit_reduction_sec) {
     325             :                 // make a note of that altitude to use it as a start height for scaling
     326          77 :                 gcs().send_text(MAV_SEVERITY_INFO, "Takeoff level-off starting at %dm", int(remaining_height_to_target_cm/100));
     327          77 :                 auto_state.height_below_takeoff_to_level_off_cm = remaining_height_to_target_cm;
     328          77 :                 takeoff_state.level_off_start_time_ms = AP_HAL::millis();
     329             :             }
     330             :         }
     331             :     }
     332       58888 :     return auto_state.takeoff_pitch_cd;
     333             : }
     334             : 
     335             : /*
     336             :   return a tail hold percentage during initial takeoff for a tail
     337             :   dragger
     338             : 
     339             :   This can be used either in auto-takeoff or in FBWA mode with
     340             :   FBWA_TDRAG_CHAN enabled
     341             :  */
     342     7540677 : int8_t Plane::takeoff_tail_hold(void)
     343             : {
     344    15019485 :     bool in_takeoff = ((plane.flight_stage == AP_FixedWing::FlightStage::TAKEOFF) ||
     345     7478808 :                        (control_mode == &mode_fbwa && auto_state.fbwa_tdrag_takeoff_mode));
     346     7540677 :     if (!in_takeoff) {
     347             :         // not in takeoff
     348     7478808 :         return 0;
     349             :     }
     350       61869 :     if (g.takeoff_tdrag_elevator == 0) {
     351             :         // no takeoff elevator set
     352       61869 :         goto return_zero;
     353             :     }
     354           0 :     if (auto_state.highest_airspeed >= g.takeoff_tdrag_speed1) {
     355             :         // we've passed speed1. We now raise the tail and aim for
     356             :         // level pitch. Return 0 meaning no fixed elevator setting
     357           0 :         goto return_zero;
     358             :     }
     359           0 :     if (ahrs.pitch_sensor > auto_state.initial_pitch_cd + 1000) {
     360             :         // the pitch has gone up by more then 10 degrees over the
     361             :         // initial pitch. This may mean the nose is coming up for an
     362             :         // early liftoff, perhaps due to a bad setting of
     363             :         // g.takeoff_tdrag_speed1. Go to level flight to prevent a
     364             :         // stall
     365           0 :         goto return_zero;
     366             :     }
     367             :     // we are holding the tail down
     368           0 :     return g.takeoff_tdrag_elevator;
     369             : 
     370       61869 : return_zero:
     371       61869 :     if (auto_state.fbwa_tdrag_takeoff_mode) {
     372           0 :         gcs().send_text(MAV_SEVERITY_NOTICE, "FBWA tdrag off");
     373           0 :         auto_state.fbwa_tdrag_takeoff_mode = false;
     374             :     }
     375       61869 :     return 0;
     376             : }
     377             : 
     378             : #if AP_LANDINGGEAR_ENABLED
     379             : /*
     380             :   update landing gear
     381             :  */
     382      273423 : void Plane::landing_gear_update(void)
     383             : {
     384      273423 :     g2.landing_gear.update(relative_ground_altitude(RangeFinderUse::TAKEOFF_LANDING));
     385      273423 : }
     386             : #endif
     387             : 
     388             : /*
     389             :  check takeoff_timeout; checks time after the takeoff start time; returns true if timeout has occurred
     390             : */
     391      134472 : bool Plane::check_takeoff_timeout(void)
     392             : {
     393      134472 :     if (takeoff_state.start_time_ms != 0 && g2.takeoff_timeout > 0) {
     394           0 :         const float ground_speed = AP::gps().ground_speed();
     395           0 :         const float takeoff_min_ground_speed = 4;
     396           0 :         if (ground_speed >= takeoff_min_ground_speed) {
     397           0 :             takeoff_state.start_time_ms = 0;
     398           0 :             return false;
     399             :         } else {
     400           0 :             uint32_t now = AP_HAL::millis();
     401           0 :             if (now - takeoff_state.start_time_ms > (uint32_t)(1000U * g2.takeoff_timeout)) {
     402           0 :                 gcs().send_text(MAV_SEVERITY_INFO, "Takeoff timeout: %.1f m/s speed < 4m/s", ground_speed);
     403           0 :                 arming.disarm(AP_Arming::Method::TAKEOFFTIMEOUT);
     404           0 :                 takeoff_state.start_time_ms = 0;
     405           0 :                 return true;
     406             :             }
     407             :         }
     408             :      }
     409      134472 :      return false;
     410             : }
     411             : 
     412             : /*
     413             :  check if the pitch level-off time has expired; returns true if timeout has occurred
     414             : */
     415      134353 : bool Plane::check_takeoff_timeout_level_off(void)
     416             : {
     417      134353 :     if (takeoff_state.level_off_start_time_ms > 0) {
     418             :         // A takeoff is in progress.
     419       72748 :         uint32_t now = AP_HAL::millis();
     420       72748 :         if ((now - takeoff_state.level_off_start_time_ms) > (uint32_t)(1000U * g.takeoff_pitch_limit_reduction_sec)) {
     421       68425 :             return true;
     422             :         }
     423             :     }
     424       65928 :     return false;
     425             : }