From a43beaaa23f9c2d47b84b488d53a958516166d22 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 23 Apr 2020 13:11:03 +0900 Subject: [PATCH] AP_NavEKF: GSF white space fixes --- libraries/AP_NavEKF/EKFGSF_yaw.cpp | 24 ++++++++++++------------ libraries/AP_NavEKF/EKFGSF_yaw.h | 30 +++++++++++++++--------------- 2 files changed, 27 insertions(+), 27 deletions(-) diff --git a/libraries/AP_NavEKF/EKFGSF_yaw.cpp b/libraries/AP_NavEKF/EKFGSF_yaw.cpp index 7d33a00fd7..8a2cf760be 100644 --- a/libraries/AP_NavEKF/EKFGSF_yaw.cpp +++ b/libraries/AP_NavEKF/EKFGSF_yaw.cpp @@ -159,16 +159,16 @@ void EKFGSF_yaw::update(const Vector3f &delAng, /* memset(&GSF.P, 0, sizeof(GSF.P)); for (uint8_t mdl_idx = 0; mdl_idx < N_MODELS_EKFGSF; mdl_idx ++) { - float delta[3]; - for (uint8_t row = 0; row < 3; row++) { - delta[row] = EKF[mdl_idx].X[row] - GSF.X[row]; - } - for (uint8_t row = 0; row < 3; row++) { - for (uint8_t col = 0; col < 3; col++) { - GSF.P[row][col] += GSF.weights[mdl_idx] * (EKF[mdl_idx].P[row][col] + delta[row] * delta[col]); - } - } - } + float delta[3]; + for (uint8_t row = 0; row < 3; row++) { + delta[row] = EKF[mdl_idx].X[row] - GSF.X[row]; + } + for (uint8_t row = 0; row < 3; row++) { + for (uint8_t col = 0; col < 3; col++) { + GSF.P[row][col] += GSF.weights[mdl_idx] * (EKF[mdl_idx].P[row][col] + delta[row] * delta[col]); + } + } + } */ GSF.yaw_variance = 0.0f; @@ -628,7 +628,7 @@ Matrix3f EKFGSF_yaw::updateRotMat(const Matrix3f &R, const Vector3f &g) ret[1][0] += R[1][1] * g[2] - R[1][2] * g[1]; ret[1][1] += R[1][2] * g[0] - R[1][0] * g[2]; ret[1][2] += R[1][0] * g[1] - R[1][1] * g[0]; - ret[2][0] += R[2][1] * g[2] - R[2][2] * g[1]; + ret[2][0] += R[2][1] * g[2] - R[2][2] * g[1]; ret[2][1] += R[2][2] * g[0] - R[2][0] * g[2]; ret[2][2] += R[2][0] * g[1] - R[2][1] * g[0]; @@ -655,4 +655,4 @@ bool EKFGSF_yaw::getYawData(float &yaw, float &yawVariance) yaw = GSF.yaw; yawVariance = GSF.yaw_variance; return true; -} \ No newline at end of file +} diff --git a/libraries/AP_NavEKF/EKFGSF_yaw.h b/libraries/AP_NavEKF/EKFGSF_yaw.h index 84ed6bc9c0..d74e7ca403 100644 --- a/libraries/AP_NavEKF/EKFGSF_yaw.h +++ b/libraries/AP_NavEKF/EKFGSF_yaw.h @@ -48,10 +48,10 @@ private: #endif // Parameters - const float EKFGSF_gyroNoise{1.0e-1f}; // yaw rate noise used for covariance prediction (rad/sec) - const float EKFGSF_accelNoise{2.0f}; // horizontal accel noise used for covariance prediction (m/sec**2) - const float EKFGSF_tiltGain{0.2f}; // gain from tilt error to gyro correction for complementary filter (1/sec) - const float EKFGSF_gyroBiasGain{0.04f}; // gain applied to integral of gyro correction for complementary filter (1/sec) + const float EKFGSF_gyroNoise{1.0e-1f}; // yaw rate noise used for covariance prediction (rad/sec) + const float EKFGSF_accelNoise{2.0f}; // horizontal accel noise used for covariance prediction (m/sec**2) + const float EKFGSF_tiltGain{0.2f}; // gain from tilt error to gyro correction for complementary filter (1/sec) + const float EKFGSF_gyroBiasGain{0.04f}; // gain applied to integral of gyro correction for complementary filter (1/sec) const float EKFGSF_accelFiltRatio{10.0f}; // ratio of time constant of AHRS tilt correction to time constant of first order LPF applied to accel data used by ahrs // Declarations used by the bank of AHRS complementary filters that use IMU data augmented by true @@ -62,20 +62,20 @@ private: float angle_dt; float velocity_dt; struct ahrs_struct { - Matrix3f R; // matrix that rotates a vector from body to earth frame - Vector3f gyro_bias; // gyro bias learned and used by the quaternion calculation - bool aligned{false}; // true when AHRS has been aligned - float accel_FR[2]; // front-right acceleration vector in a horizontal plane (m/s/s) - float vel_NE[2]; // NE velocity vector from last GPS measurement (m/s) - bool fuse_gps; // true when GPS should be fused on that frame - float accel_dt; // time step used when generating _simple_accel_FR data (sec) + Matrix3f R; // matrix that rotates a vector from body to earth frame + Vector3f gyro_bias; // gyro bias learned and used by the quaternion calculation + bool aligned{false}; // true when AHRS has been aligned + float accel_FR[2]; // front-right acceleration vector in a horizontal plane (m/s/s) + float vel_NE[2]; // NE velocity vector from last GPS measurement (m/s) + bool fuse_gps; // true when GPS should be fused on that frame + float accel_dt; // time step used when generating _simple_accel_FR data (sec) }; ahrs_struct AHRS[N_MODELS_EKFGSF]; bool ahrs_tilt_aligned; // true the initial tilt alignment has been calculated - float accel_gain; // gain from accel vector tilt error to rate gyro correction used by AHRS calculation - Vector3f ahrs_accel; // filtered body frame specific force vector used by AHRS calculation (m/s/s) - float ahrs_accel_norm; // length of body frame specific force vector used by AHRS calculation (m/s/s) - bool ahrs_turn_comp_enabled; // true when compensation for centripetal acceleration in coordinated turns using true airspeed is being used. + float accel_gain; // gain from accel vector tilt error to rate gyro correction used by AHRS calculation + Vector3f ahrs_accel; // filtered body frame specific force vector used by AHRS calculation (m/s/s) + float ahrs_accel_norm; // length of body frame specific force vector used by AHRS calculation (m/s/s) + bool ahrs_turn_comp_enabled; // true when compensation for centripetal acceleration in coordinated turns using true airspeed is being used. float true_airspeed; // true airspeed used to correct for centripetal acceleratoin in coordinated turns (m/s) // Runs quaternion prediction for the selected AHRS using IMU (and optionally true airspeed) data