Line data    Source code

       1             : #include "Copter.h"
       2             : 
       3             : #include <AP_Stats/AP_Stats.h>              // statistics library
       4             : 
       5             : // Code to detect a crash main ArduCopter code
       6             : #define LAND_CHECK_ANGLE_ERROR_DEG  30.0f       // maximum angle error to be considered landing
       7             : #define LAND_CHECK_LARGE_ANGLE_RAD   radians(15.0f)     // maximum angle target to be considered landing
       8             : #define LAND_CHECK_ACCEL_MOVING     3.0f        // maximum acceleration after subtracting gravity
       9             : 
      10             : 
      11             : // counter to verify landings
      12             : static uint32_t land_detector_count = 0;
      13             : 
      14             : // run land and crash detectors
      15             : // called at MAIN_LOOP_RATE
      16    30412448 : void Copter::update_land_and_crash_detectors()
      17             : {
      18             :     // update 1hz filtered acceleration
      19    30412448 :     Vector3f accel_ef_mss = ahrs.get_accel_ef();
      20    30412448 :     accel_ef_mss.z += GRAVITY_MSS;
      21    30412448 :     land_accel_ef_filter.apply(accel_ef_mss, scheduler.get_loop_period_s());
      22             : 
      23    30412448 :     update_land_detector();
      24             : 
      25             : #if HAL_PARACHUTE_ENABLED
      26             :     // check parachute
      27    30412448 :     parachute_check();
      28             : #endif
      29             : 
      30    30412448 :     crash_check();
      31    30412448 :     thrust_loss_check();
      32    30412448 :     yaw_imbalance_check();
      33    30412448 : }
      34             : 
      35             : // update_land_detector - checks if we have landed and updates the ap.land_complete flag
      36             : // called at MAIN_LOOP_RATE
      37    30412448 : void Copter::update_land_detector()
      38             : {
      39             :     // land detector can not use the following sensors because they are unreliable during landing
      40             :     // barometer altitude :                 ground effect can cause errors larger than 4m
      41             :     // EKF vertical velocity or altitude :  poor barometer and large acceleration from ground impact
      42             :     // earth frame angle or angle error :   landing on an uneven surface will force the airframe to match the ground angle
      43             :     // gyro output :                        on uneven surface the airframe may rock back an forth after landing
      44             :     // range finder :                       tend to be problematic at very short distances
      45             :     // input throttle :                     in slow land the input throttle may be only slightly less than hover
      46             : 
      47             : #if HAL_LOGGING_ENABLED
      48    30412448 :     uint16_t logging_flags = 0;
      49             : #define SET_LOG_FLAG(condition, flag) if (condition) { logging_flags |= (uint16_t)flag; }
      50             : #else
      51             : #define SET_LOG_FLAG(condition, flag)
      52             : #endif
      53             : 
      54    30412448 :     if (!motors->armed()) {
      55             :         // if disarmed, always landed.
      56    10667071 :         set_land_complete(true);
      57    19745377 :     } else if (ap.land_complete) {
      58             : #if FRAME_CONFIG == HELI_FRAME
      59             :         // if rotor speed and collective pitch are high then clear landing flag
      60      507640 :         if (!flightmode->is_taking_off() && motors->get_takeoff_collective() && motors->get_spool_state() == AP_Motors::SpoolState::THROTTLE_UNLIMITED) {
      61             : #else
      62             :         // if throttle output is high then clear landing flag
      63      835435 :         if (!flightmode->is_taking_off() && motors->get_throttle_out() > get_non_takeoff_throttle() && motors->get_spool_state() == AP_Motors::SpoolState::THROTTLE_UNLIMITED) {
      64             :             // this should never happen because take-off should be detected at the flight mode level
      65             :             // this here to highlight there is a bug or missing take-off detection
      66           0 :             INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
      67             : #endif
      68           9 :             set_land_complete(false);
      69             :         }
      70    18402302 :     } else if (standby_active) {
      71             :         // land detector will not run in standby mode
      72           0 :         land_detector_count = 0;
      73             :     } else {
      74             : 
      75    18402302 :         float land_trigger_sec = LAND_DETECTOR_TRIGGER_SEC;
      76             : #if FRAME_CONFIG == HELI_FRAME
      77             :         // check for both manual collective modes and modes that use altitude hold. For manual collective (called throttle
      78             :         // because multi's use throttle), check that collective pitch is below land min collective position or throttle stick is zero.
      79             :         // Including the throttle zero check will ensure the conditions where stabilize stick zero position was not below collective min. For modes
      80             :         // that use altitude hold, check that the pilot is commanding a descent and collective is at min allowed for altitude hold modes.
      81             : 
      82             :         // check if landing
      83     1813936 :         const bool landing = flightmode->is_landing();
      84     1813936 :         SET_LOG_FLAG(landing, LandDetectorLoggingFlag::LANDING);
      85     1968252 :         bool motor_at_lower_limit = (flightmode->has_manual_throttle() && (motors->get_below_land_min_coll() || heli_flags.coll_stk_low) && fabsf(ahrs.get_roll_rad()) < M_PI/2.0f)
      86             : #if MODE_AUTOROTATE_ENABLED
      87     1785758 :                                     || (flightmode->mode_number() == Mode::Number::AUTOROTATE && motors->get_below_land_min_coll())
      88             : #endif
      89     3627872 :                                     || ((!get_force_flying() || landing) && motors->limit.throttle_lower && pos_control->get_vel_desired_U_ms() < 0.0f);
      90     1813936 :         bool throttle_mix_at_min = true;
      91             : #else
      92             :         // check that the average throttle output is near minimum (less than 12.5% hover throttle)
      93    16588366 :         bool motor_at_lower_limit = motors->limit.throttle_lower;
      94    16588366 :         bool throttle_mix_at_min = attitude_control->is_throttle_mix_min();
      95             :         // set throttle_mix_at_min to true because throttle is never at mix min in airmode
      96             :         // increase land_trigger_sec when using airmode
      97    16588366 :         if (flightmode->has_manual_throttle() && air_mode_active()) {
      98       10244 :             land_trigger_sec = LAND_AIRMODE_DETECTOR_TRIGGER_SEC;
      99       10244 :             throttle_mix_at_min = true;
     100             :         }
     101             : #endif
     102    18402302 :         SET_LOG_FLAG(motor_at_lower_limit, LandDetectorLoggingFlag::MOTOR_AT_LOWER_LIMIT);
     103    18402302 :         SET_LOG_FLAG(throttle_mix_at_min, LandDetectorLoggingFlag::THROTTLE_MIX_AT_MIN);
     104             : 
     105    18402302 :         uint8_t land_detector_scalar = 1;
     106             : #if AP_LANDINGGEAR_ENABLED
     107    18402302 :         if (landinggear.get_wow_state() != AP_LandingGear::LG_WOW_UNKNOWN) {
     108             :             // we have a WoW sensor so lets loosen the strictness of the landing detector
     109           0 :             land_detector_scalar = 2;
     110             :         }
     111             : #endif
     112             : 
     113             :         // check for aggressive flight requests - requested roll or pitch angle below 15 degrees
     114    18402302 :         const Vector3f& angle_target_rad = attitude_control->get_att_target_euler_rad();
     115    18402302 :         bool large_angle_request = angle_target_rad.xy().length_squared() > sq(LAND_CHECK_LARGE_ANGLE_RAD);
     116    18402302 :         SET_LOG_FLAG(large_angle_request, LandDetectorLoggingFlag::LARGE_ANGLE_REQUEST);
     117             : 
     118             :         // check for large external disturbance - angle error over 30 degrees
     119    18402302 :         const float angle_error = attitude_control->get_att_error_angle_deg();
     120    18402302 :         bool large_angle_error = (angle_error > LAND_CHECK_ANGLE_ERROR_DEG);
     121    18402302 :         SET_LOG_FLAG(large_angle_error, LandDetectorLoggingFlag::LARGE_ANGLE_ERROR);
     122             : 
     123             :         // check that the airframe is not accelerating (not falling or braking after fast forward flight)
     124    18402302 :         bool accel_stationary = (land_accel_ef_filter.get().length() <= LAND_DETECTOR_ACCEL_MAX * land_detector_scalar);
     125    18402302 :         SET_LOG_FLAG(accel_stationary, LandDetectorLoggingFlag::ACCEL_STATIONARY);
     126             : 
     127             :         // check that vertical speed is within 1m/s of zero
     128    18402302 :         float vel_d_ms = 0;
     129    18402302 :         UNUSED_RESULT(AP::ahrs().get_velocity_D(vel_d_ms, copter.vibration_check.high_vibes));
     130    18402302 :         const bool descent_rate_low = fabsf(vel_d_ms) < LAND_DETECTOR_VEL_Z_MAX * land_detector_scalar;
     131    18402302 :         SET_LOG_FLAG(descent_rate_low, LandDetectorLoggingFlag::DESCENT_RATE_LOW);
     132             : 
     133             :         // if we have a healthy rangefinder only allow landing detection below 2 meters
     134    18402302 :         bool rangefinder_check = (!rangefinder_alt_ok() || rangefinder_state.alt_m_filt.get() < LAND_RANGEFINDER_MIN_ALT_M);
     135    18402302 :         SET_LOG_FLAG(rangefinder_check, LandDetectorLoggingFlag::RANGEFINDER_BELOW_2M);
     136             : 
     137             :         // if we have weight on wheels (WoW) or ambiguous unknown. never no WoW
     138             : #if AP_LANDINGGEAR_ENABLED
     139    18402302 :         const bool WoW_check = (landinggear.get_wow_state() == AP_LandingGear::LG_WOW || landinggear.get_wow_state() == AP_LandingGear::LG_WOW_UNKNOWN);
     140             : #else
     141             :         const bool WoW_check = true;
     142             : #endif
     143    18402302 :         SET_LOG_FLAG(WoW_check, LandDetectorLoggingFlag::WOW);
     144             : 
     145    18402302 :         if (motor_at_lower_limit && throttle_mix_at_min && !large_angle_request && !large_angle_error && accel_stationary && descent_rate_low && rangefinder_check && WoW_check) {
     146             :             // landed criteria met - increment the counter and check if we've triggered
     147      160264 :             if( land_detector_count < land_trigger_sec*scheduler.get_loop_rate_hz()) {
     148      159872 :                 land_detector_count++;
     149             :             } else {
     150         392 :                 set_land_complete(true);
     151             :             }
     152             :         } else {
     153             :             // we've sensed movement up or down so reset land_detector
     154    18242038 :             land_detector_count = 0;
     155             :         }
     156             :     }
     157             : 
     158    30412448 :     set_land_complete_maybe(ap.land_complete || (land_detector_count >= LAND_DETECTOR_MAYBE_TRIGGER_SEC*scheduler.get_loop_rate_hz()));
     159             : 
     160             : #if HAL_LOGGING_ENABLED
     161             : // @LoggerMessage: LDET
     162             : // @Description: Land Detector State
     163             : // @Field: TimeUS: Time since system startup
     164             : // @Field: Flags: boolean state flags
     165             : // @FieldBitmaskEnum: Flags: Copter::LandDetectorLoggingFlag
     166             : // @Field: Count: landing_detector pass count
     167    30412448 :     SET_LOG_FLAG(ap.land_complete, LandDetectorLoggingFlag::LANDED);
     168    30412448 :     SET_LOG_FLAG(ap.land_complete_maybe, LandDetectorLoggingFlag::LANDED_MAYBE);
     169    30412448 :     SET_LOG_FLAG(standby_active, LandDetectorLoggingFlag::STANDBY_ACTIVE);
     170    30412448 :     Log_LDET(logging_flags, land_detector_count);
     171             : #undef SET_LOG_FLAG
     172             : #endif
     173    30412448 : }
     174             : 
     175             : #if HAL_LOGGING_ENABLED
     176    30412448 : void Copter::Log_LDET(uint16_t logging_flags, uint32_t detector_count)
     177             : {
     178             :     // do not log if no change:
     179    30412448 :     if (logging_flags == land_detector.last_logged_flags &&
     180    30389782 :         detector_count == land_detector.last_logged_count) {
     181    30231186 :         return;
     182             :     }
     183             :     // do not log more than 50Hz:
     184      181262 :     const auto now = AP_HAL::millis();
     185      181262 :     if (now - land_detector.last_logged_ms < 20) {
     186      143925 :         return;
     187             :     }
     188             : 
     189       37337 :     land_detector.last_logged_count = detector_count;
     190       37337 :     land_detector.last_logged_flags = logging_flags;
     191       37337 :     land_detector.last_logged_ms = now;
     192             : 
     193       37337 :     AP::logger().WriteStreaming(
     194             :         "LDET",
     195             :         "TimeUS," "Flags," "Count",
     196             :         "s"       "-"      "-",
     197             :         "F"       "-"      "-",
     198             :         "Q"       "H"      "I",
     199             :         AP_HAL::micros64(),
     200             :         logging_flags,
     201             :         land_detector_count
     202             :     );
     203             : }
     204             : #endif
     205             : 
     206             : // set land_complete flag and disarm motors if disarm-on-land is configured
     207    10839028 : void Copter::set_land_complete(bool b)
     208             : {
     209             :     // if no change, exit immediately
     210    10839028 :     if( ap.land_complete == b )
     211    10837118 :         return;
     212             : 
     213        1910 :     land_detector_count = 0;
     214             : 
     215             : #if HAL_LOGGING_ENABLED
     216        1910 :     if(b){
     217        1453 :         AP::logger().Write_Event(LogEvent::LAND_COMPLETE);
     218             :     } else {
     219         457 :         AP::logger().Write_Event(LogEvent::NOT_LANDED);
     220             :     }
     221             : #endif
     222        1910 :     ap.land_complete = b;
     223             : 
     224             : #if AP_STATS_ENABLED
     225        1910 :     AP::stats()->set_flying(!b);
     226             : #endif
     227             : 
     228             :     // tell AHRS flying state
     229        1910 :     set_likely_flying(!b);
     230             : 
     231        1910 :     if (!b) {
     232             :         // not landed, no further action
     233         457 :         return;
     234             :     }
     235             : 
     236             :     // landed; trigger disarm-on-land if configured
     237        1453 :     if ((g.throttle_behavior & THR_BEHAVE_DISARM_ON_LAND_DETECT) == 0) {
     238             :         // not configured to disarm on landing detection
     239        1452 :         return;
     240             :     }
     241             : 
     242           1 :     if (!motors->armed()) {
     243             :         // we are not currently armed, so we don't need to disarm:
     244             :         // n.b. should this be checking vehicle-armed rather than motors-armed?
     245           0 :         return;
     246             :     }
     247             : 
     248           1 :     if (flightmode->has_manual_throttle()) {
     249             :         // we do not use the landing detector to disarm if the vehicle
     250             :         // is in e.g. STABILIZE.  The normal DISARM_DELAY logic applies.
     251           0 :         return;
     252             :     }
     253             : 
     254             :     // the flightmode may not allow disarm on landing.  Note that this
     255             :     // check returns false for the LAND flight mode - it checks the
     256             :     // landing detector (ap.land_complete) itself.
     257           1 :     if (!flightmode->allows_arming(AP_Arming::Method::LANDING)) {
     258           0 :         return;
     259             :     }
     260             : 
     261             :     // all checks passed, disarm the vehicle:
     262           1 :     arming.disarm(AP_Arming::Method::LANDED);
     263             : }
     264             : 
     265             : // set land complete maybe flag
     266    30413996 : void Copter::set_land_complete_maybe(bool b)
     267             : {
     268             :     // if no change, exit immediately
     269    30413996 :     if (ap.land_complete_maybe == b)
     270    30412080 :         return;
     271             : 
     272        1916 :     if (b) {
     273        1456 :         LOGGER_WRITE_EVENT(LogEvent::LAND_COMPLETE_MAYBE);
     274             :     }
     275        1916 :     ap.land_complete_maybe = b;
     276             : }
     277             : 
     278             : // sets motors throttle_low_comp value depending upon vehicle state
     279             : //  low values favour pilot/autopilot throttle over attitude control, high values favour attitude control over throttle
     280             : //  has no effect when throttle is above hover throttle
     281     3800955 : void Copter::update_throttle_mix()
     282             : {
     283             : #if FRAME_CONFIG != HELI_FRAME
     284             :     // if disarmed or landed prioritise throttle
     285     3396732 :     if (!motors->armed() || ap.land_complete) {
     286     1323193 :         attitude_control->set_throttle_mix_min();
     287     1323193 :         return;
     288             :     }
     289             : 
     290     2073539 :     if (flightmode->has_manual_throttle()) {
     291             :         // manual throttle
     292       22277 :         if (channel_throttle->get_control_in() <= 0 && !air_mode_active()) {
     293         113 :             attitude_control->set_throttle_mix_min();
     294             :         } else {
     295       22164 :             attitude_control->set_throttle_mix_man();
     296             :         }
     297             :     } else {
     298             :         // autopilot controlled throttle
     299             : 
     300             :         // check for aggressive flight requests - requested roll or pitch angle below 15 degrees
     301     2051262 :         const Vector3f& angle_target_rad = attitude_control->get_att_target_euler_rad();
     302     2051262 :         bool large_angle_request = angle_target_rad.xy().length_squared() > sq(LAND_CHECK_LARGE_ANGLE_RAD);
     303             : 
     304             :         // check for large external disturbance - angle error over 30 degrees
     305     2051262 :         const float angle_error = attitude_control->get_att_error_angle_deg();
     306     2051262 :         bool large_angle_error = (angle_error > LAND_CHECK_ANGLE_ERROR_DEG);
     307             : 
     308             :         // check for large acceleration - falling or high turbulence
     309     2051262 :         const bool accel_moving = (land_accel_ef_filter.get().length() > LAND_CHECK_ACCEL_MOVING);
     310             : 
     311             :         // check for requested descent
     312     2051262 :         bool descent_not_demanded = pos_control->get_vel_desired_U_ms() >= 0.0f;
     313             : 
     314             :         // check if landing
     315     2051262 :         const bool landing = flightmode->is_landing();
     316             : 
     317     2051262 :         if (((large_angle_request || get_force_flying()) && !landing) || large_angle_error || accel_moving || descent_not_demanded) {
     318     1303597 :             attitude_control->set_throttle_mix_max(pos_control->get_vel_D_control_ratio());
     319             :         } else {
     320      747665 :             attitude_control->set_throttle_mix_min();
     321             :         }
     322             :     }
     323             : #endif
     324      404223 : }
     325             : 
     326             : // helper function for force flying flag
     327    22018032 : bool Copter::get_force_flying() const
     328             : {
     329             : #if FRAME_CONFIG == HELI_FRAME
     330     3599256 :     if (attitude_control->get_inverted_flight()) {
     331       19680 :         return true;
     332             :     }
     333             : #endif
     334    21998352 :     return force_flying;
     335             : }