fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-26 09:03:34 +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/ekf2/EKF2.hpp
+43 -27
View File
@@ -39,8 +39,12 @@
*/
#pragma once
#include "EKF2Selector.hpp"
#include <float.h>
#include <containers/LockGuard.hpp>
#include <drivers/drv_hrt.h>
#include <lib/ecl/EKF/ekf.h>
#include <lib/mathlib/mathlib.h>
@@ -61,6 +65,7 @@
#include <uORB/topics/ekf2_timestamps.h>
#include <uORB/topics/ekf_gps_drift.h>
#include <uORB/topics/estimator_innovations.h>
#include <uORB/topics/estimator_optical_flow_vel.h>
#include <uORB/topics/estimator_sensor_bias.h>
#include <uORB/topics/estimator_states.h>
#include <uORB/topics/estimator_status.h>
@@ -81,14 +86,16 @@
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/wind_estimate.h>
#include <uORB/topics/yaw_estimator_status.h>
#include <uORB/topics/estimator_optical_flow_vel.h>
#include "Utility/PreFlightChecker.hpp"
class EKF2 final : public ModuleBase<EKF2>, public ModuleParams, public px4::ScheduledWorkItem
extern pthread_mutex_t ekf2_module_mutex;
class EKF2 final : public ModuleParams, public px4::ScheduledWorkItem
{
public:
explicit EKF2(bool replay_mode = false);
EKF2() = delete;
EKF2(int instance, const px4::wq_config_t &config, int imu, int mag, bool replay_mode);
~EKF2() override;
/** @see ModuleBase */
@@ -100,9 +107,15 @@ public:
/** @see ModuleBase */
static int print_usage(const char *reason = nullptr);
bool init();
int print_status();
int print_status() override;
bool should_exit() const { return _task_should_exit.load(); }
void request_stop() { _task_should_exit.store(true); }
static void lock_module() { pthread_mutex_lock(&ekf2_module_mutex); }
static bool trylock_module() { return (pthread_mutex_trylock(&ekf2_module_mutex) == 0); }
static void unlock_module() { pthread_mutex_unlock(&ekf2_module_mutex); }
private:
void Run() override;
@@ -123,19 +136,23 @@ private:
bool update_mag_decl(Param &mag_decl_param);
void publish_attitude(const hrt_abstime &timestamp);
void publish_estimator_optical_flow_vel(const hrt_abstime &timestamp);
void publish_odometry(const hrt_abstime &timestamp, const imuSample &imu, const vehicle_local_position_s &lpos);
void publish_wind_estimate(const hrt_abstime &timestamp);
void publish_yaw_estimator_status(const hrt_abstime &timestamp);
void publish_estimator_optical_flow_vel(const hrt_abstime &timestamp);
/*
* Calculate filtered WGS84 height from estimated AMSL height
*/
float filter_altitude_ellipsoid(float amsl_hgt);
inline float sq(float x) { return x * x; };
static constexpr float sq(float x) { return x * x; };
const bool _replay_mode; ///< true when we use replay data from a log
const bool _replay_mode{false}; ///< true when we use replay data from a log
const bool _multi_mode;
const int _instance;
px4::atomic_bool _task_should_exit{false};
// time slip monitoring
uint64_t _integrated_time_us = 0; ///< integral of gyro delta time from start (uSec)
@@ -186,9 +203,8 @@ private:
uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
uORB::SubscriptionCallbackWorkItem _sensor_combined_sub{this, ORB_ID(sensor_combined)};
static constexpr int MAX_SENSOR_COUNT = 3;
uORB::SubscriptionCallbackWorkItem _vehicle_imu_sub{this, ORB_ID(vehicle_imu)};
int _imu_sub_index{-1};
bool _callback_registered{false};
int _lockstep_component{-1};
@@ -200,23 +216,25 @@ private:
vehicle_land_detected_s _vehicle_land_detected{};
vehicle_status_s _vehicle_status{};
uORB::Publication<ekf2_timestamps_s> _ekf2_timestamps_pub{ORB_ID(ekf2_timestamps)};
uORB::Publication<ekf_gps_drift_s> _ekf_gps_drift_pub{ORB_ID(ekf_gps_drift)};
uORB::Publication<estimator_innovations_s> _estimator_innovation_test_ratios_pub{ORB_ID(estimator_innovation_test_ratios)};
uORB::Publication<estimator_innovations_s> _estimator_innovation_variances_pub{ORB_ID(estimator_innovation_variances)};
uORB::Publication<estimator_innovations_s> _estimator_innovations_pub{ORB_ID(estimator_innovations)};
uORB::Publication<estimator_optical_flow_vel_s> _estimator_optical_flow_vel_pub{ORB_ID(estimator_optical_flow_vel)};
uORB::Publication<estimator_sensor_bias_s> _estimator_sensor_bias_pub{ORB_ID(estimator_sensor_bias)};
uORB::Publication<estimator_states_s> _estimator_states_pub{ORB_ID(estimator_states)};
uORB::PublicationData<estimator_status_s> _estimator_status_pub{ORB_ID(estimator_status)};
uORB::Publication<vehicle_attitude_s> _att_pub{ORB_ID(vehicle_attitude)};
uORB::Publication<vehicle_odometry_s> _vehicle_odometry_pub{ORB_ID(vehicle_odometry)};
uORB::Publication<yaw_estimator_status_s> _yaw_est_pub{ORB_ID(yaw_estimator_status)};
uORB::PublicationData<vehicle_global_position_s> _vehicle_global_position_pub{ORB_ID(vehicle_global_position)};
uORB::PublicationData<vehicle_local_position_s> _vehicle_local_position_pub{ORB_ID(vehicle_local_position)};
uORB::PublicationData<vehicle_odometry_s> _vehicle_visual_odometry_aligned_pub{ORB_ID(vehicle_visual_odometry_aligned)};
uORB::PublicationMulti<ekf2_timestamps_s> _ekf2_timestamps_pub{ORB_ID(ekf2_timestamps)};
uORB::PublicationMulti<ekf_gps_drift_s> _ekf_gps_drift_pub{ORB_ID(ekf_gps_drift)};
uORB::PublicationMulti<estimator_innovations_s> _estimator_innovation_test_ratios_pub{ORB_ID(estimator_innovation_test_ratios)};
uORB::PublicationMulti<estimator_innovations_s> _estimator_innovation_variances_pub{ORB_ID(estimator_innovation_variances)};
uORB::PublicationMulti<estimator_innovations_s> _estimator_innovations_pub{ORB_ID(estimator_innovations)};
uORB::PublicationMulti<estimator_optical_flow_vel_s> _estimator_optical_flow_vel_pub{ORB_ID(estimator_optical_flow_vel)};
uORB::PublicationMulti<estimator_sensor_bias_s> _estimator_sensor_bias_pub{ORB_ID(estimator_sensor_bias)};
uORB::PublicationMulti<estimator_states_s> _estimator_states_pub{ORB_ID(estimator_states)};
uORB::PublicationMultiData<estimator_status_s> _estimator_status_pub{ORB_ID(estimator_status)};
uORB::PublicationMulti<yaw_estimator_status_s> _yaw_est_pub{ORB_ID(yaw_estimator_status)};
uORB::PublicationMulti<wind_estimate_s> _wind_pub{ORB_ID(wind_estimate)};
// publications with topic dependent on multi-mode
uORB::PublicationMulti<vehicle_attitude_s> _attitude_pub;
uORB::PublicationMultiData<vehicle_local_position_s> _local_position_pub;
uORB::PublicationMultiData<vehicle_global_position_s> _global_position_pub;
uORB::PublicationMulti<vehicle_odometry_s> _odometry_pub;
uORB::PublicationMultiData<vehicle_odometry_s> _visual_odometry_aligned_pub;
Ekf _ekf;
parameters *_params; ///< pointer to ekf parameter struct (located in _ekf class instance)
@@ -368,8 +386,6 @@ private:
(ParamExtFloat<px4::params::EKF2_OF_GATE>)
_param_ekf2_of_gate, ///< optical flow fusion innovation consistency gate size (STD)
(ParamInt<px4::params::EKF2_IMU_ID>) _param_ekf2_imu_id,
// sensor positions in body frame
(ParamExtFloat<px4::params::EKF2_IMU_POS_X>) _param_ekf2_imu_pos_x, ///< X position of IMU in body frame (m)
(ParamExtFloat<px4::params::EKF2_IMU_POS_Y>) _param_ekf2_imu_pos_y, ///< Y position of IMU in body frame (m)