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

ekf2_main - Add optical flow innovation pre-flight check (#13036)

Browse Source 
* ekf2: Add FirstOrderLpf and InnovationLpf classes for innovation lowpass filtering

* ekf2: use InnovLpf filter class in preflight checks

* ekf2: move selection of yaw test limit for pre-flight check in function

* ekf2: Move pre-flight checks into separate function

* ekf2: use static constexpr insetead of inline for sq (square) function

* ekf2: Split pre-flight checks in separate functions
Also use the same check for all the innovations:
innov_lpf < test and innov < 2xtest

* ekf2: Add optical flow pre-flight check

* ekf2: Combine FirstOrderLpf and InnovationLpf in single class

* ekf2: check vel_pos_innov when ev_pos is active as well

* ekf2: transform InnovationLpf into a header only library and pass the
spike limit during the update call to avoid storing it here

* ekf2: Static and const cleanup
- set spike_lim constants as static constexpr, set innovation
- set checker helper functions as static
- rename the mix of heading and yaw as heading to avoid confusion

* ekf2: use ternary operator in selectHeadingTestLimit instead of if-else

* ekf2: store intermediate redults in const bool flags. Those will be used for logging

* ekf2: set variable const whenever possible

* ekf2: create PreFlightChecker class that handle all the innovation
pre-flight checks.
Add simple unit testing
Use bitmask instead of general flag to have more granularity

* PreFlightChecker: use setter for the innovations to check instead of sending booleans in the update function
This makes it more scalable as more checks will be added

* ekf: Use booleans instead of bitmask for ekf preflt checks
Rename "down" to "vert"
This commit is contained in:
Mathieu Bresciani
2019-10-22 16:22:42 +02:00
committed by GitHub
parent 644c816a2a
commit 549fb0d5de
9 changed files with 620 additions and 87 deletions
Download Patch File Download Diff File Expand all files Collapse all files
msg/estimator_status.msg
+4 -2
View File
@@ -106,8 +106,10 @@ uint16 solution_status_flags # Bitmask indicating which filter kinematic state o
float32 time_slip # cumulative amount of time in seconds that the EKF inertial calculation has slipped relative to system time
bool pre_flt_fail # true when estimator has failed pre-flight checks and the vehicle should not be flown regardless of flight mode
bool pre_flt_fail_innov_heading
bool pre_flt_fail_innov_vel_horiz
bool pre_flt_fail_innov_vel_vert
bool pre_flt_fail_innov_height
# legacy local position estimator (LPE) flags
uint8 health_flags # Bitmask to indicate sensor health states (vel, pos, hgt)