Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-09 17:38:32 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF: GSF getLogData and getYawData pass by reference

Browse Source
This commit is contained in:
Randy Mackay 2020-04-23 09:44:03 +09:00 committed by Andrew Tridgell
parent 8a8271c2c7
commit 1ee9f3a6de
2 changed files with 8 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
libraries/AP_NavEKF/EKFGSF_yaw.cpp
View File

@ -592,11 +592,11 @@ float EKFGSF_yaw::gaussianDensity(const uint8_t mdl_idx) const
return normDist; return normDist;
} }
bool EKFGSF_yaw::getLogData(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 EKFGSF_yaw::getLogData(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 (vel_fuse_running) { if (vel_fuse_running) {
*yaw_composite = GSF.yaw; yaw_composite = GSF.yaw;
*yaw_composite_variance = GSF.yaw_variance; yaw_composite_variance = GSF.yaw_variance;
for (uint8_t mdl_idx = 0; mdl_idx < N_MODELS_EKFGSF; mdl_idx++) { for (uint8_t mdl_idx = 0; mdl_idx < N_MODELS_EKFGSF; mdl_idx++) {
yaw[mdl_idx] = EKF[mdl_idx].X[2]; yaw[mdl_idx] = EKF[mdl_idx].X[2];
innov_VN[mdl_idx] = EKF[mdl_idx].innov[0]; innov_VN[mdl_idx] = EKF[mdl_idx].innov[0];
@ -647,12 +647,12 @@ Matrix3f EKFGSF_yaw::updateRotMat(const Matrix3f &R, const Vector3f &g)
return ret; return ret;
} }
bool EKFGSF_yaw::getYawData(float *yaw, float *yawVariance) bool EKFGSF_yaw::getYawData(float &yaw, float &yawVariance)
{ {
if (!vel_fuse_running) { if (!vel_fuse_running) {
return false; return false;
} }
*yaw = GSF.yaw; yaw = GSF.yaw;
*yawVariance = GSF.yaw_variance; yawVariance = GSF.yaw_variance;
return true; return true;
} }

4
libraries/AP_NavEKF/EKFGSF_yaw.h
View File

@ -27,11 +27,11 @@ public:
// get solution data for logging // get solution data for logging
// return false if yaw estimation is inactive // return false if yaw estimation is inactive
bool getLogData(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 getLogData(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]);
// get yaw estimated and corresponding variance // get yaw estimated and corresponding variance
// return false if yaw estimation is inactive // return false if yaw estimation is inactive
bool getYawData(float *yaw, float *yawVariance); bool getYawData(float &yaw, float &yawVariance);
private: private: