5
0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-05 15:38:29 -04:00

AP_NavEKF: Enable external setting of yaw estimator bias states

This commit is contained in:
Paul Riseborough 2020-06-08 08:42:17 +10:00 committed by Peter Barker
parent 709a42cb7f
commit 5ad5498b07
2 changed files with 10 additions and 0 deletions
libraries/AP_NavEKF

View File

@ -641,3 +641,10 @@ bool EKFGSF_yaw::getYawData(float &yaw, float &yawVariance)
yawVariance = GSF.yaw_variance;
return true;
}
void EKFGSF_yaw::setGyroBias(Vector3f &gyroBias)
{
for (uint8_t mdl_idx = 0; mdl_idx < N_MODELS_EKFGSF; mdl_idx++) {
AHRS[mdl_idx].gyro_bias = gyroBias;
}
}

View File

@ -27,6 +27,9 @@ public:
void fuseVelData(const Vector2f &vel, // NE velocity measurement (m/s)
const float velAcc); // 1-sigma accuracy of velocity measurement (m/s)
// set the gyro bias in rad/sec
void setGyroBias(Vector3f &gyroBias);
// get solution data for logging
// return false if yaw estimation is inactive
bool getLogData(float &yaw_composite, float &yaw_composite_variance, float yaw[N_MODELS_EKFGSF], float innov_VN[N_MODELS_EKFGSF], float innov_VE[N_MODELS_EKFGSF], float weight[N_MODELS_EKFGSF]);