From 80e182f8277726fc689865712bfe053e2cf868b9 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Thu, 24 Sep 2015 12:05:42 +1000 Subject: [PATCH] AP_NavEKF2: Update EKF2 data logging --- libraries/AP_NavEKF2/AP_NavEKF2.cpp | 18 +++++----------- libraries/AP_NavEKF2/AP_NavEKF2.h | 11 ++++------ libraries/AP_NavEKF2/AP_NavEKF2_core.cpp | 27 ++++++++---------------- libraries/AP_NavEKF2/AP_NavEKF2_core.h | 11 ++++------ libraries/DataFlash/LogFile.cpp | 2 +- 5 files changed, 23 insertions(+), 46 deletions(-) diff --git a/libraries/AP_NavEKF2/AP_NavEKF2.cpp b/libraries/AP_NavEKF2/AP_NavEKF2.cpp index 56ef0c9665..36b806252d 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2.cpp @@ -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); } } diff --git a/libraries/AP_NavEKF2/AP_NavEKF2.h b/libraries/AP_NavEKF2/AP_NavEKF2.h index af9829e93e..c517187678 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2.h +++ b/libraries/AP_NavEKF2/AP_NavEKF2.h @@ -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; diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp index 7e91151a8f..43e03dca0c 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp @@ -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; } } diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.h b/libraries/AP_NavEKF2/AP_NavEKF2_core.h index 6802112eb3..ece0d899d2 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_core.h +++ b/libraries/AP_NavEKF2/AP_NavEKF2_core.h @@ -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; diff --git a/libraries/DataFlash/LogFile.cpp b/libraries/DataFlash/LogFile.cpp index eb4d101ad3..ce0268a37f 100644 --- a/libraries/DataFlash/LogFile.cpp +++ b/libraries/DataFlash/LogFile.cpp @@ -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));