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

AP_NavEKF2: getDataEKFGSF and getYawData pass by reference

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

2
libraries/AP_NavEKF2/AP_NavEKF2.cpp
View File

@ -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) {

2
libraries/AP_NavEKF2/AP_NavEKF2.h
View File

@ -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

2
libraries/AP_NavEKF2/AP_NavEKF2_Logging.cpp
View File

@ -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-----",

6
libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp
View File

@ -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);

2
libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp
View File

@ -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);

2
libraries/AP_NavEKF2/AP_NavEKF2_core.h
View File

@ -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);