fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-22 10:17: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
src/modules/commander/PreflightCheck.cpp
+16 -2
View File
@@ -494,9 +494,23 @@ static bool ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s &vehicle_s
}
// Check if preflight check performed by estimator has failed
if (status.pre_flt_fail) {
if (status.pre_flt_fail_innov_heading ||
status.pre_flt_fail_innov_vel_horiz ||
status.pre_flt_fail_innov_vel_vert ||
status.pre_flt_fail_innov_height) {
if (report_fail) {
mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Position unknown");
if (status.pre_flt_fail_innov_heading) {
mavlink_log_critical(mavlink_log_pub, "Preflight Fail: heading estimate not stable");
} else if (status.pre_flt_fail_innov_vel_horiz) {
mavlink_log_critical(mavlink_log_pub, "Preflight Fail: horizontal velocity estimate not stable");
} else if (status.pre_flt_fail_innov_vel_horiz) {
mavlink_log_critical(mavlink_log_pub, "Preflight Fail: vertical velocity estimate not stable");
} else if (status.pre_flt_fail_innov_height) {
mavlink_log_critical(mavlink_log_pub, "Preflight Fail: height estimate not stable");
}
}
success = false;