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

fix error from refactring commit, fix reset on ground (#23370)

Browse Source
This commit is contained in:
Marco Hauswirth
2024-07-08 13:55:05 +02:00
committed by GitHub
parent e04c53241a
commit 4bc0286eb8
2 changed files with 5 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/ekf.cpp
+2 -2
View File
@@ -308,10 +308,10 @@ bool Ekf::resetGlobalPosToExternalObservation(double lat_deg, double lon_deg, fl
pos_corrected += _state.vel.xy() * dt_s; pos_corrected += _state.vel.xy() * dt_s;
} }
const float obs_var = math::max(accuracy, sq(0.01f)); const float obs_var = math::max(sq(accuracy), sq(0.01f));
const Vector2f innov = Vector2f(_state.pos.xy()) - pos_corrected; const Vector2f innov = Vector2f(_state.pos.xy()) - pos_corrected;
const Vector2f innov_var = Vector2f(getStateVariance<State::vel>()) + obs_var; const Vector2f innov_var = Vector2f(getStateVariance<State::pos>()) + obs_var;
const float sq_gate = sq(5.f); // magic hardcoded gate const float sq_gate = sq(5.f); // magic hardcoded gate
const Vector2f test_ratio{sq(innov(0)) / (sq_gate * innov_var(0)), const Vector2f test_ratio{sq(innov(0)) / (sq_gate * innov_var(0)),
src/modules/ekf2/EKF2.cpp
+3 -2
View File
@@ -529,8 +529,9 @@ void EKF2::Run()
} else if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_EXTERNAL_POSITION_ESTIMATE) { } else if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_EXTERNAL_POSITION_ESTIMATE) {
if ((_ekf.control_status_flags().wind_dead_reckoning || _ekf.control_status_flags().inertial_dead_reckoning) && if ((_ekf.control_status_flags().wind_dead_reckoning || _ekf.control_status_flags().inertial_dead_reckoning
PX4_ISFINITE(vehicle_command.param2) && PX4_ISFINITE(vehicle_command.param5) && PX4_ISFINITE(vehicle_command.param6) || (!_ekf.control_status_flags().in_air && !_ekf.control_status_flags().gps)) && PX4_ISFINITE(vehicle_command.param2)
&& PX4_ISFINITE(vehicle_command.param5) && PX4_ISFINITE(vehicle_command.param6)
) { ) {
const float measurement_delay_seconds = math::constrain(vehicle_command.param2, 0.0f, const float measurement_delay_seconds = math::constrain(vehicle_command.param2, 0.0f,