From bd0f0a7536215d6283279c72833ef4e6f84038bf Mon Sep 17 00:00:00 2001 From: Lucas De Marchi Date: Sat, 24 Oct 2015 19:52:39 -0200 Subject: [PATCH] ArduPlane: convert dos linefeeds to unix --- ArduPlane/is_flying.cpp | 490 ++++++++++++++++++++-------------------- 1 file changed, 245 insertions(+), 245 deletions(-) diff --git a/ArduPlane/is_flying.cpp b/ArduPlane/is_flying.cpp index ab7d2ed42b..917b5262ac 100644 --- a/ArduPlane/is_flying.cpp +++ b/ArduPlane/is_flying.cpp @@ -1,245 +1,245 @@ -/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - -#include "Plane.h" - -/* - is_flying and crash detection logic - */ - -/* - 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 = hal.scheduler->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 - */ - - switch (flight_stage) - { - case AP_SpdHgtControl::FLIGHT_TAKEOFF: - // while on the ground, an uncalibrated airspeed sensor can drift to 7m/s so - // ensure we aren't showing a false positive. If the throttle is suppressed - // we are definitely not flying, or at least for not much longer! - if (throttle_suppressed) { - is_flying_bool = false; - crash_state.is_crashed = false; - } - break; - - case AP_SpdHgtControl::FLIGHT_LAND_ABORT: - case AP_SpdHgtControl::FLIGHT_NORMAL: - // TODO: detect ground impacts - break; - - case AP_SpdHgtControl::FLIGHT_LAND_APPROACH: - // TODO: detect ground impacts - if (fabsf(auto_state.sink_rate) > 0.2f) { - is_flying_bool = true; - } - break; - - case AP_SpdHgtControl::FLIGHT_LAND_FINAL: - 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; - } - } - - // 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; - - if (should_log(MASK_LOG_MODE)) { - Log_Write_Status(); - } -} - -/* - return true if we think we are flying. This is a probabilistic - estimate, and needs to be used very carefully. Each use case needs - to be thought about individually. - */ -bool Plane::is_flying(void) -{ - if (hal.util->get_soft_armed()) { - // when armed, assume we're flying unless we probably aren't - return (isFlyingProbability >= 0.1f); - } - - // when disarmed, assume we're not flying unless we probably are - return (isFlyingProbability >= 0.9f); -} - -/* - * 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_LAND_ABORT: - 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")); - } - } - } -} - +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +#include "Plane.h" + +/* + is_flying and crash detection logic + */ + +/* + 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 = hal.scheduler->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 + */ + + switch (flight_stage) + { + case AP_SpdHgtControl::FLIGHT_TAKEOFF: + // while on the ground, an uncalibrated airspeed sensor can drift to 7m/s so + // ensure we aren't showing a false positive. If the throttle is suppressed + // we are definitely not flying, or at least for not much longer! + if (throttle_suppressed) { + is_flying_bool = false; + crash_state.is_crashed = false; + } + break; + + case AP_SpdHgtControl::FLIGHT_LAND_ABORT: + case AP_SpdHgtControl::FLIGHT_NORMAL: + // TODO: detect ground impacts + break; + + case AP_SpdHgtControl::FLIGHT_LAND_APPROACH: + // TODO: detect ground impacts + if (fabsf(auto_state.sink_rate) > 0.2f) { + is_flying_bool = true; + } + break; + + case AP_SpdHgtControl::FLIGHT_LAND_FINAL: + 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; + } + } + + // 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; + + if (should_log(MASK_LOG_MODE)) { + Log_Write_Status(); + } +} + +/* + return true if we think we are flying. This is a probabilistic + estimate, and needs to be used very carefully. Each use case needs + to be thought about individually. + */ +bool Plane::is_flying(void) +{ + if (hal.util->get_soft_armed()) { + // when armed, assume we're flying unless we probably aren't + return (isFlyingProbability >= 0.1f); + } + + // when disarmed, assume we're not flying unless we probably are + return (isFlyingProbability >= 0.9f); +} + +/* + * 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_LAND_ABORT: + 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")); + } + } + } +} +