From c1ef894a30e2de5b0f1ba3b6b917ea6f1be7da2d Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 23 Apr 2020 09:46:12 +0900 Subject: [PATCH] AP_NavEKF3: getDataEKFGSF and getYawData pass by reference --- libraries/AP_NavEKF3/AP_NavEKF3.cpp | 2 +- libraries/AP_NavEKF3/AP_NavEKF3.h | 2 +- libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp | 2 +- libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp | 6 +++--- libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp | 2 +- libraries/AP_NavEKF3/AP_NavEKF3_core.h | 2 +- 6 files changed, 8 insertions(+), 8 deletions(-) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.cpp b/libraries/AP_NavEKF3/AP_NavEKF3.cpp index 5414cc9dcd..686ac103b4 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3.cpp @@ -1378,7 +1378,7 @@ uint32_t NavEKF3::getBodyFrameOdomDebug(int8_t instance, Vector3f &velInnov, Vec // return data for debugging EKF-GSF yaw estimator // return false if data not available -bool NavEKF3::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 NavEKF3::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_NavEKF3/AP_NavEKF3.h b/libraries/AP_NavEKF3/AP_NavEKF3.h index bc96bccf67..f06bd1cbd5 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3.h @@ -398,7 +398,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_NavEKF3/AP_NavEKF3_Logging.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp index 50f482db61..fa5aed1452 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp @@ -350,7 +350,7 @@ void NavEKF3::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_NavEKF3/AP_NavEKF3_MagFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp index a3dfd56ed4..0f5b5721c4 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp @@ -291,7 +291,7 @@ void NavEKF3_core::SelectMagFusion() } float yawEKFGSF, yawVarianceEKFGSF; - bool canUseEKFGSF = yawEstimator->getYawData(&yawEKFGSF, &yawVarianceEKFGSF) && is_positive(yawVarianceEKFGSF) && yawVarianceEKFGSF < sq(radians(15.0f)); + bool canUseEKFGSF = yawEstimator->getYawData(yawEKFGSF, yawVarianceEKFGSF) && is_positive(yawVarianceEKFGSF) && yawVarianceEKFGSF < sq(radians(15.0f)); if (yawAlignComplete && canUseEKFGSF) { // use the EKF-GSF yaw estimator output as this is more robust than the EKF can achieve without a yaw measurement yawAngDataDelayed.yawAngErr = MAX(sqrtf(yawVarianceEKFGSF), 0.05f); @@ -949,7 +949,7 @@ void NavEKF3_core::fuseEulerYaw(bool usePredictedYaw, bool useExternalYawSensor) float gsfYaw = 0.0f; float gsfYawVariance = 0.0f; if (usePredictedYaw && yawEstimator != nullptr) { - canUseGsfYaw = yawEstimator->getYawData(&gsfYaw, &gsfYawVariance) + canUseGsfYaw = yawEstimator->getYawData(gsfYaw, gsfYawVariance) && is_positive(gsfYawVariance) && gsfYawVariance < sq(radians(15.0f)); } @@ -1501,7 +1501,7 @@ bool NavEKF3_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_NavEKF3/AP_NavEKF3_Outputs.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp index 990db7b823..f907aefdf6 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp @@ -637,7 +637,7 @@ void NavEKF3_core::getOutputTrackingError(Vector3f &error) const error = outputTrackError; } -bool NavEKF3_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 NavEKF3_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_NavEKF3/AP_NavEKF3_core.h b/libraries/AP_NavEKF3/AP_NavEKF3_core.h index 37157041ab..cca6d9bdc8 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.h @@ -397,7 +397,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);