AHRS: added ahrs.yaw_initialised()

this allows the high level code to know if the AHRS driver has a
reliable yaw estimate
This commit is contained in:
Andrew Tridgell 2012-08-15 11:16:21 +10:00
parent 533aab4f66
commit 9b4b7997b6
4 changed files with 7 additions and 4 deletions

View File

@ -107,7 +107,14 @@ public:
return Vector3f(0,0,0);
}
// return true if yaw has been initialised
bool yaw_initialised(void) {
return _have_initial_yaw;
}
protected:
// whether the yaw value has been intialised with a reference
bool _have_initial_yaw;
// pointer to compass object, if available
Compass * _compass;

View File

@ -64,7 +64,6 @@ public:
private:
float _ki;
float _ki_yaw;
bool _have_initial_yaw;
// Methods
void matrix_update(float _G_Dt);

View File

@ -67,7 +67,6 @@ public:
private:
float _ki;
float _ki_yaw;
bool _have_initial_yaw;
AP_InertialSensor_MPU6000 *_mpu6000;
// Methods

View File

@ -58,8 +58,6 @@ private:
void update_IMU(float deltat, Vector3f &gyro, Vector3f &accel);
void update_MARG(float deltat, Vector3f &gyro, Vector3f &accel, Vector3f &mag);
bool _have_initial_yaw;
// Methods
void accel_adjust(void);