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

ekf2: add kconfig option to enable/disable airspeed and sideslip fusion

Browse Source
This commit is contained in:
Daniel Agar
2023-03-20 10:12:17 -04:00
parent 98ff1afc19
commit 4270a303ab
16 changed files with 218 additions and 83 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/airspeed_fusion.cpp
-26
View File
@@ -222,21 +222,6 @@ void Ekf::stopAirspeedFusion()
}
}
float Ekf::getTrueAirspeed() const
{
return (_state.vel - Vector3f(_state.wind_vel(0), _state.wind_vel(1), 0.f)).norm();
}
void Ekf::resetWind()
{
if (_control_status.flags.fuse_aspd && isRecent(_airspeed_sample_delayed.time_us, 1e6)) {
resetWindUsingAirspeed(_airspeed_sample_delayed);
} else {
resetWindToZero();
}
}
void Ekf::resetWindUsingAirspeed(const airspeedSample &airspeed_sample)
{
const float euler_yaw = getEulerYaw(_R_to_earth);
@@ -282,14 +267,3 @@ void Ekf::resetWindCovarianceUsingAirspeed(const airspeedSample &airspeed_sample
P(22, 22) += P(4, 4);
P(23, 23) += P(5, 5);
}
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);
}