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

ekf2: include 0 timestamp checks in helpers (isTimedOut(), isRecent(), etc)

Browse Source 
- EKF isTimedOut(), isRecent(), and isNewestSampleRecent() need to handle the case where the timestamp has never been set
 - reset() more thoroughly reset fields (mainly impacts unit tests)
This commit is contained in:
Daniel Agar
2023-02-03 10:00:51 -05:00
committed by GitHub
parent 264a99fb77
commit 66ad7fd06c
11 changed files with 834 additions and 767 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/airspeed_fusion.cpp
+5
View File
@@ -157,6 +157,8 @@ void Ekf::resetWindUsingAirspeed()
_state.wind_vel(0) = _state.vel(0) - _airspeed_sample_delayed.true_airspeed * cosf(euler_yaw);
_state.wind_vel(1) = _state.vel(1) - _airspeed_sample_delayed.true_airspeed * sinf(euler_yaw);
ECL_INFO("reset wind using airspeed to (%.3f, %.3f)", (double)_state.wind_vel(0), (double)_state.wind_vel(1));
resetWindCovarianceUsingAirspeed();
_aid_src_airspeed.time_last_fuse = _time_delayed_us;
@@ -164,8 +166,11 @@ void Ekf::resetWindUsingAirspeed()
void Ekf::resetWindToZero()
{
ECL_INFO("reset wind to zero");
// If we don't have an airspeed measurement, then assume the wind is zero
_state.wind_vel.setZero();
// start with a small initial uncertainty to improve the initial estimate
P.uncorrelateCovarianceSetVariance<2>(22, _params.initial_wind_uncertainty);
}