|
|
@ -33,11 +33,20 @@ |
|
|
|
|
|
|
|
|
|
|
|
#define AP_AHRS_TRIM_LIMIT 10.0f // maximum trim angle in degrees
|
|
|
|
#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 |
|
|
|
class AP_AHRS |
|
|
|
{ |
|
|
|
{ |
|
|
|
public: |
|
|
|
public: |
|
|
|
// Constructor
|
|
|
|
// Constructor
|
|
|
|
AP_AHRS(AP_InertialSensor &ins, AP_Baro &baro, AP_GPS &gps) : |
|
|
|
AP_AHRS(AP_InertialSensor &ins, AP_Baro &baro, AP_GPS &gps) : |
|
|
|
|
|
|
|
_vehicle_class(AHRS_VEHICLE_UNKNOWN), |
|
|
|
_compass(NULL), |
|
|
|
_compass(NULL), |
|
|
|
_ins(ins), |
|
|
|
_ins(ins), |
|
|
|
_baro(baro), |
|
|
|
_baro(baro), |
|
|
@ -87,6 +96,14 @@ public: |
|
|
|
return _flags.fly_forward; |
|
|
|
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) { |
|
|
|
void set_wind_estimation(bool b) { |
|
|
|
_flags.wind_estimation = b; |
|
|
|
_flags.wind_estimation = b; |
|
|
|
} |
|
|
|
} |
|
|
@ -308,6 +325,8 @@ public: |
|
|
|
uint8_t get_active_accel_instance(void) const { return _active_accel_instance; } |
|
|
|
uint8_t get_active_accel_instance(void) const { return _active_accel_instance; } |
|
|
|
|
|
|
|
|
|
|
|
protected: |
|
|
|
protected: |
|
|
|
|
|
|
|
AHRS_VehicleClass _vehicle_class; |
|
|
|
|
|
|
|
|
|
|
|
// settable parameters
|
|
|
|
// settable parameters
|
|
|
|
AP_Float beta; |
|
|
|
AP_Float beta; |
|
|
|
AP_Int8 _gps_use; |
|
|
|
AP_Int8 _gps_use; |
|
|
|