fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-20 14:09:06 +08:00
Code Issues Packages Projects Releases Wiki Activity

Accelerate tilt fine alignment by reducing obs noise when at rest

Browse Source 
Fine tilt alignment is accomplished by fusing fake position data after
a coarse tilt leveling. The observation noise can be reduced if the
vehicle is at rest to speed up the process.
This commit is contained in:
bresch 2021-03-02 14:58:52 +01:00 committed by Paul Riseborough
parent 098d5ce5c0
commit 5d441d65e1
1 changed files with 5 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
EKF/control.cpp
View File

@ -1304,6 +1304,11 @@ void Ekf::controlFakePosFusion()
if (_control_status.flags.in_air && _control_status.flags.tilt_align) {
fake_pos_obs_var(0) = fake_pos_obs_var(1) = sq(fmaxf(_params.pos_noaid_noise, _params.gps_pos_noise));
} else if (_control_status.flags.vehicle_at_rest) {
// Accelerate tilt fine alignment by fusing more
// aggressively when the vehicle is at rest
fake_pos_obs_var(0) = fake_pos_obs_var(1) = sq(0.1f);
} else {
fake_pos_obs_var(0) = fake_pos_obs_var(1) = sq(0.5f);
}