// crash_check - disarms motors if a crash or block has been detected // crashes are detected by the vehicle being static (no speed) for more than CRASH_CHECK_TRIGGER_SEC and motor are running // called at 10Hz void Rover::crash_check() { static uint16_t crash_counter; // number of iterations vehicle may have been crashed bool crashed = false; //stores crash state // return immediately if disarmed, crash checking is disabled or vehicle is Hold, Manual or Acro mode(if it is not a balance bot) if (!arming.is_armed() || g.fs_crash_check == FS_CRASH_DISABLE || ((!control_mode->is_autopilot_mode()) && (!is_balancebot()))) { crash_counter = 0; return; } // Crashed if pitch/roll > crash_angle if ((g2.crash_angle != 0) && ((fabsf(ahrs.pitch) > radians(g2.crash_angle)) || (fabsf(ahrs.roll) > radians(g2.crash_angle)))) { crashed = true; } // TODO : Check if min vel can be calculated // min_vel = ( CRASH_CHECK_THROTTLE_MIN * g.speed_cruise) / g.throttle_cruise; if (!is_balancebot()) { if (!crashed && ((ahrs.groundspeed() >= CRASH_CHECK_VEL_MIN) || // Check velocity (fabsf(ahrs.get_gyro().z) >= CRASH_CHECK_VEL_MIN) || // Check turn speed (fabsf(g2.motors.get_throttle()) < CRASH_CHECK_THROTTLE_MIN))) { crash_counter = 0; return; } // we may be crashing crash_counter++; // check if crashing for 2 seconds if (crash_counter >= (CRASH_CHECK_TRIGGER_SEC * 10)) { crashed = true; } } if (crashed) { AP::logger().Write_Error(LogErrorSubsystem::CRASH_CHECK, LogErrorCode::CRASH_CHECK_CRASH); if (is_balancebot()) { // send message to gcs gcs().send_text(MAV_SEVERITY_EMERGENCY, "Crash: Disarming"); disarm_motors(); } else { // send message to gcs gcs().send_text(MAV_SEVERITY_EMERGENCY, "Crash: Going to HOLD"); // change mode to hold and disarm set_mode(mode_hold, MODE_REASON_CRASH_FAILSAFE); if (g.fs_crash_check == FS_CRASH_HOLD_AND_DISARM) { disarm_motors(); } } } }
void Rover::handle_battery_failsafe(const char* type_str, const int8_t action) { switch ((Failsafe_Action)action) { case Failsafe_Action_None: break; case Failsafe_Action_SmartRTL: if (set_mode(mode_smartrtl, MODE_REASON_FAILSAFE)) { break; } FALLTHROUGH; case Failsafe_Action_RTL: if (set_mode(mode_rtl, MODE_REASON_FAILSAFE)) { break; } FALLTHROUGH; case Failsafe_Action_Hold: set_mode(mode_hold, MODE_REASON_FAILSAFE); break; case Failsafe_Action_SmartRTL_Hold: if (!set_mode(mode_smartrtl, MODE_REASON_FAILSAFE)) { set_mode(mode_hold, MODE_REASON_FAILSAFE); } break; case Failsafe_Action_Terminate: #if ADVANCED_FAILSAFE == ENABLED char battery_type_str[17]; snprintf(battery_type_str, 17, "%s battery", type_str); afs.gcs_terminate(true, battery_type_str); #else disarm_motors(); #endif // ADVANCED_FAILSAFE == ENABLED break; } }
/* this failsafe_check function is called from the core timer interrupt at 1kHz. */ void Rover::failsafe_check() { static uint16_t last_ticks; static uint32_t last_timestamp; const uint32_t tnow = AP_HAL::micros(); const uint16_t ticks = scheduler.ticks(); if (ticks != last_ticks) { // the main loop is running, all is OK last_ticks = ticks; last_timestamp = tnow; return; } if (tnow - last_timestamp > 200000) { // we have gone at least 0.2 seconds since the main loop // ran. That means we're in trouble, or perhaps are in // an initialisation routine or log erase. disarm the motors // To-Do: log error to dataflash if (arming.is_armed()) { // disarm motors disarm_motors(); } } }
/* check for driver input on rudder/steering stick for arming/disarming */ void Rover::rudder_arm_disarm_check() { // In Rover we need to check that its set to the throttle trim and within the DZ // if throttle is not within trim dz, then pilot cannot rudder arm/disarm if (!channel_throttle->in_trim_dz()) { rudder_arm_timer = 0; return; } // if not in a manual throttle mode then disallow rudder arming/disarming if (control_mode->auto_throttle()) { rudder_arm_timer = 0; return; } if (!arming.is_armed()) { // when not armed, full right rudder starts arming counter if (channel_steer->get_control_in() > 4000) { const uint32_t now = millis(); if (rudder_arm_timer == 0 || now - rudder_arm_timer < 3000) { if (rudder_arm_timer == 0) { rudder_arm_timer = now; } } else { // time to arm! arm_motors(AP_Arming::RUDDER); rudder_arm_timer = 0; } } else { // not at full right rudder rudder_arm_timer = 0; } } else if (!motor_active() & !g2.motors.have_skid_steering()) { // when armed and motor not active (not moving), full left rudder starts disarming counter // This is disabled for skid steering otherwise when tring to turn a skid steering rover around // the rover would disarm if (channel_steer->get_control_in() < -4000) { const uint32_t now = millis(); if (rudder_arm_timer == 0 || now - rudder_arm_timer < 3000) { if (rudder_arm_timer == 0) { rudder_arm_timer = now; } } else { // time to disarm! disarm_motors(); rudder_arm_timer = 0; } } else { // not at full left rudder rudder_arm_timer = 0; } } }
/* check for driver input on rudder/steering stick for arming/disarming */ void Rover::rudder_arm_disarm_check() { // In Rover we need to check that its set to the throttle trim and within the DZ // if throttle is not within trim dz, then pilot cannot rudder arm/disarm if (!channel_throttle->in_trim_dz()) { rudder_arm_timer = 0; return; } // check if arming/disarming allowed from this mode if (!control_mode->allows_arming_from_transmitter()) { rudder_arm_timer = 0; return; } if (!arming.is_armed()) { // when not armed, full right rudder starts arming counter if (channel_steer->get_control_in() > 4000) { const uint32_t now = millis(); if (rudder_arm_timer == 0 || now - rudder_arm_timer < ARM_DELAY_MS) { if (rudder_arm_timer == 0) { rudder_arm_timer = now; } } else { // time to arm! arm_motors(AP_Arming::RUDDER); rudder_arm_timer = 0; } } else { // not at full right rudder rudder_arm_timer = 0; } } else if (!g2.motors.active()) { // when armed and motor not active (not moving), full left rudder starts disarming counter if (channel_steer->get_control_in() < -4000) { const uint32_t now = millis(); if (rudder_arm_timer == 0 || now - rudder_arm_timer < ARM_DELAY_MS) { if (rudder_arm_timer == 0) { rudder_arm_timer = now; } } else { // time to disarm! disarm_motors(); rudder_arm_timer = 0; } } else { // not at full left rudder rudder_arm_timer = 0; } } }
/* If land_DisarmDelay is enabled (non-zero), check for a landing then auto-disarm after time expires only called from AP_Landing, when the landing library is ready to disarm */ void Plane::disarm_if_autoland_complete() { if (landing.get_disarm_delay() > 0 && !is_flying() && arming.arming_required() != AP_Arming::NO && arming.is_armed()) { /* we have auto disarm enabled. See if enough time has passed */ if (millis() - auto_state.last_flying_ms >= landing.get_disarm_delay()*1000UL) { if (disarm_motors()) { gcs().send_text(MAV_SEVERITY_INFO,"Auto disarmed"); } } } }
/* If land_DisarmDelay is enabled (non-zero), check for a landing then auto-disarm after time expires */ void Plane::disarm_if_autoland_complete() { if (g.land_disarm_delay > 0 && auto_state.land_complete && !is_flying() && arming.arming_required() != AP_Arming::NO && arming.is_armed()) { /* we have auto disarm enabled. See if enough time has passed */ if (millis() - auto_state.last_flying_ms >= g.land_disarm_delay*1000UL) { if (disarm_motors()) { gcs_send_text_P(SEVERITY_LOW,PSTR("Auto-Disarmed")); } } } }
// crash_check - disarms motors if a crash or block has been detected // crashes are detected by the vehicle being static (no speed) for more than CRASH_CHECK_TRIGGER_SEC and motor are running // called at 10Hz void Rover::crash_check() { static uint16_t crash_counter; // number of iterations vehicle may have been crashed // return immediately if disarmed, or crash checking disabled or in HOLD mode if (!arming.is_armed() || g.fs_crash_check == FS_CRASH_DISABLE || (control_mode != GUIDED && control_mode != AUTO)) { crash_counter = 0; return; } // TODO : Check if min vel can be calculated // min_vel = ( CRASH_CHECK_THROTTLE_MIN * g.speed_cruise) / g.throttle_cruise; if ((ahrs.groundspeed() >= CRASH_CHECK_VEL_MIN) || // Check velocity (fabsf(ahrs.get_gyro().z) >= CRASH_CHECK_VEL_MIN) || // Check turn speed ((100 * fabsf(SRV_Channels::get_output_norm(SRV_Channel::k_throttle))) < CRASH_CHECK_THROTTLE_MIN)) { crash_counter = 0; return; } // we may be crashing crash_counter++; // check if crashing for 2 seconds if (crash_counter >= (CRASH_CHECK_TRIGGER_SEC * 10)) { // log an error in the dataflash Log_Write_Error(ERROR_SUBSYSTEM_CRASH_CHECK, ERROR_CODE_CRASH_CHECK_CRASH); // send message to gcs gcs_send_text(MAV_SEVERITY_EMERGENCY, "Crash: Going to HOLD"); // change mode to hold and disarm set_mode(HOLD); if (g.fs_crash_check == FS_CRASH_HOLD_AND_DISARM) { disarm_motors(); } } }
// crash_check - disarms motors if a crash or block has been detected // crashes are detected by the vehicle being static (no speed) for more than CRASH_CHECK_TRIGGER_SEC and motor are running // called at 10Hz void Rover::crash_check() { static uint16_t crash_counter; // number of iterations vehicle may have been crashed // return immediately if disarmed, crash checking is disabled or vehicle is Hold, Manual or Acro mode if (!arming.is_armed() || g.fs_crash_check == FS_CRASH_DISABLE || (!control_mode->is_autopilot_mode())) { crash_counter = 0; return; } // TODO : Check if min vel can be calculated // min_vel = ( CRASH_CHECK_THROTTLE_MIN * g.speed_cruise) / g.throttle_cruise; if ((ahrs.groundspeed() >= CRASH_CHECK_VEL_MIN) || // Check velocity (fabsf(ahrs.get_gyro().z) >= CRASH_CHECK_VEL_MIN) || // Check turn speed (fabsf(g2.motors.get_throttle()) < CRASH_CHECK_THROTTLE_MIN)) { crash_counter = 0; return; } // we may be crashing crash_counter++; // check if crashing for 2 seconds if (crash_counter >= (CRASH_CHECK_TRIGGER_SEC * 10)) { // log an error in the dataflash Log_Write_Error(ERROR_SUBSYSTEM_CRASH_CHECK, ERROR_CODE_CRASH_CHECK_CRASH); // send message to gcs gcs().send_text(MAV_SEVERITY_EMERGENCY, "Crash: Going to HOLD"); // change mode to hold and disarm set_mode(mode_hold, MODE_REASON_CRASH_FAILSAFE); if (g.fs_crash_check == FS_CRASH_HOLD_AND_DISARM) { disarm_motors(); } } }
/* * Determine if we have crashed */ void Plane::crash_detection_update(void) { if (control_mode != AUTO || !g.crash_detection_enable) { // crash detection is only available in AUTO mode crash_state.debounce_timer_ms = 0; crash_state.is_crashed = false; return; } uint32_t now_ms = AP_HAL::millis(); bool crashed_near_land_waypoint = false; bool crashed = false; bool been_auto_flying = (auto_state.started_flying_in_auto_ms > 0) && (now_ms - auto_state.started_flying_in_auto_ms >= 2500); if (!is_flying() && been_auto_flying) { switch (flight_stage) { case AP_SpdHgtControl::FLIGHT_TAKEOFF: case AP_SpdHgtControl::FLIGHT_NORMAL: if (!in_preLaunch_flight_stage()) { crashed = true; } // TODO: handle auto missions without NAV_TAKEOFF mission cmd break; case AP_SpdHgtControl::FLIGHT_VTOL: // we need a totally new method for this crashed = false; break; case AP_SpdHgtControl::FLIGHT_LAND_APPROACH: crashed = true; // when altitude gets low, we automatically progress to FLIGHT_LAND_FINAL // so ground crashes most likely can not be triggered from here. However, // a crash into a tree would be caught here. break; case AP_SpdHgtControl::FLIGHT_LAND_PREFLARE: case AP_SpdHgtControl::FLIGHT_LAND_FINAL: // We should be nice and level-ish in this flight stage. If not, we most // likely had a crazy landing. Throttle is inhibited already at the flare // but go ahead and notify GCS and perform any additional post-crash actions. // Declare a crash if we are oriented more that 60deg in pitch or roll if (!crash_state.checkedHardLanding && // only check once (fabsf(ahrs.roll_sensor) > 6000 || fabsf(ahrs.pitch_sensor) > 6000)) { crashed = true; // did we "crash" within 75m of the landing location? Probably just a hard landing crashed_near_land_waypoint = get_distance(current_loc, mission.get_current_nav_cmd().content.location) < 75; // trigger hard landing event right away, or never again. This inhibits a false hard landing // event when, for example, a minute after a good landing you pick the plane up and // this logic is still running and detects the plane is on its side as you carry it. crash_state.debounce_timer_ms = now_ms + CRASH_DETECTION_DELAY_MS; } crash_state.checkedHardLanding = true; break; default: break; } // switch } else { crash_state.checkedHardLanding = false; } if (!crashed) { // reset timer crash_state.debounce_timer_ms = 0; } else if (crash_state.debounce_timer_ms == 0) { // start timer crash_state.debounce_timer_ms = now_ms; } else if ((now_ms - crash_state.debounce_timer_ms >= CRASH_DETECTION_DELAY_MS) && !crash_state.is_crashed) { crash_state.is_crashed = true; if (g.crash_detection_enable == CRASH_DETECT_ACTION_BITMASK_DISABLED) { if (crashed_near_land_waypoint) { gcs_send_text(MAV_SEVERITY_CRITICAL, "Hard landing detected. No action taken"); } else { gcs_send_text(MAV_SEVERITY_EMERGENCY, "Crash detected. No action taken"); } } else { if (g.crash_detection_enable & CRASH_DETECT_ACTION_BITMASK_DISARM) { disarm_motors(); } auto_state.land_complete = true; if (crashed_near_land_waypoint) { gcs_send_text(MAV_SEVERITY_CRITICAL, "Hard landing detected"); } else { gcs_send_text(MAV_SEVERITY_EMERGENCY, "Crash detected"); } } } }
/* check for pilot input on rudder stick for arming/disarming */ void Plane::rudder_arm_disarm_check() { AP_Arming::ArmingRudder arming_rudder = arming.rudder_arming(); if (arming_rudder == AP_Arming::ARMING_RUDDER_DISABLED) { //parameter disallows rudder arming/disabling return; } // if throttle is not down, then pilot cannot rudder arm/disarm if (channel_throttle->control_in != 0){ rudder_arm_timer = 0; return; } // if not in a manual throttle mode then disallow rudder arming/disarming if (auto_throttle_mode ) { rudder_arm_timer = 0; return; } if (!arming.is_armed()) { // when not armed, full right rudder starts arming counter if (channel_rudder->control_in > 4000) { uint32_t now = millis(); if (rudder_arm_timer == 0 || now - rudder_arm_timer < 3000) { if (rudder_arm_timer == 0) { rudder_arm_timer = now; } } else { //time to arm! arm_motors(AP_Arming::RUDDER); rudder_arm_timer = 0; } } else { // not at full right rudder rudder_arm_timer = 0; } } else if (arming_rudder == AP_Arming::ARMING_RUDDER_ARMDISARM && !is_flying()) { // when armed and not flying, full left rudder starts disarming counter if (channel_rudder->control_in < -4000) { uint32_t now = millis(); if (rudder_arm_timer == 0 || now - rudder_arm_timer < 3000) { if (rudder_arm_timer == 0) { rudder_arm_timer = now; } } else { //time to disarm! disarm_motors(); rudder_arm_timer = 0; } } else { // not at full left rudder rudder_arm_timer = 0; } } }
/* * Determine if we have crashed */ void Plane::crash_detection_update(void) { if (control_mode != AUTO || !aparm.crash_detection_enable) { // crash detection is only available in AUTO mode crash_state.debounce_timer_ms = 0; crash_state.is_crashed = false; return; } uint32_t now_ms = AP_HAL::millis(); bool crashed_near_land_waypoint = false; bool crashed = false; bool been_auto_flying = (auto_state.started_flying_in_auto_ms > 0) && (now_ms - auto_state.started_flying_in_auto_ms >= 2500); if (!is_flying() && arming.is_armed()) { if (landing.is_expecting_impact()) { // We should be nice and level-ish in this flight stage. If not, we most // likely had a crazy landing. Throttle is inhibited already at the flare // but go ahead and notify GCS and perform any additional post-crash actions. // Declare a crash if we are oriented more that 60deg in pitch or roll if (!crash_state.checkedHardLanding && // only check once been_auto_flying && (labs(ahrs.roll_sensor) > 6000 || labs(ahrs.pitch_sensor) > 6000)) { crashed = true; // did we "crash" within 75m of the landing location? Probably just a hard landing crashed_near_land_waypoint = current_loc.get_distance(mission.get_current_nav_cmd().content.location) < 75; // trigger hard landing event right away, or never again. This inhibits a false hard landing // event when, for example, a minute after a good landing you pick the plane up and // this logic is still running and detects the plane is on its side as you carry it. crash_state.debounce_timer_ms = now_ms; crash_state.debounce_time_total_ms = 0; // no debounce } crash_state.checkedHardLanding = true; } else if (landing.is_on_approach()) { // when altitude gets low, we automatically flare so ground crashes // most likely can not be triggered from here. However, // a crash into a tree would be caught here. if (been_auto_flying) { crashed = true; crash_state.debounce_time_total_ms = CRASH_DETECTION_DELAY_MS; } } else { switch (flight_stage) { case AP_Vehicle::FixedWing::FLIGHT_TAKEOFF: if (g.takeoff_throttle_min_accel > 0 && !throttle_suppressed) { // if you have an acceleration holding back throttle, but you met the // accel threshold but still not fying, then you either shook/hit the // plane or it was a failed launch. crashed = true; crash_state.debounce_time_total_ms = CRASH_DETECTION_DELAY_MS; } // TODO: handle auto missions without NAV_TAKEOFF mission cmd break; case AP_Vehicle::FixedWing::FLIGHT_NORMAL: if (!in_preLaunch_flight_stage() && been_auto_flying) { crashed = true; crash_state.debounce_time_total_ms = CRASH_DETECTION_DELAY_MS; } break; case AP_Vehicle::FixedWing::FLIGHT_VTOL: // we need a totally new method for this crashed = false; break; default: break; } // switch } } else { crash_state.checkedHardLanding = false; } // if we have no GPS lock and we don't have a functional airspeed // sensor then don't do crash detection if (gps.status() < AP_GPS::GPS_OK_FIX_3D && (!airspeed.use() || !airspeed.healthy())) { crashed = false; } if (!crashed) { // reset timer crash_state.debounce_timer_ms = 0; } else if (crash_state.debounce_timer_ms == 0) { // start timer crash_state.debounce_timer_ms = now_ms; } else if ((now_ms - crash_state.debounce_timer_ms >= crash_state.debounce_time_total_ms) && !crash_state.is_crashed) { crash_state.is_crashed = true; if (aparm.crash_detection_enable == CRASH_DETECT_ACTION_BITMASK_DISABLED) { if (crashed_near_land_waypoint) { gcs().send_text(MAV_SEVERITY_CRITICAL, "Hard landing detected. No action taken"); } else { gcs().send_text(MAV_SEVERITY_EMERGENCY, "Crash detected. No action taken"); } } else { if (aparm.crash_detection_enable & CRASH_DETECT_ACTION_BITMASK_DISARM) { disarm_motors(); } if (crashed_near_land_waypoint) { gcs().send_text(MAV_SEVERITY_CRITICAL, "Hard landing detected"); } else { gcs().send_text(MAV_SEVERITY_EMERGENCY, "Crash detected"); } } } }
/* check for pilot input on rudder stick for arming/disarming */ void Plane::rudder_arm_disarm_check() { AP_Arming::ArmingRudder arming_rudder = arming.get_rudder_arming_type(); if (arming_rudder == AP_Arming::ARMING_RUDDER_DISABLED) { //parameter disallows rudder arming/disabling return; } // if throttle is not down, then pilot cannot rudder arm/disarm if (get_throttle_input() != 0){ rudder_arm_timer = 0; return; } // if not in a manual throttle mode and not in CRUISE or FBWB // modes then disallow rudder arming/disarming if (auto_throttle_mode && (control_mode != CRUISE && control_mode != FLY_BY_WIRE_B)) { rudder_arm_timer = 0; return; } if (!arming.is_armed()) { // when not armed, full right rudder starts arming counter if (channel_rudder->get_control_in() > 4000) { uint32_t now = millis(); if (rudder_arm_timer == 0 || now - rudder_arm_timer < 3000) { if (rudder_arm_timer == 0) { rudder_arm_timer = now; } } else { //time to arm! arm_motors(AP_Arming::RUDDER); rudder_arm_timer = 0; } } else { // not at full right rudder rudder_arm_timer = 0; } } else if ((arming_rudder == AP_Arming::ARMING_RUDDER_ARMDISARM) && !is_flying()) { // when armed and not flying, full left rudder starts disarming counter if (channel_rudder->get_control_in() < -4000) { uint32_t now = millis(); if (rudder_arm_timer == 0 || now - rudder_arm_timer < 3000) { if (rudder_arm_timer == 0) { rudder_arm_timer = now; } } else { //time to disarm! disarm_motors(); rudder_arm_timer = 0; } } else { // not at full left rudder rudder_arm_timer = 0; } } }
/* * Determine if we have crashed */ void Plane::crash_detection_update(void) { if (control_mode != AUTO) { // crash detection is only available in AUTO mode crash_state.debounce_timer_ms = 0; return; } uint32_t now_ms = hal.scheduler->millis(); bool auto_launch_detected; bool crashed_near_land_waypoint = false; bool crashed = false; bool been_auto_flying = (auto_state.started_flying_in_auto_ms > 0) && (now_ms - auto_state.started_flying_in_auto_ms >= 2500); if (!is_flying()) { switch (flight_stage) { case AP_SpdHgtControl::FLIGHT_TAKEOFF: auto_launch_detected = !throttle_suppressed && (g.takeoff_throttle_min_accel > 0); if (been_auto_flying || // failed hand launch auto_launch_detected) { // threshold of been_auto_flying may not be met on auto-launches // has launched but is no longer flying. That's a crash on takeoff. crashed = true; } break; case AP_SpdHgtControl::FLIGHT_NORMAL: if (been_auto_flying) { crashed = true; } // TODO: handle auto missions without NAV_TAKEOFF mission cmd break; case AP_SpdHgtControl::FLIGHT_LAND_APPROACH: if (been_auto_flying) { crashed = true; } // when altitude gets low, we automatically progress to FLIGHT_LAND_FINAL // so ground crashes most likely can not be triggered from here. However, // a crash into a tree, for example, would be. break; case AP_SpdHgtControl::FLIGHT_LAND_FINAL: // We should be nice and level-ish in this flight stage. If not, we most // likely had a crazy landing. Throttle is inhibited already at the flare // but go ahead and notify GCS and perform any additional post-crash actions. // Declare a crash if we are oriented more that 60deg in pitch or roll if (been_auto_flying && !crash_state.checkHardLanding && // only check once (fabsf(ahrs.roll_sensor) > 6000 || fabsf(ahrs.pitch_sensor) > 6000)) { crashed = true; // did we "crash" within 75m of the landing location? Probably just a hard landing crashed_near_land_waypoint = get_distance(current_loc, mission.get_current_nav_cmd().content.location) < 75; // trigger hard landing event right away, or never again. This inhibits a false hard landing // event when, for example, a minute after a good landing you pick the plane up and // this logic is still running and detects the plane is on its side as you carry it. crash_state.debounce_timer_ms = now_ms + 2500; } crash_state.checkHardLanding = true; break; } // switch } else { crash_state.checkHardLanding = false; } if (!crashed) { // reset timer crash_state.debounce_timer_ms = 0; } else if (crash_state.debounce_timer_ms == 0) { // start timer crash_state.debounce_timer_ms = now_ms; } else if ((now_ms - crash_state.debounce_timer_ms >= 2500) && !crash_state.is_crashed) { crash_state.is_crashed = true; if (g.crash_detection_enable == CRASH_DETECT_ACTION_BITMASK_DISABLED) { if (crashed_near_land_waypoint) { gcs_send_text_P(MAV_SEVERITY_CRITICAL, PSTR("Hard Landing Detected - no action taken")); } else { gcs_send_text_P(MAV_SEVERITY_CRITICAL, PSTR("Crash Detected - no action taken")); } } else { if (g.crash_detection_enable & CRASH_DETECT_ACTION_BITMASK_DISARM) { disarm_motors(); } auto_state.land_complete = true; if (crashed_near_land_waypoint) { gcs_send_text_P(MAV_SEVERITY_CRITICAL, PSTR("Hard Landing Detected")); } else { gcs_send_text_P(MAV_SEVERITY_CRITICAL, PSTR("Crash Detected")); } } } }