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

AP_NavEKF: tidy creation of vectors

Browse Source
This commit is contained in:
Peter Barker 2022-02-14 14:00:33 +11:00 committed by Andrew Tridgell
parent 4fb8408e80
commit 2d7a4b318d
1 changed files with 9 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
libraries/AP_NavEKF/EKFGSF_yaw.cpp
View File

@ -103,7 +103,7 @@ void EKFGSF_yaw::update(const Vector3F &delAng,
// Calculate a composite yaw as a weighted average of the states for each model.
// To avoid issues with angle wrapping, the yaw state is converted to a vector with legnth
// equal to the weighting value before it is summed.
Vector2F yaw_vector = {};
Vector2F yaw_vector;
for (uint8_t mdl_idx = 0; mdl_idx < N_MODELS_EKFGSF; mdl_idx++) {
yaw_vector[0] += GSF.weights[mdl_idx] * cosF(EKF[mdl_idx].X[2]);
yaw_vector[1] += GSF.weights[mdl_idx] * sinF(EKF[mdl_idx].X[2]);
@ -196,15 +196,15 @@ void EKFGSF_yaw::predictAHRS(const uint8_t mdl_idx)
// Generate attitude solution using simple complementary filter for the selected model
// Calculate 'k' unit vector of earth frame rotated into body frame
const Vector3F k(AHRS[mdl_idx].R[2][0], AHRS[mdl_idx].R[2][1], AHRS[mdl_idx].R[2][2]);
const Vector3F k{AHRS[mdl_idx].R[2][0], AHRS[mdl_idx].R[2][1], AHRS[mdl_idx].R[2][2]};
// Calculate angular rate vector in rad/sec averaged across last sample interval
Vector3F ang_rate_delayed_raw = delta_angle / angle_dt;
const Vector3F ang_rate_delayed_raw { delta_angle / angle_dt };
// Perform angular rate correction using accel data and reduce correction as accel magnitude moves away from 1 g (reduces drift when vehicle picked up and moved).
// During fixed wing flight, compensate for centripetal acceleration assuming coordinated turns and X axis forward
Vector3F tilt_error_gyro_correction = {}; // (rad/sec)
Vector3F tilt_error_gyro_correction; // (rad/sec)
if (accel_gain > 0.0f) {
@ -213,8 +213,11 @@ void EKFGSF_yaw::predictAHRS(const uint8_t mdl_idx)
if (is_positive(true_airspeed)) {
// Calculate centripetal acceleration in body frame from cross product of body rate and body frame airspeed vector
// NOTE: this assumes X axis is aligned with airspeed vector
Vector3F centripetal_accel_vec_bf = Vector3F(0.0f, ang_rate_delayed_raw[2] * true_airspeed, - ang_rate_delayed_raw[1] * true_airspeed);
const Vector3F centripetal_accel_vec_bf {
0.0f,
ang_rate_delayed_raw[2] * true_airspeed,
- ang_rate_delayed_raw[1] * true_airspeed
};
// Correct measured accel for centripetal acceleration
accel -= centripetal_accel_vec_bf;
}