zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AP_NavEKF2: Update EKF2 data logging

mission-4.1.18
Paul Riseborough 10 years ago committed by Andrew Tridgell
parent
86ad1e6e66
commit
80e182f827
5 changed files with 23 additions and 46 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 18
      libraries/AP_NavEKF2/AP_NavEKF2.cpp
  2. 11
      libraries/AP_NavEKF2/AP_NavEKF2.h
  3. 27
      libraries/AP_NavEKF2/AP_NavEKF2_core.cpp
  4. 11
      libraries/AP_NavEKF2/AP_NavEKF2_core.h
  5. 2
      libraries/DataFlash/LogFile.cpp

18
libraries/AP_NavEKF2/AP_NavEKF2.cpp
Unescape View File

@ -468,11 +468,11 @@ void NavEKF2::getGyroBias(Vector3f &gyroBias) const
} }
} }
// return body axis gyro scale factor estimates // return body axis gyro scale factor error as a percentage
void NavEKF2::getGyroScale(Vector3f &gyroScale) const void NavEKF2::getGyroScaleErrorPercentage(Vector3f &gyroScale) const
{ {
if (core) { if (core) {
core->getGyroScale(gyroScale); core->getGyroScaleErrorPercentage(gyroScale);
} }
} }
@ -528,19 +528,11 @@ void NavEKF2::getEkfControlLimits(float &ekfGndSpdLimit, float &ekfNavVelGainSca
} }
} }
// return weighting of first IMU in blending function
void NavEKF2::getIMU1Weighting(float &ret) const
{
if (core) {
core->getIMU1Weighting(ret);
}
}
// return the individual Z-accel bias estimates in m/s^2 // return the individual Z-accel bias estimates in m/s^2
void NavEKF2::getAccelZBias(float &zbias1, float &zbias2) const void NavEKF2::getAccelZBias(float &zbias) const
{ {
if (core) { if (core) {
core->getAccelZBias(zbias1, zbias2); core->getAccelZBias(zbias);
} }
} }

11
libraries/AP_NavEKF2/AP_NavEKF2.h
Unescape View File

@ -71,8 +71,8 @@ public:
// return body axis gyro bias estimates in rad/sec // return body axis gyro bias estimates in rad/sec
void getGyroBias(Vector3f &gyroBias) const; void getGyroBias(Vector3f &gyroBias) const;
// return body axis gyro scale factor estimates // return body axis gyro scale factor error as a percentage
void getGyroScale(Vector3f &gyroScale) const; void getGyroScaleErrorPercentage(Vector3f &gyroScale) const;
// return tilt error convergence metric // return tilt error convergence metric
void getTiltError(float &ang) const; void getTiltError(float &ang) const;
@ -99,11 +99,8 @@ public:
// return the scale factor to be applied to navigation velocity gains to compensate for increase in velocity noise with height when using optical flow // return the scale factor to be applied to navigation velocity gains to compensate for increase in velocity noise with height when using optical flow
void getEkfControlLimits(float &ekfGndSpdLimit, float &ekfNavVelGainScaler) const; void getEkfControlLimits(float &ekfGndSpdLimit, float &ekfNavVelGainScaler) const;
// return weighting of first IMU in blending function // return the Z-accel bias estimate in m/s^2
void getIMU1Weighting(float &ret) const; void getAccelZBias(float &zbias) 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;

27
libraries/AP_NavEKF2/AP_NavEKF2_core.cpp
Unescape View File

@ -3272,16 +3272,16 @@ void NavEKF2_core::getGyroBias(Vector3f &gyroBias) const
gyroBias = stateStruct.gyro_bias / dtIMUavg; gyroBias = stateStruct.gyro_bias / dtIMUavg;
} }
// return body axis gyro scale factor estimates // return body axis gyro scale factor error as a percentage
void NavEKF2_core::getGyroScale(Vector3f &gyroScale) const void NavEKF2_core::getGyroScaleErrorPercentage(Vector3f &gyroScale) const
{ {
if (!statesInitialised) { if (!statesInitialised) {
gyroScale.x = gyroScale.y = gyroScale.z = 0; gyroScale.x = gyroScale.y = gyroScale.z = 0;
return; return;
} }
gyroScale.x = 1.0f/stateStruct.gyro_scale.x - 1.0f; gyroScale.x = 100.0f/stateStruct.gyro_scale.x - 100.0f;
gyroScale.y = 1.0f/stateStruct.gyro_scale.y - 1.0f; gyroScale.y = 100.0f/stateStruct.gyro_scale.y - 100.0f;
gyroScale.z = 1.0f/stateStruct.gyro_scale.z - 1.0f; gyroScale.z = 100.0f/stateStruct.gyro_scale.z - 100.0f;
} }
// return tilt error convergence metric // return tilt error convergence metric
@ -3360,21 +3360,12 @@ void NavEKF2_core::getEkfControlLimits(float &ekfGndSpdLimit, float &ekfNavVelGa
} }
} }
// return weighting of first IMU in blending function // return the Z-accel bias estimate in m/s^2
// This filter always uses the primary IMU, so a weighting of 1 is returned void NavEKF2_core::getAccelZBias(float &zbias) const {
void NavEKF2_core::getIMU1Weighting(float &ret) const
{
ret = 1.0f;
}
// return the individual Z-accel bias estimates in m/s^2
void NavEKF2_core::getAccelZBias(float &zbias1, float &zbias2) const {
if (dtIMUavg > 0) { if (dtIMUavg > 0) {
zbias1 = stateStruct.accel_zbias / dtIMUavg; zbias = stateStruct.accel_zbias / dtIMUavg;
zbias2 = 0;
} else { } else {
zbias1 = 0; zbias = 0;
zbias2 = 0;
} }
} }

11
libraries/AP_NavEKF2/AP_NavEKF2_core.h
Unescape View File

@ -64,8 +64,8 @@ public:
// return body axis gyro bias estimates in rad/sec // return body axis gyro bias estimates in rad/sec
void getGyroBias(Vector3f &gyroBias) const; void getGyroBias(Vector3f &gyroBias) const;
// return body axis gyro scale factor estimates // return body axis gyro scale factor error as a percentage
void getGyroScale(Vector3f &gyroScale) const; void getGyroScaleErrorPercentage(Vector3f &gyroScale) const;
// return tilt error convergence metric // return tilt error convergence metric
void getTiltError(float &ang) const; void getTiltError(float &ang) const;
@ -92,11 +92,8 @@ public:
// return the scale factor to be applied to navigation velocity gains to compensate for increase in velocity noise with height when using optical flow // return the scale factor to be applied to navigation velocity gains to compensate for increase in velocity noise with height when using optical flow
void getEkfControlLimits(float &ekfGndSpdLimit, float &ekfNavVelGainScaler) const; void getEkfControlLimits(float &ekfGndSpdLimit, float &ekfNavVelGainScaler) const;
// return weighting of first IMU in blending function // return the Z-accel bias estimate in m/s^2
void getIMU1Weighting(float &ret) const; void getAccelZBias(float &zbias) 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;

2
libraries/DataFlash/LogFile.cpp
Unescape View File

@ -1361,7 +1361,7 @@ void DataFlash_Class::Log_Write_EKF2(AP_AHRS_NavEKF &ahrs, bool optFlowEnabled)
innovMX : (int16_t)(magInnov.x), innovMX : (int16_t)(magInnov.x),
innovMY : (int16_t)(magInnov.y), innovMY : (int16_t)(magInnov.y),
innovMZ : (int16_t)(magInnov.z), innovMZ : (int16_t)(magInnov.z),
innovYaw : (int16_t)(100*degrees(tasInnov)), innovYaw : (int16_t)(100*degrees(yawInnov)),
innovVT : (int16_t)(100*tasInnov) innovVT : (int16_t)(100*tasInnov)
}; };
WriteBlock(&pkt3, sizeof(pkt3)); WriteBlock(&pkt3, sizeof(pkt3));

Write Preview
Loading…
Cancel
Save