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

LPE Variance Dependent Publication (#4914)

Browse Source 
* Use variance to control publishing for LPE.

* Don't stop publishing if we have gps/ baro.

* LPE tuning and cleanup.

* Added bias saturation to LPE.

* Added vector enabled low pass filter block.

* Added rk4 integration and pub lowpass to LPE.

* Fix std::abs issue on mac/ reset lowpass on state reset.

* Don't estimate gyro bias when rotating at high speed  att_est_q.

* Lowered low pass on position to 5 Hz for LPE.

* Streamline state space update for LPE.

* Added health flags to est2 log.

* Revert to old tuning, more conservative, less faults.

* Formatting.

* Fix for fault message on LPE.

* Added subscription throttling to LPE.

* Formatting.
This commit is contained in:
James Goppert
2016-07-01 11:43:09 -04:00
committed by GitHub
parent 2a395c3fec
commit 00dfc99e08
17 changed files with 417 additions and 247 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
+4 -1
View File
@@ -797,6 +797,7 @@ bool AttitudeEstimatorQ::update(float dt)
_q.normalize();
// Accelerometer correction
// Project 'k' unit vector of earth frame to body frame
// Vector<3> k = _q.conjugate_inversed(Vector<3>(0.0f, 0.0f, 1.0f));
@@ -810,7 +811,9 @@ bool AttitudeEstimatorQ::update(float dt)
corr += (k % (_accel - _pos_acc).normalized()) * _w_accel;
// Gyro bias estimation
_gyro_bias += corr * (_w_gyro_bias * dt);
if (_gyro.length() < 1.0f) {
_gyro_bias += corr * (_w_gyro_bias * dt);
}
for (int i = 0; i < 3; i++) {
_gyro_bias(i) = math::constrain(_gyro_bias(i), -_bias_max, _bias_max);