void Plane::one_second_loop() { if (should_log(MASK_LOG_CURRENT)) Log_Write_Current(); // send a heartbeat gcs_send_message(MSG_HEARTBEAT); // make it possible to change control channel ordering at runtime set_control_channels(); // make it possible to change orientation at runtime ahrs.set_orientation(); // sync MAVLink system ID mavlink_system.sysid = g.sysid_this_mav; update_aux(); // update notify flags AP_Notify::flags.pre_arm_check = arming.pre_arm_checks(false); AP_Notify::flags.pre_arm_gps_check = true; AP_Notify::flags.armed = arming.is_armed() || arming.arming_required() == AP_Arming::NO; crash_detection_update(); #if AP_TERRAIN_AVAILABLE if (should_log(MASK_LOG_GPS)) { terrain.log_terrain_data(DataFlash); } #endif // piggyback the status log entry on the MODE log entry flag if (should_log(MASK_LOG_MODE)) { Log_Write_Status(); } ins.set_raw_logging(should_log(MASK_LOG_IMU_RAW)); }
/* Do we think we are flying? Probabilistic method where a bool is low-passed and considered a probability. */ void Plane::update_is_flying_5Hz(void) { float aspeed; bool is_flying_bool; uint32_t now_ms = AP_HAL::millis(); uint32_t ground_speed_thresh_cm = (g.min_gndspeed_cm > 0) ? ((uint32_t)(g.min_gndspeed_cm*0.9f)) : GPS_IS_FLYING_SPEED_CMS; bool gps_confirmed_movement = (gps.status() >= AP_GPS::GPS_OK_FIX_3D) && (gps.ground_speed_cm() >= ground_speed_thresh_cm); // airspeed at least 75% of stall speed? bool airspeed_movement = ahrs.airspeed_estimate(&aspeed) && (aspeed >= (aparm.airspeed_min*0.75f)); if (quadplane.is_flying()) { is_flying_bool = true; } else if(arming.is_armed()) { // when armed assuming flying and we need overwhelming evidence that we ARE NOT flying // short drop-outs of GPS are common during flight due to banking which points the antenna in different directions bool gps_lost_recently = (gps.last_fix_time_ms() > 0) && // we have locked to GPS before (gps.status() < AP_GPS::GPS_OK_FIX_2D) && // and it's lost now (now_ms - gps.last_fix_time_ms() < 5000); // but it wasn't that long ago (<5s) if ((auto_state.last_flying_ms > 0) && gps_lost_recently) { // we've flown before, remove GPS constraints temporarily and only use airspeed is_flying_bool = airspeed_movement; // moving through the air } else { // we've never flown yet, require good GPS movement is_flying_bool = airspeed_movement || // moving through the air gps_confirmed_movement; // locked and we're moving } if (control_mode == AUTO) { /* make is_flying() more accurate during various auto modes */ // Detect X-axis deceleration for probable ground impacts. // Limit the max probability so it can decay faster. This // will not change the is_flying state, anything above 0.1 // is "true", it just allows it to decay faster once we decide we // aren't flying using the normal schemes if (g.crash_accel_threshold == 0) { crash_state.impact_detected = false; } else if (ins.get_accel_peak_hold_neg_x() < -(g.crash_accel_threshold)) { // large deceleration detected, lets lower confidence VERY quickly crash_state.impact_detected = true; crash_state.impact_timer_ms = now_ms; if (isFlyingProbability > 0.2f) { isFlyingProbability = 0.2f; } } else if (crash_state.impact_detected && (now_ms - crash_state.impact_timer_ms > IS_FLYING_IMPACT_TIMER_MS)) { // no impacts seen in a while, clear the flag so we stop clipping isFlyingProbability crash_state.impact_detected = false; } switch (flight_stage) { case AP_SpdHgtControl::FLIGHT_TAKEOFF: break; case AP_SpdHgtControl::FLIGHT_NORMAL: if (in_preLaunch_flight_stage()) { // while on the ground, an uncalibrated airspeed sensor can drift to 7m/s so // ensure we aren't showing a false positive. is_flying_bool = false; crash_state.is_crashed = false; auto_state.started_flying_in_auto_ms = 0; } break; case AP_SpdHgtControl::FLIGHT_VTOL: // TODO: detect ground impacts break; case AP_SpdHgtControl::FLIGHT_LAND_APPROACH: if (fabsf(auto_state.sink_rate) > 0.2f) { is_flying_bool = true; } break; case AP_SpdHgtControl::FLIGHT_LAND_PREFLARE: case AP_SpdHgtControl::FLIGHT_LAND_FINAL: break; case AP_SpdHgtControl::FLIGHT_LAND_ABORT: if (auto_state.sink_rate < -0.5f) { // steep climb is_flying_bool = true; } break; default: break; } // switch } } else { // when disarmed assume not flying and need overwhelming evidence that we ARE flying is_flying_bool = airspeed_movement && gps_confirmed_movement; if ((control_mode == AUTO) && ((flight_stage == AP_SpdHgtControl::FLIGHT_TAKEOFF) || (flight_stage == AP_SpdHgtControl::FLIGHT_LAND_FINAL)) ) { is_flying_bool = false; } } if (!crash_state.impact_detected || !is_flying_bool) { // when impact is detected, enforce a clip. Only allow isFlyingProbability to go down, not up. // low-pass the result. // coef=0.15f @ 5Hz takes 3.0s to go from 100% down to 10% (or 0% up to 90%) isFlyingProbability = (0.85f * isFlyingProbability) + (0.15f * (float)is_flying_bool); } /* update last_flying_ms so we always know how long we have not been flying for. This helps for crash detection and auto-disarm */ bool new_is_flying = is_flying(); // we are flying, note the time if (new_is_flying) { auto_state.last_flying_ms = now_ms; if (!previous_is_flying) { // just started flying in any mode started_flying_ms = now_ms; } if ((control_mode == AUTO) && ((auto_state.started_flying_in_auto_ms == 0) || !previous_is_flying) ) { // We just started flying, note that time also auto_state.started_flying_in_auto_ms = now_ms; } } previous_is_flying = new_is_flying; adsb.set_is_flying(new_is_flying); crash_detection_update(); if (should_log(MASK_LOG_MODE)) { Log_Write_Status(); } }