From 5ad5498b0728a5cdf3b770dc10aad1362b06f54c Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Mon, 8 Jun 2020 08:42:17 +1000 Subject: [PATCH] AP_NavEKF: Enable external setting of yaw estimator bias states --- libraries/AP_NavEKF/EKFGSF_yaw.cpp | 7 +++++++ libraries/AP_NavEKF/EKFGSF_yaw.h | 3 +++ 2 files changed, 10 insertions(+) diff --git a/libraries/AP_NavEKF/EKFGSF_yaw.cpp b/libraries/AP_NavEKF/EKFGSF_yaw.cpp index 24883cec8d..4c19eee9f1 100644 --- a/libraries/AP_NavEKF/EKFGSF_yaw.cpp +++ b/libraries/AP_NavEKF/EKFGSF_yaw.cpp @@ -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; + } +} \ No newline at end of file diff --git a/libraries/AP_NavEKF/EKFGSF_yaw.h b/libraries/AP_NavEKF/EKFGSF_yaw.h index f57de2b5cc..a141b3adf8 100644 --- a/libraries/AP_NavEKF/EKFGSF_yaw.h +++ b/libraries/AP_NavEKF/EKFGSF_yaw.h @@ -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]);