From 770f697cfc455d139440f265799c0524e503784d Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 19 Jun 2017 13:16:22 +1000 Subject: [PATCH] AP_AHRS: added set/get for flying state this allows the vehicle code to set the likely flying state, which can be used by EKF to trigger changes which should only happen when flying (such as mag alignment) --- libraries/AP_AHRS/AP_AHRS.h | 36 ++++++++++++++++++++++++++++++++++++ 1 file changed, 36 insertions(+) diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h index ea4e96ff97..fe9e98738c 100644 --- a/libraries/AP_AHRS/AP_AHRS.h +++ b/libraries/AP_AHRS/AP_AHRS.h @@ -115,6 +115,38 @@ public: return _flags.fly_forward; } + /* + set the "likely flying" flag. This is not guaranteed to be + accurate, but is the vehicle codes best guess as to the whether + the vehicle is currently flying + */ + void set_likely_flying(bool b) { + if (b && !_flags.likely_flying) { + _last_flying_ms = AP_HAL::millis(); + } + _flags.likely_flying = b; + } + + /* + get the likely flying status. Returns true if the vehicle code + thinks we are flying at the moment. Not guaranteed to be + accurate + */ + bool get_likely_flying(void) const { + return _flags.likely_flying; + } + + /* + return time in milliseconds since likely_flying was set + true. Returns zero if likely_flying is currently false + */ + uint32_t get_time_flying_ms(void) const { + if (!_flags.likely_flying) { + return 0; + } + return AP_HAL::millis() - _last_flying_ms; + } + AHRS_VehicleClass get_vehicle_class(void) const { return _vehicle_class; } @@ -551,8 +583,12 @@ protected: uint8_t fly_forward : 1; // 1 if we can assume the aircraft will be flying forward on its X axis uint8_t correct_centrifugal : 1; // 1 if we should correct for centrifugal forces (allows arducopter to turn this off when motors are disarmed) uint8_t wind_estimation : 1; // 1 if we should do wind estimation + uint8_t likely_flying : 1; // 1 if vehicle is probably flying } _flags; + // time when likely_flying last went true + uint32_t _last_flying_ms; + // calculate sin/cos of roll/pitch/yaw from rotation void calc_trig(const Matrix3f &rot, float &cr, float &cp, float &cy,