Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-29 20:18:31 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF2: Update EKF2 data logging

Browse Source
This commit is contained in:
Paul Riseborough 2015-09-24 12:05:42 +10:00 committed by Andrew Tridgell
parent 86ad1e6e66
commit 80e182f827
5 changed files with 23 additions and 46 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
libraries/AP_NavEKF2/AP_NavEKF2.cpp
View File

@ -468,11 +468,11 @@ void NavEKF2::getGyroBias(Vector3f &gyroBias) const
}
}
// return body axis gyro scale factor estimates
void NavEKF2::getGyroScale(Vector3f &gyroScale) const
// return body axis gyro scale factor error as a percentage
void NavEKF2::getGyroScaleErrorPercentage(Vector3f &gyroScale) const
{
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
void NavEKF2::getAccelZBias(float &zbias1, float &zbias2) const
void NavEKF2::getAccelZBias(float &zbias) const
{
if (core) {
core->getAccelZBias(zbias1, zbias2);
core->getAccelZBias(zbias);
}
}

11
libraries/AP_NavEKF2/AP_NavEKF2.h
View File

@ -71,8 +71,8 @@ public:
// return body axis gyro bias estimates in rad/sec
void getGyroBias(Vector3f &gyroBias) const;
// return body axis gyro scale factor estimates
void getGyroScale(Vector3f &gyroScale) const;
// return body axis gyro scale factor error as a percentage
void getGyroScaleErrorPercentage(Vector3f &gyroScale) const;
// return tilt error convergence metric
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
void getEkfControlLimits(float &ekfGndSpdLimit, float &ekfNavVelGainScaler) 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 Z-accel bias estimate in m/s^2
void getAccelZBias(float &zbias) 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;

27
libraries/AP_NavEKF2/AP_NavEKF2_core.cpp
View File

@ -3272,16 +3272,16 @@ void NavEKF2_core::getGyroBias(Vector3f &gyroBias) const
gyroBias = stateStruct.gyro_bias / dtIMUavg;
}
// return body axis gyro scale factor estimates
void NavEKF2_core::getGyroScale(Vector3f &gyroScale) const
// return body axis gyro scale factor error as a percentage
void NavEKF2_core::getGyroScaleErrorPercentage(Vector3f &gyroScale) const
{
if (!statesInitialised) {
gyroScale.x = gyroScale.y = gyroScale.z = 0;
return;
}
gyroScale.x = 1.0f/stateStruct.gyro_scale.x - 1.0f;
gyroScale.y = 1.0f/stateStruct.gyro_scale.y - 1.0f;
gyroScale.z = 1.0f/stateStruct.gyro_scale.z - 1.0f;
gyroScale.x = 100.0f/stateStruct.gyro_scale.x - 100.0f;
gyroScale.y = 100.0f/stateStruct.gyro_scale.y - 100.0f;
gyroScale.z = 100.0f/stateStruct.gyro_scale.z - 100.0f;
}
// 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
// This filter always uses the primary IMU, so a weighting of 1 is returned
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 {
// return the Z-accel bias estimate in m/s^2
void NavEKF2_core::getAccelZBias(float &zbias) const {
if (dtIMUavg > 0) {
zbias1 = stateStruct.accel_zbias / dtIMUavg;
zbias2 = 0;
zbias = stateStruct.accel_zbias / dtIMUavg;
} else {
zbias1 = 0;
zbias2 = 0;
zbias = 0;
}
}

11
libraries/AP_NavEKF2/AP_NavEKF2_core.h
View File

@ -64,8 +64,8 @@ public:
// return body axis gyro bias estimates in rad/sec
void getGyroBias(Vector3f &gyroBias) const;
// return body axis gyro scale factor estimates
void getGyroScale(Vector3f &gyroScale) const;
// return body axis gyro scale factor error as a percentage
void getGyroScaleErrorPercentage(Vector3f &gyroScale) const;
// return tilt error convergence metric
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
void getEkfControlLimits(float &ekfGndSpdLimit, float &ekfNavVelGainScaler) 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 Z-accel bias estimate in m/s^2
void getAccelZBias(float &zbias) 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;

2
libraries/DataFlash/LogFile.cpp
View File

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