From a9cbd5aa291d7b123c9cecb6bc8dcd69c7e78dce Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 23 Apr 2020 09:45:34 +0900 Subject: [PATCH] AP_NavEKF2: getDataEKFGSF and getYawData pass by reference --- libraries/AP_NavEKF2/AP_NavEKF2.cpp | 2 +- libraries/AP_NavEKF2/AP_NavEKF2.h | 2 +- libraries/AP_NavEKF2/AP_NavEKF2_Logging.cpp | 2 +- libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp | 6 +++--- libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp | 2 +- libraries/AP_NavEKF2/AP_NavEKF2_core.h | 2 +- 6 files changed, 8 insertions(+), 8 deletions(-) diff --git a/libraries/AP_NavEKF2/AP_NavEKF2.cpp b/libraries/AP_NavEKF2/AP_NavEKF2.cpp index de483c6812..e0a8257709 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2.cpp @@ -1693,7 +1693,7 @@ void NavEKF2::requestYawReset(void) // return data for debugging EKF-GSF yaw estimator // return false if data not available -bool NavEKF2::getDataEKFGSF(int8_t instance, float *yaw_composite, float *yaw_composite_variance, float yaw[N_MODELS_EKFGSF], float innov_VN[N_MODELS_EKFGSF], float innov_VE[N_MODELS_EKFGSF], float weight[N_MODELS_EKFGSF]) const +bool NavEKF2::getDataEKFGSF(int8_t instance, float &yaw_composite, float &yaw_composite_variance, float yaw[N_MODELS_EKFGSF], float innov_VN[N_MODELS_EKFGSF], float innov_VE[N_MODELS_EKFGSF], float weight[N_MODELS_EKFGSF]) const { if (instance < 0 || instance >= num_cores) instance = primary; if (core) { diff --git a/libraries/AP_NavEKF2/AP_NavEKF2.h b/libraries/AP_NavEKF2/AP_NavEKF2.h index 10f9b47f04..af3a12eef8 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2.h +++ b/libraries/AP_NavEKF2/AP_NavEKF2.h @@ -367,7 +367,7 @@ public: // log debug data for yaw estimator // return false if data not available - bool getDataEKFGSF(int8_t instance, float *yaw_composite, float *yaw_composite_variance, float yaw[N_MODELS_EKFGSF], float innov_VN[N_MODELS_EKFGSF], float innov_VE[N_MODELS_EKFGSF], float weight[N_MODELS_EKFGSF]) const; + bool getDataEKFGSF(int8_t instance, float &yaw_composite, float &yaw_composite_variance, float yaw[N_MODELS_EKFGSF], float innov_VN[N_MODELS_EKFGSF], float innov_VE[N_MODELS_EKFGSF], float weight[N_MODELS_EKFGSF]) const; private: uint8_t num_cores; // number of allocated cores diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Logging.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Logging.cpp index d398ede650..ba31e5bf98 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Logging.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_Logging.cpp @@ -280,7 +280,7 @@ void NavEKF2::Log_Write_GSF(uint8_t _core, uint64_t time_us) const float ive[N_MODELS_EKFGSF]; float wgt[N_MODELS_EKFGSF]; - if (getDataEKFGSF(_core, &yaw_composite, &yaw_composite_variance, yaw, ivn, ive, wgt)) { + if (getDataEKFGSF(_core, yaw_composite, yaw_composite_variance, yaw, ivn, ive, wgt)) { AP::logger().Write("GSF0", "TimeUS,C,YC,YCS,Y0,Y1,Y2,Y3,Y4,W0,W1,W2,W3,W4", "s#rrrrrrr-----", diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp index 5b4848a4a3..3fb4d47b90 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp @@ -840,7 +840,7 @@ void NavEKF2_core::fuseEulerYaw() } else { if (imuSampleTime_ms - prevBetaStep_ms > 1000 && yawEstimator != nullptr) { float gsfYaw, gsfYawVariance; - if (yawEstimator->getYawData(&gsfYaw, &gsfYawVariance) && + if (yawEstimator->getYawData(gsfYaw, gsfYawVariance) && is_positive(gsfYawVariance) && gsfYawVariance < sq(radians(15.0f))) { measured_yaw = gsfYaw; @@ -899,7 +899,7 @@ void NavEKF2_core::fuseEulerYaw() } else { if (imuSampleTime_ms - prevBetaStep_ms > 1000 && yawEstimator != nullptr) { float gsfYaw, gsfYawVariance; - if (yawEstimator->getYawData(&gsfYaw, &gsfYawVariance) && + if (yawEstimator->getYawData(gsfYaw, gsfYawVariance) && is_positive(gsfYawVariance) && gsfYawVariance < sq(radians(15.0f))) { measured_yaw = gsfYaw; @@ -1224,7 +1224,7 @@ bool NavEKF2_core::EKFGSF_resetMainFilterYaw() }; float yawEKFGSF, yawVarianceEKFGSF; - if (yawEstimator->getYawData(&yawEKFGSF, &yawVarianceEKFGSF) && is_positive(yawVarianceEKFGSF) && yawVarianceEKFGSF < sq(radians(15.0f))) { + if (yawEstimator->getYawData(yawEKFGSF, yawVarianceEKFGSF) && is_positive(yawVarianceEKFGSF) && yawVarianceEKFGSF < sq(radians(15.0f))) { // keep roll and pitch and reset yaw resetQuatStateYawOnly(yawEKFGSF, yawVarianceEKFGSF, false); diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp index 4b935bb35d..32dabcc2f7 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp @@ -633,7 +633,7 @@ bool NavEKF2_core::isExtNavUsedForYaw() return extNavUsedForYaw; } -bool NavEKF2_core::getDataEKFGSF(float *yaw_composite, float *yaw_composite_variance, float yaw[N_MODELS_EKFGSF], float innov_VN[N_MODELS_EKFGSF], float innov_VE[N_MODELS_EKFGSF], float weight[N_MODELS_EKFGSF]) +bool NavEKF2_core::getDataEKFGSF(float &yaw_composite, float &yaw_composite_variance, float yaw[N_MODELS_EKFGSF], float innov_VN[N_MODELS_EKFGSF], float innov_VE[N_MODELS_EKFGSF], float weight[N_MODELS_EKFGSF]) { if (yawEstimator != nullptr) { return yawEstimator->getLogData(yaw_composite, yaw_composite_variance, yaw, innov_VN, innov_VE, weight); diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.h b/libraries/AP_NavEKF2/AP_NavEKF2_core.h index 5f9eed5856..c0cf5d340e 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_core.h +++ b/libraries/AP_NavEKF2/AP_NavEKF2_core.h @@ -338,7 +338,7 @@ public: // get solution data for the EKF-GSF emergency yaw estimator // return false if data not available - bool getDataEKFGSF(float *yaw_composite, float *yaw_composite_variance, float yaw[N_MODELS_EKFGSF], float innov_VN[N_MODELS_EKFGSF], float innov_VE[N_MODELS_EKFGSF], float weight[N_MODELS_EKFGSF]); + bool getDataEKFGSF(float &yaw_composite, float &yaw_composite_variance, float yaw[N_MODELS_EKFGSF], float innov_VN[N_MODELS_EKFGSF], float innov_VE[N_MODELS_EKFGSF], float weight[N_MODELS_EKFGSF]); // Writes the default equivalent airspeed in m/s to be used in forward flight if a measured airspeed is required and not available. void writeDefaultAirSpeed(float airspeed);