/*
* 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");
}
}
}
}
/*
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)) : 500;
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(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);
}
if (quadplane.is_flying()) {
is_flying_bool = true;
}
/*
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;
if (should_log(MASK_LOG_MODE)) {
Log_Write_Status();
}
}
/*
setup flap outputs
*/
void Plane::set_servos_flaps(void)
{
// Auto flap deployment
int8_t auto_flap_percent = 0;
int8_t manual_flap_percent = 0;
static int8_t last_auto_flap;
static int8_t last_manual_flap;
// work out any manual flap input
RC_Channel *flapin = RC_Channel::rc_channel(g.flapin_channel-1);
if (flapin != NULL && !failsafe.ch3_failsafe && failsafe.ch3_counter == 0) {
flapin->input();
manual_flap_percent = flapin->percent_input();
}
if (auto_throttle_mode) {
int16_t flapSpeedSource = 0;
if (ahrs.airspeed_sensor_enabled()) {
flapSpeedSource = target_airspeed_cm * 0.01f;
} else {
flapSpeedSource = aparm.throttle_cruise;
}
if (g.flap_2_speed != 0 && flapSpeedSource <= g.flap_2_speed) {
auto_flap_percent = g.flap_2_percent;
} else if ( g.flap_1_speed != 0 && flapSpeedSource <= g.flap_1_speed) {
auto_flap_percent = g.flap_1_percent;
} //else flaps stay at default zero deflection
/*
special flap levels for takeoff and landing. This works
better than speed based flaps as it leads to less
possibility of oscillation
*/
if (control_mode == AUTO) {
switch (flight_stage) {
case AP_SpdHgtControl::FLIGHT_TAKEOFF:
case AP_SpdHgtControl::FLIGHT_LAND_ABORT:
if (g.takeoff_flap_percent != 0) {
auto_flap_percent = g.takeoff_flap_percent;
}
break;
case AP_SpdHgtControl::FLIGHT_NORMAL:
if (auto_flap_percent != 0 && in_preLaunch_flight_stage()) {
// TODO: move this to a new FLIGHT_PRE_TAKEOFF stage
auto_flap_percent = g.takeoff_flap_percent;
}
break;
case AP_SpdHgtControl::FLIGHT_LAND_APPROACH:
case AP_SpdHgtControl::FLIGHT_LAND_PREFLARE:
case AP_SpdHgtControl::FLIGHT_LAND_FINAL:
if (g.land_flap_percent != 0) {
auto_flap_percent = g.land_flap_percent;
}
break;
default:
break;
}
}
}
// manual flap input overrides auto flap input
if (abs(manual_flap_percent) > auto_flap_percent) {
auto_flap_percent = manual_flap_percent;
}
flap_slew_limit(last_auto_flap, auto_flap_percent);
flap_slew_limit(last_manual_flap, manual_flap_percent);
RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_flap_auto, auto_flap_percent);
RC_Channel_aux::set_servo_out_for(RC_Channel_aux::k_flap, manual_flap_percent);
if (g.flaperon_output != MIXING_DISABLED && g.elevon_output == MIXING_DISABLED && g.mix_mode == 0) {
flaperon_update(auto_flap_percent);
}
}
/*
setup flap outputs
*/
void Plane::set_servos_flaps(void)
{
// Auto flap deployment
int8_t auto_flap_percent = 0;
int8_t manual_flap_percent = 0;
// work out any manual flap input
RC_Channel *flapin = RC_Channels::rc_channel(g.flapin_channel-1);
if (flapin != nullptr && !failsafe.rc_failsafe && failsafe.throttle_counter == 0) {
manual_flap_percent = flapin->percent_input();
}
if (auto_throttle_mode) {
int16_t flapSpeedSource = 0;
if (ahrs.airspeed_sensor_enabled()) {
flapSpeedSource = target_airspeed_cm * 0.01f;
} else {
flapSpeedSource = aparm.throttle_cruise;
}
if (g.flap_2_speed != 0 && flapSpeedSource <= g.flap_2_speed) {
auto_flap_percent = g.flap_2_percent;
} else if ( g.flap_1_speed != 0 && flapSpeedSource <= g.flap_1_speed) {
auto_flap_percent = g.flap_1_percent;
} //else flaps stay at default zero deflection
if (flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND && landing.get_flap_percent() != 0) {
auto_flap_percent = landing.get_flap_percent();
}
/*
special flap levels for takeoff and landing. This works
better than speed based flaps as it leads to less
possibility of oscillation
*/
if (control_mode == AUTO) {
switch (flight_stage) {
case AP_Vehicle::FixedWing::FLIGHT_TAKEOFF:
case AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND:
if (g.takeoff_flap_percent != 0) {
auto_flap_percent = g.takeoff_flap_percent;
}
break;
case AP_Vehicle::FixedWing::FLIGHT_NORMAL:
if (g.takeoff_flap_percent != 0 && in_preLaunch_flight_stage()) {
// TODO: move this to a new FLIGHT_PRE_TAKEOFF stage
auto_flap_percent = g.takeoff_flap_percent;
}
break;
default:
break;
}
}
}
// manual flap input overrides auto flap input
if (abs(manual_flap_percent) > auto_flap_percent) {
auto_flap_percent = manual_flap_percent;
}
SRV_Channels::set_output_scaled(SRV_Channel::k_flap_auto, auto_flap_percent);
SRV_Channels::set_output_scaled(SRV_Channel::k_flap, manual_flap_percent);
if (g.flap_slewrate) {
SRV_Channels::limit_slew_rate(SRV_Channel::k_flap_auto, g.flap_slewrate, G_Dt);
SRV_Channels::limit_slew_rate(SRV_Channel::k_flap, g.flap_slewrate, G_Dt);
}
// output to flaperons, if any
flaperon_update(auto_flap_percent);
}
/*
* 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");
}
}
}
}
