mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 14:38:30 -04:00
AP_AHRS: added vehicle class to AHRS
used by EKF to control use of get_fly_forward()
This commit is contained in:
parent
bd28cdbdcf
commit
c8c6e05a4a
@ -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;
|
||||||
|
@ -26,7 +26,7 @@ class AP_AHRS_DCM : public AP_AHRS
|
|||||||
public:
|
public:
|
||||||
// Constructors
|
// Constructors
|
||||||
AP_AHRS_DCM(AP_InertialSensor &ins, AP_Baro &baro, AP_GPS &gps) :
|
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),
|
_last_declination(0),
|
||||||
_mag_earth(1,0)
|
_mag_earth(1,0)
|
||||||
{
|
{
|
||||||
|
@ -34,7 +34,7 @@ class AP_AHRS_NavEKF : public AP_AHRS_DCM
|
|||||||
public:
|
public:
|
||||||
// Constructor
|
// Constructor
|
||||||
AP_AHRS_NavEKF(AP_InertialSensor &ins, AP_Baro &baro, AP_GPS &gps) :
|
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(this, baro),
|
||||||
ekf_started(false),
|
ekf_started(false),
|
||||||
startup_delay_ms(10000)
|
startup_delay_ms(10000)
|
||||||
|
Loading…
Reference in New Issue
Block a user