Line data    Source code

       1             : #include "AP_Airspeed_config.h"
       2             : 
       3             : #if AP_AIRSPEED_ENABLED
       4             : 
       5             : #include "AP_Airspeed.h"
       6             : 
       7             : #include <AP_Common/AP_Common.h>
       8             : #include <AP_GPS/AP_GPS.h>
       9             : #include <AP_Math/AP_Math.h>
      10             : #include <GCS_MAVLink/GCS.h>
      11             : #include <AP_AHRS/AP_AHRS.h>
      12             : #include <AP_Logger/AP_Logger.h>
      13             : 
      14      559330 : void AP_Airspeed::check_sensor_failures()
      15             : {
      16             : #ifndef HAL_BUILD_AP_PERIPH
      17     1677990 :     for (uint8_t i=0; i<AIRSPEED_MAX_SENSORS; i++) {
      18     1118660 :         check_sensor_ahrs_wind_max_failures(i);
      19             :     }
      20             : #endif
      21      559330 : }
      22             : 
      23     1118660 : void AP_Airspeed::check_sensor_ahrs_wind_max_failures(uint8_t i)
      24             : {
      25             : #ifndef HAL_BUILD_AP_PERIPH
      26     1118660 :     const uint32_t now_ms = AP_HAL::millis();
      27     1118660 :     if ((now_ms - state[i].failures.last_check_ms) <= 200) {
      28             :         // slow the checking rate
      29      940221 :         return;
      30             :     }
      31      379068 :     state[i].failures.last_check_ms = now_ms;
      32             : 
      33      379068 :     if (!is_positive(_wind_max) && !is_positive(_wind_gate)) {
      34             :         // nothing to do
      35        9878 :         return;
      36             :     }
      37             : 
      38      369190 :     if (state[i].airspeed <= 0) {
      39             :         // invalid estimate
      40      178078 :         return;
      41             :     }
      42             : 
      43      191112 :     const AP_GPS &gps = AP::gps();
      44      191112 :     if (gps.status() < AP_GPS_FixType::FIX_3D) {
      45             :         // GPS speed can't be trusted, re-enable airspeed as a fallback
      46       12673 :         if ((param[i].use == 0) && (state[i].failures.param_use_backup == 1)) {
      47           0 :             GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Airspeed sensor %d, Re-enabled as GPS fall-back", i+1);
      48           0 :             param[i].use.set_and_notify(state[i].failures.param_use_backup); 
      49           0 :             state[i].failures.param_use_backup = -1;
      50             :         }
      51       12673 :         return;
      52             :     }
      53             : 
      54             :     // check for airspeed consistent with wind and vehicle velocity using the EKF
      55             :     uint32_t age_ms;
      56             :     float innovation, innovationVariance;
      57      178439 :     if (AP::ahrs().airspeed_health_data(i, innovation, innovationVariance, age_ms) && age_ms < 1000 && is_positive(innovationVariance)) {
      58      111141 :         state[i].failures.test_ratio = fabsf(innovation) / safe_sqrt(innovationVariance);
      59             :     } else {
      60       67298 :         state[i].failures.test_ratio = 0.0f;
      61             :     }
      62      178439 :     bool data_is_inconsistent = false;
      63      178439 :     if (is_positive(_wind_gate) && (AP_Airspeed::OptionsMask::USE_EKF_CONSISTENCY & _options) != 0) {
      64      178439 :         float gate_size = MAX(_wind_gate, 0.0f);
      65      178439 :         if (param[i].use == 0) {
      66             :             // require a smaller inconsistency for a disabled sensor to be declared consistent
      67       14228 :             gate_size *= 0.7f;
      68             :         }
      69      178439 :         data_is_inconsistent = state[i].failures.test_ratio > gate_size;
      70             :     }
      71             :     
      72      178439 :     const auto gps_speed = gps.velocity().length();
      73      178439 :     const float speed_diff = fabsf(state[i].airspeed-gps_speed);
      74      178439 :     const bool data_is_implausible = is_positive(_wind_max) && speed_diff > _wind_max;
      75             :     // update health_probability with LowPassFilter
      76      178439 :     if (data_is_implausible || data_is_inconsistent) {
      77             :         // bad, decay fast
      78         192 :         const float probability_coeff = 0.90f;
      79         192 :         state[i].failures.health_probability = probability_coeff*state[i].failures.health_probability;
      80             : 
      81      178439 :     } else if (!data_is_implausible && !data_is_inconsistent) {
      82             :         // good, grow slow
      83      178247 :         const float probability_coeff = 0.98f;
      84      178247 :         state[i].failures.health_probability = probability_coeff*state[i].failures.health_probability + (1.0f-probability_coeff)*1.0f;
      85             :     }
      86             : 
      87             :     // Now check if we need to disable or enable the sensor
      88             : 
      89             :     // here are some probability thresholds
      90             :     static const float DISABLE_PROB_THRESH_CRIT = 0.1f;
      91             :     static const float RE_ENABLE_PROB_THRESH_OK = 0.95f;
      92             : 
      93      178439 :     if (param[i].use > 0) {
      94      328422 :         if (((AP_Airspeed::OptionsMask::ON_FAILURE_AHRS_WIND_MAX_DO_DISABLE & _options) != 0) &&
      95      164211 :                 (state[i].failures.health_probability < DISABLE_PROB_THRESH_CRIT)) {
      96             :             // if "disable" option is allowed and sensor is enabled and is probably not healthy
      97           5 :             GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "Airspeed sensor %d failure. Disabling", i+1);
      98           5 :             state[i].failures.param_use_backup = param[i].use;
      99           5 :             param[i].use.set_and_notify(0);
     100           5 :             state[i].healthy = false;
     101             :         }
     102             : 
     103             :         // Warn if necessary
     104             : 
     105             :         // set warn to max if not set or larger than max
     106      164211 :         float wind_warn = _wind_warn;
     107      164211 :         if ((!is_positive(wind_warn) || (wind_warn > _wind_max)) && is_positive(_wind_max)) {
     108         323 :             wind_warn = _wind_max;
     109             :         }
     110             : 
     111      164211 :         if (is_positive(wind_warn) && (speed_diff > wind_warn) && ((now_ms - state[i].failures.last_warn_ms) > 15000)) {
     112           1 :             state[i].failures.last_warn_ms = now_ms;
     113           1 :             GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Airspeed %d warning %0.1fm/s air to gnd speed diff", i+1, speed_diff);
     114             :         }
     115             : 
     116             :     // if Re-Enable options is allowed, and sensor is disabled but was previously enabled, and is probably healthy
     117       14228 :     } else if (((AP_Airspeed::OptionsMask::ON_FAILURE_AHRS_WIND_MAX_RECOVERY_DO_REENABLE & _options) != 0) &&
     118       15153 :                 (state[i].failures.param_use_backup > 0) && 
     119         925 :                 (state[i].failures.health_probability > RE_ENABLE_PROB_THRESH_OK)) {
     120             : 
     121           5 :         GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Airspeed sensor %d now OK. Re-enabled", i+1);
     122           5 :         param[i].use.set_and_notify(state[i].failures.param_use_backup); // resume
     123           5 :         state[i].failures.param_use_backup = -1; // set to invalid so we don't use it
     124             :     }
     125             : #endif // HAL_BUILD_AP_PERIPH
     126             : }
     127             : 
     128             : #endif  // AP_AIRSPEED_ENABLED