diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp index e18b9fabd9..936ceccc7b 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.cpp +++ b/libraries/AP_NavEKF/AP_NavEKF.cpp @@ -2819,16 +2819,20 @@ void NavEKF::resetGyroBias(void) } -// return weighting of first IMU in blending function and the individual Z-accel bias estimates in m/s^2 -void NavEKF::getAccelBias(Vector3f &accelBias) const +// return weighting of first IMU in blending function +void NavEKF::getIMU1Weighting(float &ret) const { - accelBias.x = IMU1_weighting; + ret = IMU1_weighting; +} + +// return the individual Z-accel bias estimates in m/s^2 +void NavEKF::getAccelZBias(float &zbias1, float &zbias2) const { if (dtIMU == 0) { - accelBias.y = 0; - accelBias.z = 0; + zbias1 = 0; + zbias2 = 0; } else { - accelBias.y = state.accel_zbias2 / dtIMU; - accelBias.z = state.accel_zbias1 / dtIMU; + zbias1 = state.accel_zbias1 / dtIMU; + zbias2 = state.accel_zbias2 / dtIMU; } } diff --git a/libraries/AP_NavEKF/AP_NavEKF.h b/libraries/AP_NavEKF/AP_NavEKF.h index 222045b126..00d63af6e4 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.h +++ b/libraries/AP_NavEKF/AP_NavEKF.h @@ -108,8 +108,11 @@ public: // reset body axis gyro bias estimates void resetGyroBias(void); - // return weighting of first IMU in blending function and the individual Z-accel bias estimates in m/s^2 - void getAccelBias(Vector3f &accelBias) const; + // return weighting of first IMU in blending function + void getIMU1Weighting(float &ret) const; + + // return the individual Z-accel bias estimates in m/s^2 + void getAccelZBias(float &zbias1, float &zbias2) const; // return the NED wind speed estimates in m/s (positive is air moving in the direction of the axis) void getWind(Vector3f &wind) const;