Plane: isFlying redesign

The is_flying() function is too simplistic to be used as a generic
status. Added a sticky heuristic method that depends on the arm
state. More thought should go into the boolean result of isFlying but
this is at least safer than the original method.
This commit is contained in:
Tom Pittenger 2015-02-17 00:21:42 -08:00 committed by Andrew Tridgell
parent 0b261e8695
commit 58b0702793
2 changed files with 54 additions and 23 deletions

View File

@ -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)

View File

@ -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