From c8c6e05a4a38d4accabce0c7264364870af6e243 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 21 Apr 2014 17:10:27 +1000 Subject: [PATCH] AP_AHRS: added vehicle class to AHRS used by EKF to control use of get_fly_forward() --- libraries/AP_AHRS/AP_AHRS.h | 19 +++++++++++++++++++ libraries/AP_AHRS/AP_AHRS_DCM.h | 2 +- libraries/AP_AHRS/AP_AHRS_NavEKF.h | 2 +- 3 files changed, 21 insertions(+), 2 deletions(-) diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h index e1d3df518e..2a1f8ecf5d 100644 --- a/libraries/AP_AHRS/AP_AHRS.h +++ b/libraries/AP_AHRS/AP_AHRS.h @@ -33,11 +33,20 @@ #define AP_AHRS_TRIM_LIMIT 10.0f // maximum trim angle in degrees +enum AHRS_VehicleClass { + AHRS_VEHICLE_UNKNOWN, + AHRS_VEHICLE_GROUND, + AHRS_VEHICLE_COPTER, + AHRS_VEHICLE_FIXED_WING, +}; + + class AP_AHRS { public: // Constructor AP_AHRS(AP_InertialSensor &ins, AP_Baro &baro, AP_GPS &gps) : + _vehicle_class(AHRS_VEHICLE_UNKNOWN), _compass(NULL), _ins(ins), _baro(baro), @@ -87,6 +96,14 @@ public: return _flags.fly_forward; } + AHRS_VehicleClass get_vehicle_class(void) const { + return _vehicle_class; + } + + void set_vehicle_class(AHRS_VehicleClass vclass) { + _vehicle_class = vclass; + } + void set_wind_estimation(bool b) { _flags.wind_estimation = b; } @@ -308,6 +325,8 @@ public: uint8_t get_active_accel_instance(void) const { return _active_accel_instance; } protected: + AHRS_VehicleClass _vehicle_class; + // settable parameters AP_Float beta; AP_Int8 _gps_use; diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.h b/libraries/AP_AHRS/AP_AHRS_DCM.h index 0ca2da70f6..e8709f7997 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.h +++ b/libraries/AP_AHRS/AP_AHRS_DCM.h @@ -26,7 +26,7 @@ class AP_AHRS_DCM : public AP_AHRS public: // Constructors AP_AHRS_DCM(AP_InertialSensor &ins, AP_Baro &baro, AP_GPS &gps) : - AP_AHRS(ins, baro, gps), + AP_AHRS(ins, baro, gps), _last_declination(0), _mag_earth(1,0) { diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.h b/libraries/AP_AHRS/AP_AHRS_NavEKF.h index 0829878e3a..20775fdc81 100644 --- a/libraries/AP_AHRS/AP_AHRS_NavEKF.h +++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.h @@ -34,7 +34,7 @@ class AP_AHRS_NavEKF : public AP_AHRS_DCM public: // Constructor AP_AHRS_NavEKF(AP_InertialSensor &ins, AP_Baro &baro, AP_GPS &gps) : - AP_AHRS_DCM(ins, baro, gps), + AP_AHRS_DCM(ins, baro, gps), EKF(this, baro), ekf_started(false), startup_delay_ms(10000)