Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-23 14:47:44 +08:00
Code Issues Actions 16 Packages Projects Releases Wiki Activity

ekf2-grav: accelerate tilt alignment

Browse Source
This commit is contained in:
bresch
2025-04-07 15:36:11 +02:00
committed by Mathieu Bresciani
parent c59101e8ed
commit 6a105bcbdb
2 changed files with 3 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/aid_sources/fake_pos_control.cpp
+1 -1
View File
@@ -78,7 +78,7 @@ void Ekf::controlFakePosFusion()
const bool enable_valid_fake_pos = _control_status.flags.constant_pos || _control_status.flags.vehicle_at_rest;
const bool enable_fake_pos = !enable_valid_fake_pos
&& (getTiltVariance() > sq(math::radians(3.f)))
&& !(_params.imu_ctrl & static_cast<int32_t>(ImuCtrl::GravityVector))
&& !_control_status.flags.gravity_vector
&& _horizontal_deadreckon_time_exceeded;
_control_status.flags.fake_pos = runFakePosStateMachine(enable_fake_pos, _control_status.flags.fake_pos, aid_src);
src/modules/ekf2/EKF/aid_sources/gravity/gravity_fusion.cpp
+2 -1
View File
@@ -61,7 +61,8 @@ void Ekf::controlGravityFusion(const imuSample &imu)
// fuse gravity observation if our overall acceleration isn't too big
_control_status.flags.gravity_vector = (_params.imu_ctrl & static_cast<int32_t>(ImuCtrl::GravityVector))
&& (accel_lpf_norm_good || _control_status.flags.vehicle_at_rest)
&& !isHorizontalAidingActive();
&& !isHorizontalAidingActive()
&& _control_status.flags.tilt_align; // Let fake position do the initial alignment (more robust before takeoff)
// calculate kalman gains and innovation variances
Vector3f innovation = _state.quat_nominal.rotateVectorInverse(Vector3f(0.f, 0.f, -1.f)) - measurement;