Line data    Source code

       1             : #include "Copter.h"
       2             : 
       3             : // Code to detect a crash main ArduCopter code
       4             : #define CRASH_CHECK_TRIGGER_SEC         2       // 2 seconds inverted indicates a crash
       5             : #define CRASH_CHECK_ANGLE_DEVIATION_DEG 30.0f   // 30 degrees beyond target angle is signal we are out of control
       6             : #define CRASH_CHECK_ANGLE_MIN_DEG       15.0f   // vehicle must be leaning at least 15deg to trigger crash check
       7             : #define CRASH_CHECK_SPEED_MAX           10.0f   // vehicle must be moving at less than 10m/s to trigger crash check
       8             : #define CRASH_CHECK_ACCEL_MAX           3.0f    // vehicle must be accelerating less than 3m/s/s to be considered crashed
       9             : 
      10             : // Code to detect a thrust loss main ArduCopter code
      11             : #define THRUST_LOSS_CHECK_TRIGGER_SEC         1             // 1 second descent while level and high throttle indicates thrust loss
      12             : #define THRUST_LOSS_CHECK_ANGLE_DEVIATION_RAD radians(15.0) // we can't expect to maintain altitude beyond 15 degrees on all aircraft
      13             : #define THRUST_LOSS_CHECK_MINIMUM_THROTTLE    0.9f          // we can expect to maintain altitude above 90 % throttle
      14             : 
      15             : // Yaw imbalance check
      16             : #define YAW_IMBALANCE_IMAX_THRESHOLD 0.75f
      17             : #define YAW_IMBALANCE_WARN_MS 10000
      18             : 
      19             : // crash_check - disarms motors if a crash has been detected
      20             : // crashes are detected by the vehicle being more than 20 degrees beyond it's angle limits continuously for more than 1 second
      21             : // called at MAIN_LOOP_RATE
      22    30412448 : void Copter::crash_check()
      23             : {
      24             :     static uint16_t crash_counter;  // number of iterations vehicle may have been crashed
      25             : 
      26             :     // return immediately if disarmed, or crash checking disabled
      27    30412448 :     if (!motors->armed() || ap.land_complete || g.fs_crash_check == 0) {
      28    12010531 :         crash_counter = 0;
      29    30411763 :         return;
      30             :     }
      31             : 
      32             :     // exit immediately if in standby
      33    18401917 :     if (standby_active) {
      34           0 :         crash_counter = 0;
      35           0 :         return;
      36             :     }
      37             : 
      38             :     // exit immediately if in force flying
      39    18401917 :     if (get_force_flying() && !flightmode->is_landing()) {
      40        9840 :         crash_counter = 0;
      41        9840 :         return;
      42             :     }
      43             : 
      44             :     // return immediately if we are not in an angle stabilize flight mode or we are flipping
      45    18392077 :     if (!flightmode->crash_check_enabled()) {
      46        4308 :         crash_counter = 0;
      47        4308 :         return;
      48             :     }
      49             : 
      50             : #if MODE_AUTOROTATE_ENABLED
      51             :     //return immediately if in autorotation mode
      52     1803470 :     if (flightmode->mode_number() == Mode::Number::AUTOROTATE) {
      53        7120 :         crash_counter = 0;
      54        7120 :         return;
      55             :     }
      56             : #endif
      57             : 
      58             :     // vehicle not crashed if 1hz filtered acceleration is more than 3m/s (1G on Z-axis has been subtracted)
      59    18380649 :     const float filtered_acc = land_accel_ef_filter.get().length();
      60    18380649 :     if (filtered_acc >= CRASH_CHECK_ACCEL_MAX) {
      61      351812 :         crash_counter = 0;
      62      351812 :         return;
      63             :     }
      64             : 
      65             :     // check for lean angle over 15 degrees
      66    18028837 :     const float lean_angle_deg = degrees(acosf(ahrs.cos_roll()*ahrs.cos_pitch()));
      67    18028837 :     if (lean_angle_deg <= CRASH_CHECK_ANGLE_MIN_DEG) {
      68    16426542 :         crash_counter = 0;
      69    16426542 :         return;
      70             :     }
      71             : 
      72             :     // check for angle error over 30 degrees
      73     1602295 :     const float angle_error = attitude_control->get_att_error_angle_deg();
      74     1602295 :     if (angle_error <= CRASH_CHECK_ANGLE_DEVIATION_DEG) {
      75     1601610 :         crash_counter = 0;
      76     1601610 :         return;
      77             :     }
      78             : 
      79             :     // check for speed under 10m/s (if available)
      80         685 :     Vector3f vel_ned_ms;
      81         685 :     if (ahrs.get_velocity_NED(vel_ned_ms) && (vel_ned_ms.length() >= CRASH_CHECK_SPEED_MAX)) {
      82           0 :         crash_counter = 0;
      83           0 :         return;
      84             :     }
      85             : 
      86             :     // we may be crashing
      87         685 :     crash_counter++;
      88             : 
      89             :     // check if crashing for 2 seconds
      90         685 :     if (crash_counter >= (CRASH_CHECK_TRIGGER_SEC * scheduler.get_loop_rate_hz())) {
      91           0 :         LOGGER_WRITE_ERROR(LogErrorSubsystem::CRASH_CHECK, LogErrorCode::CRASH_CHECK_CRASH);
      92             :         // send message to gcs
      93           0 :         gcs().send_text(MAV_SEVERITY_EMERGENCY,"Crash: Disarming: AngErr=%.0f>%.0f, Accel=%.1f<%.1f", angle_error, CRASH_CHECK_ANGLE_DEVIATION_DEG, filtered_acc, CRASH_CHECK_ACCEL_MAX);
      94             :         // disarm motors
      95           0 :         copter.arming.disarm(AP_Arming::Method::CRASH);
      96             :     }
      97             : }
      98             : 
      99             : // check for loss of thrust and trigger thrust boost in motors library
     100    30412448 : void Copter::thrust_loss_check()
     101             : {
     102             :     static uint16_t thrust_loss_counter;  // number of iterations vehicle may have been crashed
     103             : 
     104             :     // no-op if suppressed by flight options param
     105    30412448 :     if (copter.option_is_enabled(FlightOption::DISABLE_THRUST_LOSS_CHECK)) {
     106    30407256 :         return;
     107             :     }
     108             : 
     109             :     // exit immediately if thrust boost is already engaged
     110    30412448 :     if (motors->get_thrust_boost()) {
     111        6553 :         return;
     112             :     }
     113             : 
     114             :     // return immediately if disarmed
     115    30405895 :     if (!motors->armed() || ap.land_complete) {
     116    12010531 :         thrust_loss_counter = 0;
     117    12010531 :         return;
     118             :     }
     119             : 
     120             :     // exit immediately if in standby
     121    18395364 :     if (standby_active) {
     122           0 :         return;
     123             :     }
     124             : 
     125             :     // check for desired angle over 15 degrees
     126             :     // todo: add thrust angle to AC_AttitudeControl
     127    18395364 :     const Vector3f& angle_target_rad = attitude_control->get_att_target_euler_rad();
     128    18395364 :     if (angle_target_rad.xy().length_squared() > sq(THRUST_LOSS_CHECK_ANGLE_DEVIATION_RAD)) {
     129     1811323 :         thrust_loss_counter = 0;
     130     1811323 :         return;
     131             :     }
     132             : 
     133             :     // check for throttle over 90% or throttle saturation
     134    16584041 :     if ((attitude_control->get_throttle_in() < THRUST_LOSS_CHECK_MINIMUM_THROTTLE) && (!motors->limit.throttle_upper)) {
     135    16535497 :         thrust_loss_counter = 0;
     136    16535497 :         return;
     137             :     }
     138             : 
     139             :     // check throttle is over 25% to prevent checks triggering from thrust limitations caused by low commanded throttle
     140       48544 :     if ((attitude_control->get_throttle_in() < 0.25f)) {
     141        2780 :         thrust_loss_counter = 0;
     142        2780 :         return;
     143             :     }
     144             : 
     145             :     // check for descent
     146       45764 :     float vel_d_ms = 0;
     147       45764 :     if (!AP::ahrs().get_velocity_D(vel_d_ms, vibration_check.high_vibes) || !is_positive(vel_d_ms)) {
     148             :         // we have no vertical velocity estimate and/or we are not descending
     149       28448 :         thrust_loss_counter = 0;
     150       28448 :         return;
     151             :     }
     152             : 
     153             :     // check for angle error over 30 degrees to ensure the aircraft has attitude control
     154       17316 :     const float angle_error = attitude_control->get_att_error_angle_deg();
     155       17316 :     if (angle_error >= CRASH_CHECK_ANGLE_DEVIATION_DEG) {
     156       12124 :         thrust_loss_counter = 0;
     157       12124 :         return;
     158             :     }
     159             : 
     160             :     // the aircraft is descending with low requested roll and pitch, at full available throttle, with attitude control
     161             :     // we may have lost thrust
     162        5192 :     thrust_loss_counter++;
     163             : 
     164             :     // check if thrust loss for 1 second
     165        5192 :     if (thrust_loss_counter >= (THRUST_LOSS_CHECK_TRIGGER_SEC * scheduler.get_loop_rate_hz())) {
     166             :         // reset counter
     167           4 :         thrust_loss_counter = 0;
     168           4 :         LOGGER_WRITE_ERROR(LogErrorSubsystem::THRUST_LOSS_CHECK, LogErrorCode::FAILSAFE_OCCURRED);
     169             :         // send message to gcs
     170           4 :         gcs().send_text(MAV_SEVERITY_EMERGENCY, "Potential Thrust Loss (%d)", (int)motors->get_lost_motor() + 1);
     171             :         // enable thrust loss handling
     172           4 :         motors->set_thrust_boost(true);
     173             :         // the motors library disables this when it is no longer needed to achieve the commanded output
     174             : 
     175             : #if AP_GRIPPER_ENABLED
     176           4 :         if (copter.option_is_enabled(FlightOption::RELEASE_GRIPPER_ON_THRUST_LOSS)) {
     177           1 :             gripper.release();
     178             :         }
     179             : #endif
     180             :     }
     181             : }
     182             : 
     183             : // check for a large yaw imbalance, could be due to badly calibrated ESC or misaligned motors
     184    30412448 : void Copter::yaw_imbalance_check()
     185             : {
     186             :     // no-op if suppressed by flight options param
     187    30412448 :     if (copter.option_is_enabled(FlightOption::DISABLE_YAW_IMBALANCE_WARNING)) {
     188    12017088 :         return;
     189             :     }
     190             : 
     191             :     // If I is disabled it is unlikely that the issue is not obvious
     192    30412448 :     if (!is_positive(attitude_control->get_rate_yaw_pid().kI())) {
     193           0 :         return;
     194             :     }
     195             : 
     196             :     // thrust loss is triggered, yaw issues are expected
     197    30412448 :     if (motors->get_thrust_boost()) {
     198        6557 :         yaw_I_filt.reset(0.0f);
     199        6557 :         return;
     200             :     }
     201             : 
     202             :     // return immediately if disarmed
     203    30405891 :     if (!motors->armed() || ap.land_complete) {
     204    12010531 :         yaw_I_filt.reset(0.0f);
     205    12010531 :         return;
     206             :     }
     207             : 
     208             :     // exit immediately if in standby
     209    18395360 :     if (standby_active) {
     210           0 :         yaw_I_filt.reset(0.0f);
     211           0 :         return;
     212             :     }
     213             : 
     214             :     // magnitude of low pass filtered I term
     215    18395360 :     const float I_term = attitude_control->get_rate_yaw_pid().get_pid_info().I;
     216    18395360 :     const float I = fabsf(yaw_I_filt.apply(attitude_control->get_rate_yaw_pid().get_pid_info().I,G_Dt));
     217    18395360 :     if (I > fabsf(I_term)) {
     218             :         // never allow to be larger than I
     219     6949472 :         yaw_I_filt.reset(I_term);
     220             :     }
     221             : 
     222    18395360 :     const float I_max = attitude_control->get_rate_yaw_pid().imax();
     223    18395360 :     if ((is_positive(I_max) && ((I > YAW_IMBALANCE_IMAX_THRESHOLD * I_max) || (is_equal(I_term,I_max))))) {
     224             :         // filtered using over percentage of I max or unfiltered = I max
     225             :         // I makes up more than percentage of total available control power
     226       24370 :         const uint32_t now = millis();
     227       24370 :         if (now - last_yaw_warn_ms > YAW_IMBALANCE_WARN_MS) {
     228           7 :             last_yaw_warn_ms = now;
     229           7 :             gcs().send_text(MAV_SEVERITY_EMERGENCY, "Yaw Imbalance %0.0f%%", I *100);
     230             :         }
     231             :     }
     232             : }
     233             : 
     234             : #if HAL_PARACHUTE_ENABLED
     235             : 
     236             : // Code to detect a crash main ArduCopter code
     237             : #define PARACHUTE_CHECK_TRIGGER_SEC         1       // 1 second of loss of control triggers the parachute
     238             : #define PARACHUTE_CHECK_ANGLE_DEVIATION_DEG 30.0f   // 30 degrees off from target indicates a loss of control
     239             : 
     240             : // parachute_check - disarms motors and triggers the parachute if serious loss of control has been detected
     241             : // vehicle is considered to have a "serious loss of control" by the vehicle being more than 30 degrees off from the target roll and pitch angles continuously for 1 second
     242             : // called at MAIN_LOOP_RATE
     243    30412448 : void Copter::parachute_check()
     244             : {
     245             :     static uint16_t control_loss_count; // number of iterations we have been out of control
     246             :     static float baro_alt_start_m;
     247             : 
     248             :     // exit immediately if parachute is not enabled
     249    30412448 :     if (!parachute.enabled()) {
     250    30411194 :         return;
     251             :     }
     252             : 
     253             :     // pass is_flying to parachute library
     254       96650 :     parachute.set_is_flying(!ap.land_complete);
     255             : 
     256             :     // pass sink rate to parachute library
     257       96650 :     float vel_d_ms = 0;
     258       96650 :     UNUSED_RESULT(AP::ahrs().get_velocity_D(vel_d_ms, vibration_check.high_vibes));
     259       96650 :     parachute.set_sink_rate(vel_d_ms);
     260             : 
     261             :     // exit immediately if in standby
     262       96650 :     if (standby_active) {
     263           0 :         return;
     264             :     }
     265             : 
     266             :     // call update to give parachute a chance to move servo or relay back to off position
     267       96650 :     parachute.update();
     268             : 
     269             :     // return immediately if motors are not armed or pilot's throttle is above zero
     270       96650 :     if (!motors->armed()) {
     271       74597 :         control_loss_count = 0;
     272       74597 :         return;
     273             :     }
     274             : 
     275       22053 :     if (parachute.release_initiated()) {
     276           0 :         return;
     277             :     }
     278             : 
     279             :     // return immediately if we are not in an angle stabilize flight mode or we are flipping
     280       22053 :     if (!flightmode->crash_check_enabled()) {
     281           0 :         control_loss_count = 0;
     282           0 :         return;
     283             :     }
     284             : 
     285             :     // ensure we are flying
     286       22053 :     if (ap.land_complete) {
     287        4889 :         control_loss_count = 0;
     288        4889 :         return;
     289             :     }
     290             : 
     291             :     // ensure the first control_loss event is from above the min altitude
     292       17164 :     if (control_loss_count == 0 && parachute.alt_min() != 0 && (current_loc.alt < (int32_t)parachute.alt_min() * 100)) {
     293        6009 :         return;
     294             :     }
     295             : 
     296             :     // trigger parachute release based on sink rate
     297       11155 :     parachute.check_sink_rate();
     298             : 
     299             :     // check for angle error over 30 degrees
     300       11155 :     const float angle_error = attitude_control->get_att_error_angle_deg();
     301       11155 :     if (angle_error <= PARACHUTE_CHECK_ANGLE_DEVIATION_DEG) {
     302        9564 :         if (control_loss_count > 0) {
     303         117 :             control_loss_count--;
     304             :         }
     305        9564 :         return;
     306             :     }
     307             : 
     308             :     // increment counter
     309        1591 :     if (control_loss_count < (PARACHUTE_CHECK_TRIGGER_SEC*scheduler.get_loop_rate_hz())) {
     310        1591 :         control_loss_count++;
     311             :     }
     312             : 
     313             :     // record baro alt if we have just started losing control
     314        1591 :     if (control_loss_count == 1) {
     315         339 :         baro_alt_start_m = baro_alt_m;
     316             : 
     317             :     // exit if baro altitude change indicates we are not falling
     318        1252 :     } else if (baro_alt_m >= baro_alt_start_m) {
     319         337 :         control_loss_count = 0;
     320         337 :         return;
     321             : 
     322             :     // To-Do: add check that the vehicle is actually falling
     323             : 
     324             :     // check if loss of control for at least 1 second
     325         915 :     } else if (control_loss_count >= (PARACHUTE_CHECK_TRIGGER_SEC*scheduler.get_loop_rate_hz())) {
     326             :         // reset control loss counter
     327           2 :         control_loss_count = 0;
     328           2 :         LOGGER_WRITE_ERROR(LogErrorSubsystem::CRASH_CHECK, LogErrorCode::CRASH_CHECK_LOSS_OF_CONTROL);
     329             :         // release parachute
     330           2 :         parachute_release();
     331             :     }
     332             : }
     333             : 
     334             : // parachute_release - trigger the release of the parachute, disarm the motors and notify the user
     335           8 : void Copter::parachute_release()
     336             : {
     337             :     // release parachute
     338           8 :     parachute.release();
     339             : 
     340             : #if AP_LANDINGGEAR_ENABLED
     341             :     // deploy landing gear
     342           8 :     landinggear.set_position(AP_LandingGear::LandingGear_Deploy);
     343             : #endif
     344           8 : }
     345             : 
     346             : // parachute_manual_release - trigger the release of the parachute, after performing some checks for pilot error
     347             : //   checks if the vehicle is landed 
     348           8 : void Copter::parachute_manual_release()
     349             : {
     350             :     // exit immediately if parachute is not enabled
     351           8 :     if (!parachute.enabled()) {
     352           2 :         return;
     353             :     }
     354             : 
     355             :     // do not release if vehicle is landed
     356             :     // do not release if we are landed or below the minimum altitude above home
     357           6 :     if (ap.land_complete) {
     358             :         // warn user of reason for failure
     359           0 :         gcs().send_text(MAV_SEVERITY_INFO,"Parachute: Landed");
     360           0 :         LOGGER_WRITE_ERROR(LogErrorSubsystem::PARACHUTES, LogErrorCode::PARACHUTE_LANDED);
     361           0 :         return;
     362             :     }
     363             : 
     364             :     // do not release if we are landed or below the minimum altitude above home
     365           6 :     if ((parachute.alt_min() != 0 && (current_loc.alt < (int32_t)parachute.alt_min() * 100))) {
     366             :         // warn user of reason for failure
     367           0 :         gcs().send_text(MAV_SEVERITY_ALERT,"Parachute: Too low");
     368           0 :         LOGGER_WRITE_ERROR(LogErrorSubsystem::PARACHUTES, LogErrorCode::PARACHUTE_TOO_LOW);
     369           0 :         return;
     370             :     }
     371             : 
     372             :     // if we get this far release parachute
     373           6 :     parachute_release();
     374             : }
     375             : 
     376             : #endif  // HAL_PARACHUTE_ENABLED