fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-19 02:40:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: apply astyle formatting and enforce

Browse Source
This commit is contained in:
Daniel Agar
2024-07-10 10:40:29 -04:00
parent 3fe609f769
commit c29b4ff87e
36 changed files with 525 additions and 300 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/ekf.cpp
+24 -13
View File
@@ -168,7 +168,8 @@ bool Ekf::update()
// control fusion of observation data
controlFusionModes(imu_sample_delayed);
_output_predictor.correctOutputStates(imu_sample_delayed.time_us, _state.quat_nominal, _state.vel, _state.pos, _state.gyro_bias, _state.accel_bias);
_output_predictor.correctOutputStates(imu_sample_delayed.time_us, _state.quat_nominal, _state.vel, _state.pos,
_state.gyro_bias, _state.accel_bias);
return true;
}
@@ -281,7 +282,8 @@ void Ekf::predictState(const imuSample &imu_delayed)
_height_rate_lpf = _height_rate_lpf * (1.0f - alpha_height_rate_lpf) + _state.vel(2) * alpha_height_rate_lpf;
}
bool Ekf::resetGlobalPosToExternalObservation(double lat_deg, double lon_deg, float accuracy, uint64_t timestamp_observation)
bool Ekf::resetGlobalPosToExternalObservation(double lat_deg, double lon_deg, float accuracy,
uint64_t timestamp_observation)
{
if (!_pos_ref.isInitialized()) {
ECL_WARN("unable to reset global position, position reference not initialized");
@@ -315,7 +317,7 @@ bool Ekf::resetGlobalPosToExternalObservation(double lat_deg, double lon_deg, fl
const float sq_gate = sq(5.f); // magic hardcoded gate
const Vector2f test_ratio{sq(innov(0)) / (sq_gate * innov_var(0)),
sq(innov(1)) / (sq_gate * innov_var(1))};
sq(innov(1)) / (sq_gate * innov_var(1))};
const bool innov_rejected = (test_ratio.max() > 1.f);
@@ -331,8 +333,8 @@ bool Ekf::resetGlobalPosToExternalObservation(double lat_deg, double lon_deg, fl
} else {
if (fuseDirectStateMeasurement(innov(0), innov_var(0), obs_var, State::pos.idx + 0)
&& fuseDirectStateMeasurement(innov(1), innov_var(1), obs_var, State::pos.idx + 1)
) {
&& fuseDirectStateMeasurement(innov(1), innov_var(1), obs_var, State::pos.idx + 1)
) {
ECL_INFO("fused external observation as position measurement");
_time_last_hor_pos_fuse = _time_delayed_us;
_last_known_pos.xy() = _state.pos.xy();
@@ -366,40 +368,49 @@ void Ekf::print_status()
printf("\nStates: (%.4f seconds ago)\n", (_time_latest_us - _time_delayed_us) * 1e-6);
printf("Orientation (%d-%d): [%.3f, %.3f, %.3f, %.3f] (Euler [%.1f, %.1f, %.1f] deg) var: [%.1e, %.1e, %.1e]\n",
State::quat_nominal.idx, State::quat_nominal.idx + State::quat_nominal.dof - 1,
(double)_state.quat_nominal(0), (double)_state.quat_nominal(1), (double)_state.quat_nominal(2), (double)_state.quat_nominal(3),
(double)math::degrees(matrix::Eulerf(_state.quat_nominal).phi()), (double)math::degrees(matrix::Eulerf(_state.quat_nominal).theta()), (double)math::degrees(matrix::Eulerf(_state.quat_nominal).psi()),
(double)getStateVariance<State::quat_nominal>()(0), (double)getStateVariance<State::quat_nominal>()(1), (double)getStateVariance<State::quat_nominal>()(2)
(double)_state.quat_nominal(0), (double)_state.quat_nominal(1), (double)_state.quat_nominal(2),
(double)_state.quat_nominal(3),
(double)math::degrees(matrix::Eulerf(_state.quat_nominal).phi()),
(double)math::degrees(matrix::Eulerf(_state.quat_nominal).theta()),
(double)math::degrees(matrix::Eulerf(_state.quat_nominal).psi()),
(double)getStateVariance<State::quat_nominal>()(0), (double)getStateVariance<State::quat_nominal>()(1),
(double)getStateVariance<State::quat_nominal>()(2)
);
printf("Velocity (%d-%d): [%.3f, %.3f, %.3f] var: [%.1e, %.1e, %.1e]\n",
State::vel.idx, State::vel.idx + State::vel.dof - 1,
(double)_state.vel(0), (double)_state.vel(1), (double)_state.vel(2),
(double)getStateVariance<State::vel>()(0), (double)getStateVariance<State::vel>()(1), (double)getStateVariance<State::vel>()(2)
(double)getStateVariance<State::vel>()(0), (double)getStateVariance<State::vel>()(1),
(double)getStateVariance<State::vel>()(2)
);
printf("Position (%d-%d): [%.3f, %.3f, %.3f] var: [%.1e, %.1e, %.1e]\n",
State::pos.idx, State::pos.idx + State::pos.dof - 1,
(double)_state.pos(0), (double)_state.pos(1), (double)_state.pos(2),
(double)getStateVariance<State::pos>()(0), (double)getStateVariance<State::pos>()(1), (double)getStateVariance<State::pos>()(2)
(double)getStateVariance<State::pos>()(0), (double)getStateVariance<State::pos>()(1),
(double)getStateVariance<State::pos>()(2)
);
printf("Gyro Bias (%d-%d): [%.6f, %.6f, %.6f] var: [%.1e, %.1e, %.1e]\n",
State::gyro_bias.idx, State::gyro_bias.idx + State::gyro_bias.dof - 1,
(double)_state.gyro_bias(0), (double)_state.gyro_bias(1), (double)_state.gyro_bias(2),
(double)getStateVariance<State::gyro_bias>()(0), (double)getStateVariance<State::gyro_bias>()(1), (double)getStateVariance<State::gyro_bias>()(2)
(double)getStateVariance<State::gyro_bias>()(0), (double)getStateVariance<State::gyro_bias>()(1),
(double)getStateVariance<State::gyro_bias>()(2)
);
printf("Accel Bias (%d-%d): [%.6f, %.6f, %.6f] var: [%.1e, %.1e, %.1e]\n",
State::accel_bias.idx, State::accel_bias.idx + State::accel_bias.dof - 1,
(double)_state.accel_bias(0), (double)_state.accel_bias(1), (double)_state.accel_bias(2),
(double)getStateVariance<State::accel_bias>()(0), (double)getStateVariance<State::accel_bias>()(1), (double)getStateVariance<State::accel_bias>()(2)
(double)getStateVariance<State::accel_bias>()(0), (double)getStateVariance<State::accel_bias>()(1),
(double)getStateVariance<State::accel_bias>()(2)
);
#if defined(CONFIG_EKF2_MAGNETOMETER)
printf("Magnetic Field (%d-%d): [%.3f, %.3f, %.3f] var: [%.1e, %.1e, %.1e]\n",
State::mag_I.idx, State::mag_I.idx + State::mag_I.dof - 1,
(double)_state.mag_I(0), (double)_state.mag_I(1), (double)_state.mag_I(2),
(double)getStateVariance<State::mag_I>()(0), (double)getStateVariance<State::mag_I>()(1), (double)getStateVariance<State::mag_I>()(2)
(double)getStateVariance<State::mag_I>()(0), (double)getStateVariance<State::mag_I>()(1),
(double)getStateVariance<State::mag_I>()(2)
);
printf("Magnetic Bias (%d-%d): [%.3f, %.3f, %.3f] var: [%.1e, %.1e, %.1e]\n",