Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-25 09:13:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF: constify EKFGSF_yaw members

Browse Source
This commit is contained in:
Randy Mackay 2020-12-15 16:29:31 +09:00 committed by Andrew Tridgell
parent 0c3fcfd9d6
commit 754002525e
2 changed files with 9 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
libraries/AP_NavEKF/EKFGSF_yaw.cpp
View File

@ -577,7 +577,7 @@ float EKFGSF_yaw::gaussianDensity(const uint8_t mdl_idx) const
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]) const
{
if (vel_fuse_running) {
yaw_composite = GSF.yaw;
@ -604,7 +604,7 @@ void EKFGSF_yaw::forceSymmetry(const uint8_t mdl_idx)
}
// Apply a body frame delta angle to the body to earth frame rotation matrix using a small angle approximation
Matrix3f EKFGSF_yaw::updateRotMat(const Matrix3f &R, const Vector3f &g)
Matrix3f EKFGSF_yaw::updateRotMat(const Matrix3f &R, const Vector3f &g) const
{
Matrix3f ret = R;
ret[0][0] += R[0][1] * g[2] - R[0][2] * g[1];
@ -632,7 +632,7 @@ Matrix3f EKFGSF_yaw::updateRotMat(const Matrix3f &R, const Vector3f &g)
return ret;
}
bool EKFGSF_yaw::getYawData(float &yaw, float &yawVariance)
bool EKFGSF_yaw::getYawData(float &yaw, float &yawVariance) const
{
if (!vel_fuse_running) {
return false;
@ -642,7 +642,7 @@ bool EKFGSF_yaw::getYawData(float &yaw, float &yawVariance)
return true;
}
bool EKFGSF_yaw::getVelInnovLength(float &velInnovLength)
bool EKFGSF_yaw::getVelInnovLength(float &velInnovLength) const
{
if (!vel_fuse_running) {
return false;
@ -659,4 +659,4 @@ void EKFGSF_yaw::setGyroBias(Vector3f &gyroBias)
for (uint8_t mdl_idx = 0; mdl_idx < N_MODELS_EKFGSF; mdl_idx++) {
AHRS[mdl_idx].gyro_bias = gyroBias;
}
}
}

8
libraries/AP_NavEKF/EKFGSF_yaw.h
View File

@ -32,15 +32,15 @@ public:
// get solution data for logging
// 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]) const;
// get yaw estimated and corresponding variance
// return false if yaw estimation is inactive
bool getYawData(float &yaw, float &yawVariance);
bool getYawData(float &yaw, float &yawVariance) const;
// get the length of the weighted average velocity innovation vector
// return false if not available
bool getVelInnovLength(float &velInnovLength);
bool getVelInnovLength(float &velInnovLength) const;
private:
@ -90,7 +90,7 @@ private:
void predictAHRS(const uint8_t mdl_idx);
// Applies a body frame delta angle to a body to earth frame rotation matrix using a small angle approximation
Matrix3f updateRotMat(const Matrix3f &R, const Vector3f &g);
Matrix3f updateRotMat(const Matrix3f &R, const Vector3f &g) const;
// Initialises the tilt (roll and pitch) for all AHRS using IMU acceleration data
void alignTilt();