Browse Source

AP_NavEKF: Split getAccelBias into getAccelZBias and getIMU1Weighting

mission-4.1.18
Jonathan Challinger 10 years ago committed by Andrew Tridgell
parent
commit
967986d5c6
  1. 18
      libraries/AP_NavEKF/AP_NavEKF.cpp
  2. 7
      libraries/AP_NavEKF/AP_NavEKF.h

18
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 // return weighting of first IMU in blending function
void NavEKF::getAccelBias(Vector3f &accelBias) const 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) { if (dtIMU == 0) {
accelBias.y = 0; zbias1 = 0;
accelBias.z = 0; zbias2 = 0;
} else { } else {
accelBias.y = state.accel_zbias2 / dtIMU; zbias1 = state.accel_zbias1 / dtIMU;
accelBias.z = state.accel_zbias1 / dtIMU; zbias2 = state.accel_zbias2 / dtIMU;
} }
} }

7
libraries/AP_NavEKF/AP_NavEKF.h

@ -108,8 +108,11 @@ public:
// reset body axis gyro bias estimates // reset body axis gyro bias estimates
void resetGyroBias(void); void resetGyroBias(void);
// return weighting of first IMU in blending function and the individual Z-accel bias estimates in m/s^2 // return weighting of first IMU in blending function
void getAccelBias(Vector3f &accelBias) const; 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) // return the NED wind speed estimates in m/s (positive is air moving in the direction of the axis)
void getWind(Vector3f &wind) const; void getWind(Vector3f &wind) const;

Loading…
Cancel
Save