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

Multi-EKF support (ekf2)

Browse Source 
- ekf2 can now run in multi-instance mode (currently up to 9 instances)
    - in multi mode all estimates are published to alternate topics (eg estimator_attitude instead of vehicle_attitude)
 - new ekf2 selector runs in multi-instance mode to monitor and compare all instances, selecting a primary (eg N x estimator_attitude => vehicle_attitude)
 - sensors module accel & gyro inconsistency checks are now relative to the mean of all instances, rather than the current primary (when active ekf2 selector is responsible for choosing primary accel & gyro)
 - existing consumers of estimator_status must check estimator_selector_status to select current primary instance status
 - ekf2 single instance mode is still fully supported and the default

Co-authored-by: Paul Riseborough <gncsolns@gmail.com>
This commit is contained in:
Daniel Agar
2020-10-27 10:56:11 -04:00
committed by GitHub
co-authored by Paul Riseborough
parent d5245a22d3
commit 0f411d6820
56 changed files with 1747 additions and 295 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/Arming/PreFlightCheck/checks/ekf2Check.cpp
+6 -2
View File
@@ -38,6 +38,7 @@
#include <lib/parameters/param.h>
#include <systemlib/mavlink_log.h>
#include <uORB/Subscription.hpp>
#include <uORB/topics/estimator_selector_status.h>
#include <uORB/topics/estimator_states.h>
#include <uORB/topics/estimator_status.h>
@@ -66,7 +67,8 @@ bool PreFlightCheck::ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s &
bool gps_present = true;
// Get estimator status data if available and exit with a fail recorded if not
uORB::SubscriptionData<estimator_status_s> status_sub{ORB_ID(estimator_status)};
uORB::SubscriptionData<estimator_selector_status_s> estimator_selector_status_sub{ORB_ID(estimator_selector_status)};
uORB::SubscriptionData<estimator_status_s> status_sub{ORB_ID(estimator_status), estimator_selector_status_sub.get().primary_instance};
status_sub.update();
const estimator_status_s &status = status_sub.get();
@@ -238,7 +240,9 @@ bool PreFlightCheck::ekf2CheckStates(orb_advert_t *mavlink_log_pub, const bool r
param_get(param_find("COM_ARM_EKF_GB"), &ekf_gb_test_limit);
// Get estimator states data if available and exit with a fail recorded if not
uORB::Subscription states_sub{ORB_ID(estimator_states)};
uORB::SubscriptionData<estimator_selector_status_s> estimator_selector_status_sub{ORB_ID(estimator_selector_status)};
uORB::Subscription states_sub{ORB_ID(estimator_states), estimator_selector_status_sub.get().primary_instance};
estimator_states_s states;
if (states_sub.copy(&states)) {