Line data    Source code

       1             : #include "Plane.h"
       2             : 
       3             : #include <AP_Stats/AP_Stats.h>     // statistics library
       4             : 
       5             : /*
       6             :   is_flying and crash detection logic
       7             :  */
       8             : 
       9             : #define CRASH_DETECTION_DELAY_MS            500  // milliseconds
      10             : #define IS_FLYING_IMPACT_TIMER_MS           3000  // milliseconds
      11             : 
      12             : #define GPS_IS_FLYING_SPEED_MS              1.5  // metres/second
      13             : 
      14             : /*
      15             :   Do we think we are flying?
      16             :   Probabilistic method where a bool is low-passed and considered a probability.
      17             : */
      18      273423 : void Plane::update_is_flying_5Hz(void)
      19             : {
      20      273423 :     float aspeed=0;
      21      273423 :     bool is_flying_bool = false;
      22      273423 :     uint32_t now_ms = AP_HAL::millis();
      23             : 
      24      273423 :     const float ground_speed_thresh_ms = (aparm.min_groundspeed > 0) ? (aparm.min_groundspeed*0.9) : GPS_IS_FLYING_SPEED_MS;
      25      529292 :     bool gps_confirmed_movement = (gps.status() >= AP_GPS_FixType::FIX_3D) &&
      26      255869 :                                     (gps.ground_speed() >= ground_speed_thresh_ms);
      27             : 
      28             :     // airspeed at least 75% of stall speed?
      29      273423 :     const float airspeed_threshold = MAX(aparm.airspeed_min,2)*0.75f;
      30      273423 :     bool airspeed_movement = ahrs.airspeed_EAS(aspeed) && (aspeed >= airspeed_threshold);
      31             : 
      32      273423 :     if (gps.status() < AP_GPS_FixType::FIX_2D && arming.is_armed() && !airspeed_movement && isFlyingProbability > 0.3) {
      33             :         // when flying with no GPS, use the last airspeed estimate to
      34             :         // determine if we think we have airspeed movement. This
      35             :         // prevents the crash detector from triggering when
      36             :         // dead-reckoning under long GPS loss
      37          23 :         airspeed_movement = aspeed >= airspeed_threshold;
      38             :     }
      39             : 
      40             : #if HAL_QUADPLANE_ENABLED
      41      273423 :     is_flying_bool = quadplane.is_flying();
      42             : #endif
      43      273423 :     if (is_flying_bool) {
      44             :         // no need to look further
      45      235817 :     } else if(arming.is_armed()) {
      46             :         // when armed assuming flying and we need overwhelming evidence that we ARE NOT flying
      47             :         // short drop-outs of GPS are common during flight due to banking which points the antenna in different directions
      48      338811 :         bool gps_lost_recently = (gps.last_fix_time_ms() > 0) && // we have locked to GPS before
      49      338811 :                         (gps.status() < AP_GPS_FixType::FIX_2D) && // and it's lost now
      50        1026 :                         (now_ms - gps.last_fix_time_ms() < 5000); // but it wasn't that long ago (<5s)
      51             : 
      52      169406 :         if ((auto_state.last_flying_ms > 0) && gps_lost_recently) {
      53             :             // we've flown before, remove GPS constraints temporarily and only use airspeed
      54         154 :             is_flying_bool = airspeed_movement; // moving through the air
      55             :         } else {
      56             :             // Because ahrs.airspeed_estimate can return a continued high value after landing if flying in
      57             :             // strong winds above stall speed it is necessary to include the IMU based movement check.
      58      169252 :             is_flying_bool = (airspeed_movement && !AP::ins().is_still()) || // moving through the air
      59             :                                 gps_confirmed_movement; // locked and we're moving
      60             :         }
      61             : 
      62      169406 :         if (control_mode == &mode_auto) {
      63             :             /*
      64             :               make is_flying() more accurate during various auto modes
      65             :              */
      66             : 
      67             :             // Detect X-axis deceleration for probable ground impacts.
      68             :             // Limit the max probability so it can decay faster. This
      69             :             // will not change the is_flying state, anything above 0.1
      70             :             // is "true", it just allows it to decay faster once we decide we
      71             :             // aren't flying using the normal schemes
      72      104323 :             if (g.crash_accel_threshold == 0) {
      73      104323 :                 crash_state.impact_detected = false;
      74           0 :             } else if (ins.get_accel_peak_hold_neg_x() < -(g.crash_accel_threshold)) {
      75             :                 // large deceleration detected, lets lower confidence VERY quickly
      76           0 :                 crash_state.impact_detected = true;
      77           0 :                 crash_state.impact_timer_ms = now_ms;
      78           0 :                 if (isFlyingProbability > 0.2f) {
      79           0 :                     isFlyingProbability = 0.2f;
      80             :                 }
      81           0 :             } else if (crash_state.impact_detected &&
      82           0 :                 (now_ms - crash_state.impact_timer_ms > IS_FLYING_IMPACT_TIMER_MS)) {
      83             :                 // no impacts seen in a while, clear the flag so we stop clipping isFlyingProbability
      84           0 :                 crash_state.impact_detected = false;
      85             :             }
      86             : 
      87      104323 :             switch (flight_stage)
      88             :             {
      89        1845 :             case AP_FixedWing::FlightStage::TAKEOFF:
      90        1845 :                 break;
      91             : 
      92       82286 :             case AP_FixedWing::FlightStage::NORMAL:
      93       82286 :                 if (in_preLaunch_flight_stage()) {
      94             :                     // while on the ground, an uncalibrated airspeed sensor can drift to 7m/s so
      95             :                     // ensure we aren't showing a false positive.
      96          84 :                     is_flying_bool = false;
      97          84 :                     crash_state.is_crashed = false;
      98          84 :                     auto_state.started_flying_in_auto_ms = 0;
      99             :                 }
     100       82286 :                 break;
     101             : 
     102        2551 :             case AP_FixedWing::FlightStage::VTOL:
     103             :                 // TODO: detect ground impacts
     104        2551 :                 break;
     105             : 
     106       17518 :             case AP_FixedWing::FlightStage::LAND:
     107       17518 :                 if (landing.is_on_approach() && auto_state.sink_rate > 0.2f) {
     108        2805 :                     is_flying_bool = true;
     109             :                 }
     110       17518 :                 break;
     111             : 
     112         123 :             case AP_FixedWing::FlightStage::ABORT_LANDING:
     113         123 :                 if (auto_state.sink_rate < -0.5f) {
     114             :                     // steep climb
     115         115 :                     is_flying_bool = true;
     116             :                 }
     117         123 :                 break;
     118             : 
     119           0 :             default:
     120           0 :                 break;
     121             :             } // switch
     122             :         }
     123             :     } else {
     124             :         // when disarmed assume not flying and need overwhelming evidence that we ARE flying
     125       66411 :         is_flying_bool = airspeed_movement && gps_confirmed_movement;
     126             : 
     127       66411 :         if ((flight_stage == AP_FixedWing::FlightStage::TAKEOFF) || landing.is_flaring()) {
     128           0 :             is_flying_bool = false;
     129             :         }
     130             :     }
     131             : 
     132      273423 :     if (!crash_state.impact_detected || !is_flying_bool) {
     133             :         // when impact is detected, enforce a clip. Only allow isFlyingProbability to go down, not up.
     134             :         // low-pass the result.
     135             :         // coef=0.15f @ 5Hz takes 3.0s to go from 100% down to 10% (or 0% up to 90%)
     136      273423 :         isFlyingProbability = (0.85f * isFlyingProbability) + (0.15f * (float)is_flying_bool);
     137             :     }
     138             : 
     139             :     /*
     140             :       update last_flying_ms so we always know how long we have not
     141             :       been flying for. This helps for crash detection and auto-disarm
     142             :      */
     143      273423 :     bool new_is_flying = is_flying();
     144             : 
     145             :     // we are flying, note the time
     146      273423 :     if (new_is_flying) {
     147             : 
     148      196350 :         auto_state.last_flying_ms = now_ms;
     149             : 
     150      196350 :         if (!previous_is_flying) {
     151             :             // just started flying in any mode
     152         305 :             started_flying_ms = now_ms;
     153             :         }
     154             : 
     155      196350 :         if ((control_mode == &mode_auto) &&
     156      111745 :             ((auto_state.started_flying_in_auto_ms == 0) || !previous_is_flying) ) {
     157             : 
     158             :             // We just started flying, note that time also
     159         196 :             auto_state.started_flying_in_auto_ms = now_ms;
     160             :         }
     161             :     }
     162      273423 :     previous_is_flying = new_is_flying;
     163             : #if HAL_ADSB_ENABLED
     164      273423 :     adsb.set_is_flying(new_is_flying);
     165             : #endif
     166             : #if HAL_PARACHUTE_ENABLED
     167      273423 :     parachute.set_is_flying(new_is_flying);
     168             : #endif
     169             : #if AP_STATS_ENABLED
     170      273423 :     AP::stats()->set_flying(new_is_flying);
     171             : #endif
     172      273423 :     AP_Notify::flags.flying = new_is_flying;
     173             : 
     174      273423 :     crash_detection_update();
     175             : 
     176             : #if HAL_LOGGING_ENABLED
     177      273423 :     Log_Write_Status();
     178             : #endif
     179             : 
     180             :     // tell AHRS flying state
     181      273423 :     set_likely_flying(new_is_flying);
     182             : 
     183             :     // conservative ground mode value for rate D suppression
     184      273423 :     ground_mode = !is_flying() && !arming.is_armed_and_safety_off();
     185      273423 : }
     186             : 
     187             : /*
     188             :   return true if we think we are flying. This is a probabilistic
     189             :   estimate, and needs to be used very carefully. Each use case needs
     190             :   to be thought about individually.
     191             :  */
     192    13534976 : bool Plane::is_flying(void)
     193             : {
     194    13534976 :     if (arming.is_armed_and_safety_off()) {
     195             : #if HAL_QUADPLANE_ENABLED
     196     9948163 :         if (quadplane.is_flying_vtol()) {
     197     4943957 :             return true;
     198             :         }
     199             : #endif
     200             :         // when armed, assume we're flying unless we probably aren't
     201     5004206 :         return (isFlyingProbability >= 0.1f);
     202             :     }
     203             : 
     204             :     // when disarmed, assume we're not flying unless we probably are
     205     3586813 :     return (isFlyingProbability >= 0.9f);
     206             : }
     207             : 
     208             : /*
     209             :  * Determine if we have crashed
     210             :  */
     211      273423 : void Plane::crash_detection_update(void)
     212             : {
     213      273423 :     if (control_mode != &mode_auto || !aparm.crash_detection_enable)
     214             :     {
     215             :         // crash detection is only available in AUTO mode
     216      273423 :         crash_state.debounce_timer_ms = 0;
     217      273423 :         crash_state.is_crashed = false;
     218      273423 :         return;
     219             :     }
     220             : 
     221           0 :     uint32_t now_ms = AP_HAL::millis();
     222           0 :     bool crashed_near_land_waypoint = false;
     223           0 :     bool crashed = false;
     224           0 :     bool been_auto_flying = (auto_state.started_flying_in_auto_ms > 0) &&
     225           0 :                             (now_ms - auto_state.started_flying_in_auto_ms >= 2500);
     226             : 
     227           0 :     if (!is_flying() && arming.is_armed())
     228             :     {
     229           0 :         if (landing.is_expecting_impact()) {
     230             :             // We should be nice and level-ish in this flight stage. If not, we most
     231             :             // likely had a crazy landing. Throttle is inhibited already at the flare
     232             :             // but go ahead and notify GCS and perform any additional post-crash actions.
     233             :             // Declare a crash if we are oriented more that 60deg in pitch or roll
     234           0 :             if (!crash_state.checkedHardLanding && // only check once
     235           0 :                 been_auto_flying &&
     236           0 :                 (fabsf(ahrs.get_roll_deg()) > 60 || fabsf(ahrs.get_pitch_deg()) > 60)) {
     237           0 :                 crashed = true;
     238             : 
     239             :                 // did we "crash" within 75m of the landing location? Probably just a hard landing
     240           0 :                 crashed_near_land_waypoint =
     241           0 :                         current_loc.get_distance(mission.get_current_nav_cmd().content.location) < 75;
     242             : 
     243             :                 // trigger hard landing event right away, or never again. This inhibits a false hard landing
     244             :                 // event when, for example, a minute after a good landing you pick the plane up and
     245             :                 // this logic is still running and detects the plane is on its side as you carry it.
     246           0 :                 crash_state.debounce_timer_ms = now_ms;
     247           0 :                 crash_state.debounce_time_total_ms = 0; // no debounce
     248             :             }
     249             : 
     250           0 :             crash_state.checkedHardLanding = true;
     251             : 
     252           0 :         } else if (landing.is_on_approach()) {
     253             :             // when altitude gets low, we automatically flare so ground crashes
     254             :             // most likely can not be triggered from here. However,
     255             :             // a crash into a tree would be caught here.
     256           0 :             if (been_auto_flying) {
     257           0 :                 crashed = true;
     258           0 :                 crash_state.debounce_time_total_ms = CRASH_DETECTION_DELAY_MS;
     259             :             }
     260             : 
     261             :         } else {
     262           0 :             switch (flight_stage)
     263             :             {
     264           0 :             case AP_FixedWing::FlightStage::TAKEOFF:
     265           0 :                 if (g2.takeoff_throttle_accel_count == 1 && g.takeoff_throttle_min_accel > 0 &&
     266           0 :                     !throttle_suppressed) {
     267             :                     // if launching requires a single acceleration event and it
     268             :                     // has already happened but the aircraft is still not
     269             :                     // flying, then you either shook/hit the plane or it was a
     270             :                     // failed launch.
     271           0 :                     crashed = true;
     272           0 :                     crash_state.debounce_time_total_ms = CRASH_DETECTION_DELAY_MS;
     273             :                 }
     274             :                 // TODO: handle auto missions without NAV_TAKEOFF mission cmd
     275           0 :                 break;
     276             : 
     277           0 :             case AP_FixedWing::FlightStage::NORMAL:
     278           0 :                 if (!in_preLaunch_flight_stage() && been_auto_flying) {
     279           0 :                     crashed = true;
     280           0 :                     crash_state.debounce_time_total_ms = CRASH_DETECTION_DELAY_MS;
     281             :                 }
     282           0 :                 break;
     283             : 
     284           0 :             case AP_FixedWing::FlightStage::VTOL:
     285             :                 // we need a totally new method for this
     286           0 :                 crashed = false;
     287           0 :                 break;
     288             : 
     289           0 :             default:
     290           0 :                 break;
     291             :             } // switch
     292             :         }
     293             :     } else {
     294           0 :         crash_state.checkedHardLanding = false;
     295             :     }
     296             : 
     297             :     // if we have no GPS lock and we don't have a functional airspeed
     298             :     // sensor then don't do crash detection
     299           0 :     if (gps.status() < AP_GPS_FixType::FIX_3D) {
     300             : #if AP_AIRSPEED_ENABLED
     301           0 :         if (!airspeed.use() || !airspeed.healthy()) {
     302           0 :             crashed = false;
     303             :         }
     304             : #else
     305             :         crashed = false;
     306             : #endif
     307             :     }
     308             : 
     309           0 :     if (!crashed) {
     310             :         // reset timer
     311           0 :         crash_state.debounce_timer_ms = 0;
     312             : 
     313           0 :     } else if (crash_state.debounce_timer_ms == 0) {
     314             :         // start timer
     315           0 :         crash_state.debounce_timer_ms = now_ms;
     316             : 
     317           0 :     } else if ((now_ms - crash_state.debounce_timer_ms >= crash_state.debounce_time_total_ms) && !crash_state.is_crashed) {
     318           0 :         crash_state.is_crashed = true;
     319           0 :         if (aparm.crash_detection_enable & CRASH_DETECT_ACTION_BITMASK_DISARM) {
     320           0 :             arming.disarm(AP_Arming::Method::CRASH);
     321             :         }
     322           0 :         if (crashed_near_land_waypoint) {
     323           0 :             gcs().send_text(MAV_SEVERITY_CRITICAL, "Hard landing detected");
     324             :         } else {
     325           0 :             gcs().send_text(MAV_SEVERITY_EMERGENCY, "Crash detected");
     326             :         }
     327             :     }
     328             : }
     329             : 
     330             : /*
     331             :  * return true if we are in a pre-launch phase of an auto-launch, typically used in bungee launches
     332             :  */
     333       82286 : bool Plane::in_preLaunch_flight_stage(void)
     334             : {
     335       82286 :     if (control_mode == &mode_takeoff && throttle_suppressed) {
     336           0 :         return true;
     337             :     }
     338             : #if HAL_QUADPLANE_ENABLED
     339       82286 :     if (quadplane.is_vtol_takeoff(mission.get_current_nav_cmd().id)) {
     340           0 :         return false;
     341             :     }
     342             : #endif
     343      164572 :     return (control_mode == &mode_auto &&
     344       82286 :             throttle_suppressed &&
     345      164997 :             flight_stage == AP_FixedWing::FlightStage::NORMAL &&
     346       82711 :             mission.get_current_nav_cmd().id == MAV_CMD_NAV_TAKEOFF);
     347             : }