diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde index 96b62a695e..34c1f60e83 100644 --- a/ArduPlane/ArduPlane.pde +++ b/ArduPlane/ArduPlane.pde @@ -585,6 +585,10 @@ static bool throttle_suppressed; AP_SpdHgtControl::FlightStage flight_stage = AP_SpdHgtControl::FLIGHT_NORMAL; +// probability of aircraft is currently in flight. range from 0 to 1 where 1 is 100% sure we're in flight +static float isFlyingProbability = 0; +static void determine_is_flying(void); + //////////////////////////////////////////////////////////////////////////////// // Loiter management //////////////////////////////////////////////////////////////////////////////// @@ -1024,6 +1028,9 @@ static void one_second_loop() update_aux(); + // determine if we are flying or not + determine_is_flying(); + // update notify flags AP_Notify::flags.pre_arm_check = arming.pre_arm_checks(false); AP_Notify::flags.pre_arm_gps_check = true; @@ -1536,6 +1543,52 @@ static void update_flight_stage(void) airspeed.set_EAS2TAS(barometer.get_EAS2TAS()); } + + +/* + Do we think we are flying? + Probabilistic method where a bool is low-passed and considered a probability. +*/ +static void determine_is_flying(void) +{ + float aspeed; + bool isFlyingBool; + + bool airspeedMovement = ahrs.airspeed_estimate(&aspeed) && (aspeed >= 5); + + // If we don't have a GPS lock then don't use GPS for this test + bool gpsMovement = (gps.status() < AP_GPS::GPS_OK_FIX_2D || + gps.ground_speed() >= 5); + + + if(ahrs.get_armed()) { + // when armed, we need overwhelming evidence that we ARE NOT flying + isFlyingBool = airspeedMovement || gpsMovement; + + } else { + // when disarmed, we need overwhelming evidence that we ARE flying + isFlyingBool = airspeedMovement && gpsMovement; + } + + // low-pass the result. + isFlyingProbability = (0.8 * isFlyingProbability) + (0.2*(float)isFlyingBool); +} + +static bool is_flying(void) +{ + if(ahrs.get_armed()) { + // when armed, assume we're flying unless we probably aren't + return (isFlyingProbability >= 0.1); + } + else + { + // when disarmed, assume we're not flying unless we probably are + return (isFlyingProbability >= 0.9); + } + +} + + #if OPTFLOW == ENABLED // called at 50hz static void update_optical_flow(void) diff --git a/ArduPlane/Attitude.pde b/ArduPlane/Attitude.pde index 0b4a4b4826..ce3306b57d 100644 --- a/ArduPlane/Attitude.pde +++ b/ArduPlane/Attitude.pde @@ -545,29 +545,7 @@ static void flap_slew_limit(int8_t &last_value, int8_t &new_value) last_value = new_value; } - - -/** - Do we think we are flying? - This is a heuristic so it could be wrong in some cases. In particular, if we don't have GPS lock we'll fall - back to only using altitude. (This is probably more optimistic than what suppress_throttle wants...) -*/ -static bool is_flying(void) -{ - // If we don't have a GPS lock then don't use GPS for this test - bool gpsMovement = (gps.status() < AP_GPS::GPS_OK_FIX_2D || - gps.ground_speed() >= 5); - - bool airspeedMovement = (!ahrs.airspeed_sensor_enabled()) || airspeed.get_airspeed() >= 5; - - // we're more than 5m from the home altitude - bool inAir = relative_altitude_abs_cm() > 500; - - return inAir && gpsMovement && airspeedMovement; -} - - -/* We want to supress the throttle if we think we are on the ground and in an autopilot controlled throttle mode. +/* We want to suppress the throttle if we think we are on the ground and in an autopilot controlled throttle mode. Disable throttle if following conditions are met: * 1 - We are in Circle mode (which we use for short term failsafe), or in FBW-B or higher