From 33a7fed2402ab299a7a1f8de7650b8f7a8b3d45b Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Mon, 24 Aug 2020 14:02:42 -0400 Subject: [PATCH] ekf2: split out header and rename --- src/modules/ekf2/CMakeLists.txt | 7 +- src/modules/ekf2/{ekf2_main.cpp => EKF2.cpp} | 558 +------------------ src/modules/ekf2/EKF2.hpp | 538 ++++++++++++++++++ src/modules/ekf2/Utility/CMakeLists.txt | 8 +- 4 files changed, 573 insertions(+), 538 deletions(-) rename src/modules/ekf2/{ekf2_main.cpp => EKF2.cpp} (71%) create mode 100644 src/modules/ekf2/EKF2.hpp diff --git a/src/modules/ekf2/CMakeLists.txt b/src/modules/ekf2/CMakeLists.txt index 85dfc34173..6292baf227 100644 --- a/src/modules/ekf2/CMakeLists.txt +++ b/src/modules/ekf2/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# Copyright (c) 2015-2020 PX4 Development Team. All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions @@ -39,11 +39,12 @@ px4_add_module( COMPILE_FLAGS STACK_MAX 2400 SRCS - ekf2_main.cpp + EKF2.cpp + EKF2.hpp DEPENDS git_ecl ecl_EKF ecl_geo perf - Ekf2Utility + EKF2Utility ) diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/EKF2.cpp similarity index 71% rename from src/modules/ekf2/ekf2_main.cpp rename to src/modules/ekf2/EKF2.cpp index 91adc93ced..c3771e62c9 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2015-2019 PX4 Development Team. All rights reserved. + * Copyright (c) 2015-2020 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -31,513 +31,9 @@ * ****************************************************************************/ -/** - * @file ekf2_main.cpp - * Implementation of the attitude and position estimator. - * - * @author Roman Bapst - */ +#include "EKF2.hpp" -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "Utility/PreFlightChecker.hpp" - -// defines used to specify the mask position for use of different accuracy metrics in the GPS blending algorithm -#define BLEND_MASK_USE_SPD_ACC 1 -#define BLEND_MASK_USE_HPOS_ACC 2 -#define BLEND_MASK_USE_VPOS_ACC 4 - -// define max number of GPS receivers supported and 0 base instance used to access virtual 'blended' GPS solution -#define GPS_MAX_RECEIVERS 2 -#define GPS_BLENDED_INSTANCE 2 - -using math::constrain; -using namespace time_literals; - -class Ekf2 final : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem -{ -public: - explicit Ekf2(bool replay_mode = false); - ~Ekf2() override; - - /** @see ModuleBase */ - static int task_spawn(int argc, char *argv[]); - - /** @see ModuleBase */ - static int custom_command(int argc, char *argv[]); - - /** @see ModuleBase */ - static int print_usage(const char *reason = nullptr); - - bool init(); - - int print_status() override; - -private: - void Run() override; - - int getRangeSubIndex(); ///< get subscription index of first downward-facing range sensor - void fillGpsMsgWithVehicleGpsPosData(gps_message &msg, const vehicle_gps_position_s &data); - - PreFlightChecker _preflt_checker; - void runPreFlightChecks(float dt, const filter_control_status_u &control_status, - const vehicle_status_s &vehicle_status, - const estimator_innovations_s &innov); - void resetPreFlightChecks(); - - template - void update_mag_bias(Param &mag_bias_param, int axis_index); - - template - bool update_mag_decl(Param &mag_decl_param); - - void publish_attitude(const hrt_abstime ×tamp); - void publish_wind_estimate(const hrt_abstime ×tamp); - void publish_yaw_estimator_status(const hrt_abstime ×tamp); - - /* - * Update the internal state estimate for a blended GPS solution that is a weighted average of the phsyical - * receiver solutions. This internal state cannot be used directly by estimators because if physical receivers - * have significant position differences, variation in receiver estimated accuracy will cause undesirable - * variation in the position solution. - */ - bool blend_gps_data(); - - /* - * Calculate internal states used to blend GPS data from multiple receivers using weightings calculated - * by calc_blend_weights() - * States are written to _gps_state and _gps_blended_state class variables - */ - void update_gps_blend_states(); - - /* - * The location in _gps_blended_state will move around as the relative accuracy changes. - * To mitigate this effect a low-pass filtered offset from each GPS location to the blended location is - * calculated. - */ - void update_gps_offsets(); - - /* - * Apply the steady state physical receiver offsets calculated by update_gps_offsets(). - */ - void apply_gps_offsets(); - - /* - Calculate GPS output that is a blend of the offset corrected physical receiver data - */ - void calc_gps_blend_output(); - - /* - * Calculate filtered WGS84 height from estimated AMSL height - */ - float filter_altitude_ellipsoid(float amsl_hgt); - - inline float sq(float x) { return x * x; }; - - const bool _replay_mode; ///< true when we use replay data from a log - - // time slip monitoring - uint64_t _integrated_time_us = 0; ///< integral of gyro delta time from start (uSec) - uint64_t _start_time_us = 0; ///< system time at EKF start (uSec) - int64_t _last_time_slip_us = 0; ///< Last time slip (uSec) - - perf_counter_t _ekf_update_perf; - - // Initialise time stamps used to send sensor data to the EKF and for logging - uint8_t _invalid_mag_id_count = 0; ///< number of times an invalid magnetomer device ID has been detected - - // Used to check, save and use learned magnetometer biases - hrt_abstime _last_magcal_us = 0; ///< last time the EKF was operating a mode that estimates magnetomer biases (uSec) - hrt_abstime _total_cal_time_us = 0; ///< accumulated calibration time since the last save - - float _last_valid_mag_cal[3] = {}; ///< last valid XYZ magnetometer bias estimates (mGauss) - bool _valid_cal_available[3] = {}; ///< true when an unsaved valid calibration for the XYZ magnetometer bias is available - float _last_valid_variance[3] = {}; ///< variances for the last valid magnetometer XYZ bias estimates (mGauss**2) - - // Used to control saving of mag declination to be used on next startup - bool _mag_decl_saved = false; ///< true when the magnetic declination has been saved - - // set pose/velocity as invalid if standard deviation is bigger than max_std_dev - // TODO: the user should be allowed to set these values by a parameter - static constexpr float ep_max_std_dev = 100.0f; ///< Maximum permissible standard deviation for estimated position - static constexpr float eo_max_std_dev = 100.0f; ///< Maximum permissible standard deviation for estimated orientation - //static constexpr float ev_max_std_dev = 100.0f; ///< Maximum permissible standard deviation for estimated velocity - - // GPS blending and switching - gps_message _gps_state[GPS_MAX_RECEIVERS] {}; ///< internal state data for the physical GPS - gps_message _gps_blended_state{}; ///< internal state data for the blended GPS - gps_message _gps_output[GPS_MAX_RECEIVERS + 1] {}; ///< output state data for the physical and blended GPS - Vector2f _NE_pos_offset_m[GPS_MAX_RECEIVERS] = {}; ///< Filtered North,East position offset from GPS instance to blended solution in _output_state.location (m) - float _hgt_offset_mm[GPS_MAX_RECEIVERS] = {}; ///< Filtered height offset from GPS instance relative to blended solution in _output_state.location (mm) - Vector3f _blended_antenna_offset = {}; ///< blended antenna offset - float _blend_weights[GPS_MAX_RECEIVERS] = {}; ///< blend weight for each GPS. The blend weights must sum to 1.0 across all instances. - uint64_t _time_prev_us[GPS_MAX_RECEIVERS] = {}; ///< the previous value of time_us for that GPS instance - used to detect new data. - uint8_t _gps_best_index = 0; ///< index of the physical receiver with the lowest reported error - uint8_t _gps_select_index = 0; ///< 0 = GPS1, 1 = GPS2, 2 = blended - uint8_t _gps_time_ref_index = - 0; ///< index of the receiver that is used as the timing reference for the blending update - uint8_t _gps_oldest_index = 0; ///< index of the physical receiver with the oldest data - uint8_t _gps_newest_index = 0; ///< index of the physical receiver with the newest data - uint8_t _gps_slowest_index = 0; ///< index of the physical receiver with the slowest update rate - float _gps_dt[GPS_MAX_RECEIVERS] = {}; ///< average time step in seconds. - bool _gps_new_output_data = false; ///< true if there is new output data for the EKF - bool _had_valid_terrain = false; ///< true if at any time there was a valid terrain estimate - - int32_t _gps_alttitude_ellipsoid[GPS_MAX_RECEIVERS] {}; ///< altitude in 1E-3 meters (millimeters) above ellipsoid - uint64_t _gps_alttitude_ellipsoid_previous_timestamp[GPS_MAX_RECEIVERS] {}; ///< storage for previous timestamp to compute dt - float _wgs84_hgt_offset = 0; ///< height offset between AMSL and WGS84 - - bool _imu_bias_reset_request{false}; - - // republished aligned external visual odometry - bool new_ev_data_received = false; - vehicle_odometry_s _ev_odom{}; - - uORB::Subscription _airdata_sub{ORB_ID(vehicle_air_data)}; - uORB::Subscription _airspeed_sub{ORB_ID(airspeed)}; - uORB::Subscription _ev_odom_sub{ORB_ID(vehicle_visual_odometry)}; - uORB::Subscription _landing_target_pose_sub{ORB_ID(landing_target_pose)}; - uORB::Subscription _magnetometer_sub{ORB_ID(vehicle_magnetometer)}; - uORB::Subscription _optical_flow_sub{ORB_ID(optical_flow)}; - uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; - uORB::Subscription _sensor_selection_sub{ORB_ID(sensor_selection)}; - uORB::Subscription _status_sub{ORB_ID(vehicle_status)}; - 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}; - - // because we can have several distance sensor instances with different orientations - uORB::SubscriptionMultiArray _distance_sensor_subs{ORB_ID::distance_sensor}; - int _range_finder_sub_index = -1; // index for downward-facing range finder subscription - - // because we can have multiple GPS instances - uORB::Subscription _gps_subs[GPS_MAX_RECEIVERS] {{ORB_ID(vehicle_gps_position), 0}, {ORB_ID(vehicle_gps_position), 1}}; - - sensor_selection_s _sensor_selection{}; - vehicle_land_detected_s _vehicle_land_detected{}; - vehicle_status_s _vehicle_status{}; - - uORB::Publication _ekf2_timestamps_pub{ORB_ID(ekf2_timestamps)}; - uORB::Publication _ekf_gps_drift_pub{ORB_ID(ekf_gps_drift)}; - uORB::Publication _blended_gps_pub{ORB_ID(ekf_gps_position)}; - uORB::Publication _estimator_innovation_test_ratios_pub{ORB_ID(estimator_innovation_test_ratios)}; - uORB::Publication _estimator_innovation_variances_pub{ORB_ID(estimator_innovation_variances)}; - uORB::Publication _estimator_innovations_pub{ORB_ID(estimator_innovations)}; - uORB::Publication _estimator_sensor_bias_pub{ORB_ID(estimator_sensor_bias)}; - uORB::Publication _estimator_status_pub{ORB_ID(estimator_status)}; - uORB::Publication _att_pub{ORB_ID(vehicle_attitude)}; - uORB::Publication _vehicle_odometry_pub{ORB_ID(vehicle_odometry)}; - uORB::Publication _yaw_est_pub{ORB_ID(yaw_estimator_status)}; - uORB::PublicationData _vehicle_global_position_pub{ORB_ID(vehicle_global_position)}; - uORB::PublicationData _vehicle_local_position_pub{ORB_ID(vehicle_local_position)}; - uORB::PublicationData _vehicle_visual_odometry_aligned_pub{ORB_ID(vehicle_visual_odometry_aligned)}; - uORB::PublicationMulti _wind_pub{ORB_ID(wind_estimate)}; - - Ekf _ekf; - - parameters *_params; ///< pointer to ekf parameter struct (located in _ekf class instance) - - DEFINE_PARAMETERS( - (ParamExtInt) - _param_ekf2_min_obs_dt, ///< Maximum time delay of any sensor used to increase buffer length to handle large timing jitter (mSec) - (ParamExtFloat) - _param_ekf2_mag_delay, ///< magnetometer measurement delay relative to the IMU (mSec) - (ParamExtFloat) - _param_ekf2_baro_delay, ///< barometer height measurement delay relative to the IMU (mSec) - (ParamExtFloat) - _param_ekf2_gps_delay, ///< GPS measurement delay relative to the IMU (mSec) - (ParamExtFloat) - _param_ekf2_of_delay, ///< optical flow measurement delay relative to the IMU (mSec) - this is to the middle of the optical flow integration interval - (ParamExtFloat) - _param_ekf2_rng_delay, ///< range finder measurement delay relative to the IMU (mSec) - (ParamExtFloat) - _param_ekf2_asp_delay, ///< airspeed measurement delay relative to the IMU (mSec) - (ParamExtFloat) - _param_ekf2_ev_delay, ///< off-board vision measurement delay relative to the IMU (mSec) - (ParamExtFloat) - _param_ekf2_avel_delay, ///< auxillary velocity measurement delay relative to the IMU (mSec) - - (ParamExtFloat) - _param_ekf2_gyr_noise, ///< IMU angular rate noise used for covariance prediction (rad/sec) - (ParamExtFloat) - _param_ekf2_acc_noise, ///< IMU acceleration noise use for covariance prediction (m/sec**2) - - // process noise - (ParamExtFloat) - _param_ekf2_gyr_b_noise, ///< process noise for IMU rate gyro bias prediction (rad/sec**2) - (ParamExtFloat) - _param_ekf2_acc_b_noise,///< process noise for IMU accelerometer bias prediction (m/sec**3) - (ParamExtFloat) - _param_ekf2_mag_e_noise, ///< process noise for earth magnetic field prediction (Gauss/sec) - (ParamExtFloat) - _param_ekf2_mag_b_noise, ///< process noise for body magnetic field prediction (Gauss/sec) - (ParamExtFloat) - _param_ekf2_wind_noise, ///< process noise for wind velocity prediction (m/sec**2) - (ParamExtFloat) _param_ekf2_terr_noise, ///< process noise for terrain offset (m/sec) - (ParamExtFloat) - _param_ekf2_terr_grad, ///< gradient of terrain used to estimate process noise due to changing position (m/m) - - (ParamExtFloat) - _param_ekf2_gps_v_noise, ///< minimum allowed observation noise for gps velocity fusion (m/sec) - (ParamExtFloat) - _param_ekf2_gps_p_noise, ///< minimum allowed observation noise for gps position fusion (m) - (ParamExtFloat) - _param_ekf2_noaid_noise, ///< observation noise for non-aiding position fusion (m) - (ParamExtFloat) - _param_ekf2_baro_noise, ///< observation noise for barometric height fusion (m) - (ParamExtFloat) - _param_ekf2_baro_gate, ///< barometric height innovation consistency gate size (STD) - (ParamExtFloat) - _param_ekf2_gnd_eff_dz, ///< barometric deadzone range for negative innovations (m) - (ParamExtFloat) - _param_ekf2_gnd_max_hgt, ///< maximum height above the ground level for expected negative baro innovations (m) - (ParamExtFloat) - _param_ekf2_gps_p_gate, ///< GPS horizontal position innovation consistency gate size (STD) - (ParamExtFloat) - _param_ekf2_gps_v_gate, ///< GPS velocity innovation consistency gate size (STD) - (ParamExtFloat) - _param_ekf2_tas_gate, ///< True Airspeed innovation consistency gate size (STD) - - // control of magnetometer fusion - (ParamExtFloat) - _param_ekf2_head_noise, ///< measurement noise used for simple heading fusion (rad) - (ParamExtFloat) - _param_ekf2_mag_noise, ///< measurement noise used for 3-axis magnetoemeter fusion (Gauss) - (ParamExtFloat) - _param_ekf2_eas_noise, ///< measurement noise used for airspeed fusion (m/sec) - (ParamExtFloat) - _param_ekf2_beta_gate, ///< synthetic sideslip innovation consistency gate size (STD) - (ParamExtFloat) _param_ekf2_beta_noise, ///< synthetic sideslip noise (rad) - (ParamExtFloat) _param_ekf2_mag_decl,///< magnetic declination (degrees) - (ParamExtFloat) - _param_ekf2_hdg_gate,///< heading fusion innovation consistency gate size (STD) - (ParamExtFloat) - _param_ekf2_mag_gate, ///< magnetometer fusion innovation consistency gate size (STD) - (ParamExtInt) - _param_ekf2_decl_type, ///< bitmask used to control the handling of declination data - (ParamExtInt) - _param_ekf2_mag_type, ///< integer used to specify the type of magnetometer fusion used - (ParamExtFloat) - _param_ekf2_mag_acclim, ///< integer used to specify the type of magnetometer fusion used - (ParamExtFloat) - _param_ekf2_mag_yawlim, ///< yaw rate threshold used by mode select logic (rad/sec) - - (ParamExtInt) - _param_ekf2_gps_check, ///< bitmask used to control which GPS quality checks are used - (ParamExtFloat) _param_ekf2_req_eph, ///< maximum acceptable horiz position error (m) - (ParamExtFloat) _param_ekf2_req_epv, ///< maximum acceptable vert position error (m) - (ParamExtFloat) _param_ekf2_req_sacc, ///< maximum acceptable speed error (m/s) - (ParamExtInt) _param_ekf2_req_nsats, ///< minimum acceptable satellite count - (ParamExtFloat) - _param_ekf2_req_pdop, ///< maximum acceptable position dilution of precision - (ParamExtFloat) - _param_ekf2_req_hdrift, ///< maximum acceptable horizontal drift speed (m/s) - (ParamExtFloat) _param_ekf2_req_vdrift, ///< maximum acceptable vertical drift speed (m/s) - - // measurement source control - (ParamExtInt) - _param_ekf2_aid_mask, ///< bitmasked integer that selects which of the GPS and optical flow aiding sources will be used - (ParamExtInt) _param_ekf2_hgt_mode, ///< selects the primary source for height data - (ParamExtInt) - _param_ekf2_terr_mask, ///< bitmasked integer that selects which of range finder and optical flow aiding sources will be used for terrain estimation - (ParamExtInt) - _param_ekf2_noaid_tout, ///< maximum lapsed time from last fusion of measurements that constrain drift before the EKF will report the horizontal nav solution invalid (uSec) - - // range finder fusion - (ParamExtFloat) - _param_ekf2_rng_noise, ///< observation noise for range finder measurements (m) - (ParamExtFloat) _param_ekf2_rng_sfe, ///< scale factor from range to range noise (m/m) - (ParamExtFloat) - _param_ekf2_rng_gate, ///< range finder fusion innovation consistency gate size (STD) - (ParamExtFloat) _param_ekf2_min_rng, ///< minimum valid value for range when on ground (m) - (ParamExtFloat) _param_ekf2_rng_pitch, ///< range sensor pitch offset (rad) - (ParamExtInt) - _param_ekf2_rng_aid, ///< enables use of a range finder even if primary height source is not range finder - (ParamExtFloat) - _param_ekf2_rng_a_vmax, ///< maximum allowed horizontal velocity for range aid (m/s) - (ParamExtFloat) - _param_ekf2_rng_a_hmax, ///< maximum allowed absolute altitude (AGL) for range aid (m) - (ParamExtFloat) - _param_ekf2_rng_a_igate, ///< gate size used for innovation consistency checks for range aid fusion (STD) - - // vision estimate fusion - (ParamInt) - _param_ekf2_ev_noise_md, ///< determine source of vision observation noise - (ParamFloat) - _param_ekf2_evp_noise, ///< default position observation noise for exernal vision measurements (m) - (ParamFloat) - _param_ekf2_evv_noise, ///< default velocity observation noise for exernal vision measurements (m/s) - (ParamFloat) - _param_ekf2_eva_noise, ///< default angular observation noise for exernal vision measurements (rad) - (ParamExtFloat) - _param_ekf2_evv_gate, ///< external vision velocity innovation consistency gate size (STD) - (ParamExtFloat) - _param_ekf2_evp_gate, ///< external vision position innovation consistency gate size (STD) - - // optical flow fusion - (ParamExtFloat) - _param_ekf2_of_n_min, ///< best quality observation noise for optical flow LOS rate measurements (rad/sec) - (ParamExtFloat) - _param_ekf2_of_n_max, ///< worst quality observation noise for optical flow LOS rate measurements (rad/sec) - (ParamExtInt) - _param_ekf2_of_qmin, ///< minimum acceptable quality integer from the flow sensor - (ParamExtFloat) - _param_ekf2_of_gate, ///< optical flow fusion innovation consistency gate size (STD) - - (ParamInt) _param_ekf2_imu_id, - - // sensor positions in body frame - (ParamExtFloat) _param_ekf2_imu_pos_x, ///< X position of IMU in body frame (m) - (ParamExtFloat) _param_ekf2_imu_pos_y, ///< Y position of IMU in body frame (m) - (ParamExtFloat) _param_ekf2_imu_pos_z, ///< Z position of IMU in body frame (m) - (ParamExtFloat) _param_ekf2_gps_pos_x, ///< X position of GPS antenna in body frame (m) - (ParamExtFloat) _param_ekf2_gps_pos_y, ///< Y position of GPS antenna in body frame (m) - (ParamExtFloat) _param_ekf2_gps_pos_z, ///< Z position of GPS antenna in body frame (m) - (ParamExtFloat) _param_ekf2_rng_pos_x, ///< X position of range finder in body frame (m) - (ParamExtFloat) _param_ekf2_rng_pos_y, ///< Y position of range finder in body frame (m) - (ParamExtFloat) _param_ekf2_rng_pos_z, ///< Z position of range finder in body frame (m) - (ParamExtFloat) - _param_ekf2_of_pos_x, ///< X position of optical flow sensor focal point in body frame (m) - (ParamExtFloat) - _param_ekf2_of_pos_y, ///< Y position of optical flow sensor focal point in body frame (m) - (ParamExtFloat) - _param_ekf2_of_pos_z, ///< Z position of optical flow sensor focal point in body frame (m) - (ParamExtFloat) - _param_ekf2_ev_pos_x, ///< X position of VI sensor focal point in body frame (m) - (ParamExtFloat) - _param_ekf2_ev_pos_y, ///< Y position of VI sensor focal point in body frame (m) - (ParamExtFloat) - _param_ekf2_ev_pos_z, ///< Z position of VI sensor focal point in body frame (m) - - // control of airspeed and sideslip fusion - (ParamFloat) - _param_ekf2_arsp_thr, ///< A value of zero will disabled airspeed fusion. Any positive value sets the minimum airspeed which will be used (m/sec) - (ParamInt) - _param_ekf2_fuse_beta, ///< Controls synthetic sideslip fusion, 0 disables, 1 enables - - // output predictor filter time constants - (ParamExtFloat) - _param_ekf2_tau_vel, ///< time constant used by the output velocity complementary filter (sec) - (ParamExtFloat) - _param_ekf2_tau_pos, ///< time constant used by the output position complementary filter (sec) - - // IMU switch on bias parameters - (ParamExtFloat) - _param_ekf2_gbias_init, ///< 1-sigma gyro bias uncertainty at switch on (rad/sec) - (ParamExtFloat) - _param_ekf2_abias_init, ///< 1-sigma accelerometer bias uncertainty at switch on (m/sec**2) - (ParamExtFloat) - _param_ekf2_angerr_init, ///< 1-sigma tilt error after initial alignment using gravity vector (rad) - - // EKF saved XYZ magnetometer bias values - (ParamFloat) _param_ekf2_magbias_x, ///< X magnetometer bias (mGauss) - (ParamFloat) _param_ekf2_magbias_y, ///< Y magnetometer bias (mGauss) - (ParamFloat) _param_ekf2_magbias_z, ///< Z magnetometer bias (mGauss) - (ParamInt) - _param_ekf2_magbias_id, ///< ID of the magnetometer sensor used to learn the bias values - (ParamFloat) - _param_ekf2_magb_vref, ///< Assumed error variance of previously saved magnetometer bias estimates (mGauss**2) - (ParamFloat) - _param_ekf2_magb_k, ///< maximum fraction of the learned magnetometer bias that is saved at each disarm - - // EKF accel bias learning control - (ParamExtFloat) _param_ekf2_abl_lim, ///< Accelerometer bias learning limit (m/s**2) - (ParamExtFloat) - _param_ekf2_abl_acclim, ///< Maximum IMU accel magnitude that allows IMU bias learning (m/s**2) - (ParamExtFloat) - _param_ekf2_abl_gyrlim, ///< Maximum IMU gyro angular rate magnitude that allows IMU bias learning (m/s**2) - (ParamExtFloat) - _param_ekf2_abl_tau, ///< Time constant used to inhibit IMU delta velocity bias learning (sec) - - // Multi-rotor drag specific force fusion - (ParamExtFloat) - _param_ekf2_drag_noise, ///< observation noise variance for drag specific force measurements (m/sec**2)**2 - (ParamExtFloat) _param_ekf2_bcoef_x, ///< ballistic coefficient along the X-axis (kg/m**2) - (ParamExtFloat) _param_ekf2_bcoef_y, ///< ballistic coefficient along the Y-axis (kg/m**2) - - // Corrections for static pressure position error where Ps_error = Ps_meas - Ps_truth - // Coef = Ps_error / Pdynamic, where Pdynamic = 1/2 * density * TAS**2 - (ParamExtFloat) - _param_ekf2_aspd_max, ///< upper limit on airspeed used for correction (m/s**2) - (ParamExtFloat) - _param_ekf2_pcoef_xp, ///< static pressure position error coefficient along the positive X body axis - (ParamExtFloat) - _param_ekf2_pcoef_xn, ///< static pressure position error coefficient along the negative X body axis - (ParamExtFloat) - _param_ekf2_pcoef_yp, ///< static pressure position error coefficient along the positive Y body axis - (ParamExtFloat) - _param_ekf2_pcoef_yn, ///< static pressure position error coefficient along the negative Y body axis - (ParamExtFloat) - _param_ekf2_pcoef_z, ///< static pressure position error coefficient along the Z body axis - - // GPS blending - (ParamInt) - _param_ekf2_gps_mask, ///< mask defining when GPS accuracy metrics are used to calculate the blend ratio - (ParamFloat) - _param_ekf2_gps_tau, ///< time constant controlling how rapidly the offset used to bring GPS solutions together is allowed to change (sec) - - // Test used to determine if the vehicle is static or moving - (ParamExtFloat) - _param_ekf2_move_test, ///< scaling applied to IMU data thresholds used to determine if the vehicle is static or moving. - - (ParamFloat) _param_ekf2_req_gps_h, ///< Required GPS health time - (ParamExtInt) _param_ekf2_mag_check, ///< Mag field strength check - - // Used by EKF-GSF experimental yaw estimator - (ParamExtFloat) - _param_ekf2_gsf_tas_default ///< default value of true airspeed assumed during fixed wing operation - - ) - -}; - -Ekf2::Ekf2(bool replay_mode): +EKF2::EKF2(bool replay_mode): ModuleParams(nullptr), ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers), _replay_mode(replay_mode), @@ -653,13 +149,13 @@ Ekf2::Ekf2(bool replay_mode): _ekf.set_min_required_gps_health_time(_param_ekf2_req_gps_h.get() * 1_s); } -Ekf2::~Ekf2() +EKF2::~EKF2() { px4_lockstep_unregister_component(_lockstep_component); perf_free(_ekf_update_perf); } -bool Ekf2::init() +bool EKF2::init() { const uint32_t device_id = _param_ekf2_imu_id.get(); @@ -695,7 +191,7 @@ bool Ekf2::init() return true; } -int Ekf2::print_status() +int EKF2::print_status() { PX4_INFO("local position: %s", (_ekf.local_position_is_valid()) ? "valid" : "invalid"); PX4_INFO("global position: %s", (_ekf.global_position_is_valid()) ? "valid" : "invalid"); @@ -708,7 +204,7 @@ int Ekf2::print_status() } template -void Ekf2::update_mag_bias(Param &mag_bias_param, int axis_index) +void EKF2::update_mag_bias(Param &mag_bias_param, int axis_index) { if (_valid_cal_available[axis_index]) { @@ -727,7 +223,7 @@ void Ekf2::update_mag_bias(Param &mag_bias_param, int axis_index) } template -bool Ekf2::update_mag_decl(Param &mag_decl_param) +bool EKF2::update_mag_decl(Param &mag_decl_param) { // update stored declination value float declination_deg; @@ -741,7 +237,7 @@ bool Ekf2::update_mag_decl(Param &mag_decl_param) return false; } -void Ekf2::Run() +void EKF2::Run() { if (should_exit()) { _sensor_combined_sub.unregisterCallback(); @@ -1738,7 +1234,7 @@ void Ekf2::Run() } } -void Ekf2::fillGpsMsgWithVehicleGpsPosData(gps_message &msg, const vehicle_gps_position_s &data) +void EKF2::fillGpsMsgWithVehicleGpsPosData(gps_message &msg, const vehicle_gps_position_s &data) { msg.time_usec = data.timestamp; msg.lat = data.lat; @@ -1759,7 +1255,7 @@ void Ekf2::fillGpsMsgWithVehicleGpsPosData(gps_message &msg, const vehicle_gps_p msg.pdop = sqrtf(data.hdop * data.hdop + data.vdop * data.vdop); } -void Ekf2::runPreFlightChecks(const float dt, +void EKF2::runPreFlightChecks(const float dt, const filter_control_status_u &control_status, const vehicle_status_s &vehicle_status, const estimator_innovations_s &innov) @@ -1775,12 +1271,12 @@ void Ekf2::runPreFlightChecks(const float dt, _preflt_checker.update(dt, innov); } -void Ekf2::resetPreFlightChecks() +void EKF2::resetPreFlightChecks() { _preflt_checker.reset(); } -int Ekf2::getRangeSubIndex() +int EKF2::getRangeSubIndex() { for (unsigned i = 0; i < _distance_sensor_subs.size(); i++) { distance_sensor_s report; @@ -1797,7 +1293,7 @@ int Ekf2::getRangeSubIndex() return -1; } -void Ekf2::publish_attitude(const hrt_abstime ×tamp) +void EKF2::publish_attitude(const hrt_abstime ×tamp) { if (_ekf.attitude_valid()) { // generate vehicle attitude quaternion data @@ -1819,7 +1315,7 @@ void Ekf2::publish_attitude(const hrt_abstime ×tamp) } } -void Ekf2::publish_yaw_estimator_status(const hrt_abstime ×tamp) +void EKF2::publish_yaw_estimator_status(const hrt_abstime ×tamp) { yaw_estimator_status_s yaw_est_test_data{}; @@ -1837,7 +1333,7 @@ void Ekf2::publish_yaw_estimator_status(const hrt_abstime ×tamp) } } -void Ekf2::publish_wind_estimate(const hrt_abstime ×tamp) +void EKF2::publish_wind_estimate(const hrt_abstime ×tamp) { if (_ekf.get_wind_status()) { // Publish wind estimate only if ekf declares them valid @@ -1859,7 +1355,7 @@ void Ekf2::publish_wind_estimate(const hrt_abstime ×tamp) } } -bool Ekf2::blend_gps_data() +bool EKF2::blend_gps_data() { // zero the blend weights memset(&_blend_weights, 0, sizeof(_blend_weights)); @@ -2115,7 +1611,7 @@ bool Ekf2::blend_gps_data() * because if physical receivers have significant position differences, variation in receiver estimated accuracy will * cause undesirable variation in the position solution. */ -void Ekf2::update_gps_blend_states() +void EKF2::update_gps_blend_states() { // initialise the blended states so we can accumulate the results using the weightings for each GPS receiver. _gps_blended_state.time_usec = 0; @@ -2262,7 +1758,7 @@ void Ekf2::update_gps_blend_states() * To mitigate this effect a low-pass filtered offset from each GPS location to the blended location is * calculated. */ -void Ekf2::update_gps_offsets() +void EKF2::update_gps_offsets() { // Calculate filter coefficients to be applied to the offsets for each GPS position and height offset @@ -2320,7 +1816,7 @@ void Ekf2::update_gps_offsets() /* * Apply the steady state physical receiver offsets calculated by update_gps_offsets(). */ -void Ekf2::apply_gps_offsets() +void EKF2::apply_gps_offsets() { // calculate offset corrected output for each physical GPS. for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) { @@ -2354,7 +1850,7 @@ void Ekf2::apply_gps_offsets() /* Calculate GPS output that is a blend of the offset corrected physical receiver data */ -void Ekf2::calc_gps_blend_output() +void EKF2::calc_gps_blend_output() { // Convert each GPS position to a local NEU offset relative to the reference position // which is defined as the positon of the blended solution calculated from non offset corrected data @@ -2410,7 +1906,7 @@ void Ekf2::calc_gps_blend_output() } -float Ekf2::filter_altitude_ellipsoid(float amsl_hgt) +float EKF2::filter_altitude_ellipsoid(float amsl_hgt) { float height_diff = static_cast(_gps_alttitude_ellipsoid[0]) * 1e-3f - amsl_hgt; @@ -2432,12 +1928,12 @@ float Ekf2::filter_altitude_ellipsoid(float amsl_hgt) return amsl_hgt + _wgs84_hgt_offset; } -int Ekf2::custom_command(int argc, char *argv[]) +int EKF2::custom_command(int argc, char *argv[]) { return print_usage("unknown command"); } -int Ekf2::task_spawn(int argc, char *argv[]) +int EKF2::task_spawn(int argc, char *argv[]) { bool replay_mode = false; @@ -2446,7 +1942,7 @@ int Ekf2::task_spawn(int argc, char *argv[]) replay_mode = true; } - Ekf2 *instance = new Ekf2(replay_mode); + EKF2 *instance = new EKF2(replay_mode); if (instance) { _object.store(instance); @@ -2467,7 +1963,7 @@ int Ekf2::task_spawn(int argc, char *argv[]) return PX4_ERROR; } -int Ekf2::print_usage(const char *reason) +int EKF2::print_usage(const char *reason) { if (reason) { PX4_WARN("%s\n", reason); @@ -2495,5 +1991,5 @@ timestamps from the sensor topics. extern "C" __EXPORT int ekf2_main(int argc, char *argv[]) { - return Ekf2::main(argc, argv); + return EKF2::main(argc, argv); } diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp new file mode 100644 index 0000000000..c8fb1e1d33 --- /dev/null +++ b/src/modules/ekf2/EKF2.hpp @@ -0,0 +1,538 @@ +/**************************************************************************** + * + * Copyright (c) 2015-2020 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file EKF2.cpp + * Implementation of the attitude and position estimator. + * + * @author Roman Bapst + */ + +#pragma once +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "Utility/PreFlightChecker.hpp" + +// defines used to specify the mask position for use of different accuracy metrics in the GPS blending algorithm +#define BLEND_MASK_USE_SPD_ACC 1 +#define BLEND_MASK_USE_HPOS_ACC 2 +#define BLEND_MASK_USE_VPOS_ACC 4 + +// define max number of GPS receivers supported and 0 base instance used to access virtual 'blended' GPS solution +#define GPS_MAX_RECEIVERS 2 +#define GPS_BLENDED_INSTANCE 2 + +using math::constrain; +using namespace time_literals; + +class EKF2 final : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem +{ +public: + explicit EKF2(bool replay_mode = false); + ~EKF2() override; + + /** @see ModuleBase */ + static int task_spawn(int argc, char *argv[]); + + /** @see ModuleBase */ + static int custom_command(int argc, char *argv[]); + + /** @see ModuleBase */ + static int print_usage(const char *reason = nullptr); + + bool init(); + + int print_status() override; + +private: + void Run() override; + + int getRangeSubIndex(); ///< get subscription index of first downward-facing range sensor + void fillGpsMsgWithVehicleGpsPosData(gps_message &msg, const vehicle_gps_position_s &data); + + PreFlightChecker _preflt_checker; + void runPreFlightChecks(float dt, const filter_control_status_u &control_status, + const vehicle_status_s &vehicle_status, + const estimator_innovations_s &innov); + void resetPreFlightChecks(); + + template + void update_mag_bias(Param &mag_bias_param, int axis_index); + + template + bool update_mag_decl(Param &mag_decl_param); + + void publish_attitude(const hrt_abstime ×tamp); + void publish_wind_estimate(const hrt_abstime ×tamp); + void publish_yaw_estimator_status(const hrt_abstime ×tamp); + + /* + * Update the internal state estimate for a blended GPS solution that is a weighted average of the phsyical + * receiver solutions. This internal state cannot be used directly by estimators because if physical receivers + * have significant position differences, variation in receiver estimated accuracy will cause undesirable + * variation in the position solution. + */ + bool blend_gps_data(); + + /* + * Calculate internal states used to blend GPS data from multiple receivers using weightings calculated + * by calc_blend_weights() + * States are written to _gps_state and _gps_blended_state class variables + */ + void update_gps_blend_states(); + + /* + * The location in _gps_blended_state will move around as the relative accuracy changes. + * To mitigate this effect a low-pass filtered offset from each GPS location to the blended location is + * calculated. + */ + void update_gps_offsets(); + + /* + * Apply the steady state physical receiver offsets calculated by update_gps_offsets(). + */ + void apply_gps_offsets(); + + /* + Calculate GPS output that is a blend of the offset corrected physical receiver data + */ + void calc_gps_blend_output(); + + /* + * Calculate filtered WGS84 height from estimated AMSL height + */ + float filter_altitude_ellipsoid(float amsl_hgt); + + inline float sq(float x) { return x * x; }; + + const bool _replay_mode; ///< true when we use replay data from a log + + // time slip monitoring + uint64_t _integrated_time_us = 0; ///< integral of gyro delta time from start (uSec) + uint64_t _start_time_us = 0; ///< system time at EKF start (uSec) + int64_t _last_time_slip_us = 0; ///< Last time slip (uSec) + + perf_counter_t _ekf_update_perf; + + // Initialise time stamps used to send sensor data to the EKF and for logging + uint8_t _invalid_mag_id_count = 0; ///< number of times an invalid magnetomer device ID has been detected + + // Used to check, save and use learned magnetometer biases + hrt_abstime _last_magcal_us = 0; ///< last time the EKF was operating a mode that estimates magnetomer biases (uSec) + hrt_abstime _total_cal_time_us = 0; ///< accumulated calibration time since the last save + + float _last_valid_mag_cal[3] = {}; ///< last valid XYZ magnetometer bias estimates (mGauss) + bool _valid_cal_available[3] = {}; ///< true when an unsaved valid calibration for the XYZ magnetometer bias is available + float _last_valid_variance[3] = {}; ///< variances for the last valid magnetometer XYZ bias estimates (mGauss**2) + + // Used to control saving of mag declination to be used on next startup + bool _mag_decl_saved = false; ///< true when the magnetic declination has been saved + + // set pose/velocity as invalid if standard deviation is bigger than max_std_dev + // TODO: the user should be allowed to set these values by a parameter + static constexpr float ep_max_std_dev = 100.0f; ///< Maximum permissible standard deviation for estimated position + static constexpr float eo_max_std_dev = 100.0f; ///< Maximum permissible standard deviation for estimated orientation + //static constexpr float ev_max_std_dev = 100.0f; ///< Maximum permissible standard deviation for estimated velocity + + // GPS blending and switching + gps_message _gps_state[GPS_MAX_RECEIVERS] {}; ///< internal state data for the physical GPS + gps_message _gps_blended_state{}; ///< internal state data for the blended GPS + gps_message _gps_output[GPS_MAX_RECEIVERS + 1] {}; ///< output state data for the physical and blended GPS + Vector2f _NE_pos_offset_m[GPS_MAX_RECEIVERS] = {}; ///< Filtered North,East position offset from GPS instance to blended solution in _output_state.location (m) + float _hgt_offset_mm[GPS_MAX_RECEIVERS] = {}; ///< Filtered height offset from GPS instance relative to blended solution in _output_state.location (mm) + Vector3f _blended_antenna_offset = {}; ///< blended antenna offset + float _blend_weights[GPS_MAX_RECEIVERS] = {}; ///< blend weight for each GPS. The blend weights must sum to 1.0 across all instances. + uint64_t _time_prev_us[GPS_MAX_RECEIVERS] = {}; ///< the previous value of time_us for that GPS instance - used to detect new data. + uint8_t _gps_best_index = 0; ///< index of the physical receiver with the lowest reported error + uint8_t _gps_select_index = 0; ///< 0 = GPS1, 1 = GPS2, 2 = blended + uint8_t _gps_time_ref_index = + 0; ///< index of the receiver that is used as the timing reference for the blending update + uint8_t _gps_oldest_index = 0; ///< index of the physical receiver with the oldest data + uint8_t _gps_newest_index = 0; ///< index of the physical receiver with the newest data + uint8_t _gps_slowest_index = 0; ///< index of the physical receiver with the slowest update rate + float _gps_dt[GPS_MAX_RECEIVERS] = {}; ///< average time step in seconds. + bool _gps_new_output_data = false; ///< true if there is new output data for the EKF + bool _had_valid_terrain = false; ///< true if at any time there was a valid terrain estimate + + int32_t _gps_alttitude_ellipsoid[GPS_MAX_RECEIVERS] {}; ///< altitude in 1E-3 meters (millimeters) above ellipsoid + uint64_t _gps_alttitude_ellipsoid_previous_timestamp[GPS_MAX_RECEIVERS] {}; ///< storage for previous timestamp to compute dt + float _wgs84_hgt_offset = 0; ///< height offset between AMSL and WGS84 + + bool _imu_bias_reset_request{false}; + + // republished aligned external visual odometry + bool new_ev_data_received = false; + vehicle_odometry_s _ev_odom{}; + + uORB::Subscription _airdata_sub{ORB_ID(vehicle_air_data)}; + uORB::Subscription _airspeed_sub{ORB_ID(airspeed)}; + uORB::Subscription _ev_odom_sub{ORB_ID(vehicle_visual_odometry)}; + uORB::Subscription _landing_target_pose_sub{ORB_ID(landing_target_pose)}; + uORB::Subscription _magnetometer_sub{ORB_ID(vehicle_magnetometer)}; + uORB::Subscription _optical_flow_sub{ORB_ID(optical_flow)}; + uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; + uORB::Subscription _sensor_selection_sub{ORB_ID(sensor_selection)}; + uORB::Subscription _status_sub{ORB_ID(vehicle_status)}; + 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}; + + // because we can have several distance sensor instances with different orientations + uORB::SubscriptionMultiArray _distance_sensor_subs{ORB_ID::distance_sensor}; + int _range_finder_sub_index = -1; // index for downward-facing range finder subscription + + // because we can have multiple GPS instances + uORB::Subscription _gps_subs[GPS_MAX_RECEIVERS] {{ORB_ID(vehicle_gps_position), 0}, {ORB_ID(vehicle_gps_position), 1}}; + + sensor_selection_s _sensor_selection{}; + vehicle_land_detected_s _vehicle_land_detected{}; + vehicle_status_s _vehicle_status{}; + + uORB::Publication _ekf2_timestamps_pub{ORB_ID(ekf2_timestamps)}; + uORB::Publication _ekf_gps_drift_pub{ORB_ID(ekf_gps_drift)}; + uORB::Publication _blended_gps_pub{ORB_ID(ekf_gps_position)}; + uORB::Publication _estimator_innovation_test_ratios_pub{ORB_ID(estimator_innovation_test_ratios)}; + uORB::Publication _estimator_innovation_variances_pub{ORB_ID(estimator_innovation_variances)}; + uORB::Publication _estimator_innovations_pub{ORB_ID(estimator_innovations)}; + uORB::Publication _estimator_sensor_bias_pub{ORB_ID(estimator_sensor_bias)}; + uORB::Publication _estimator_status_pub{ORB_ID(estimator_status)}; + uORB::Publication _att_pub{ORB_ID(vehicle_attitude)}; + uORB::Publication _vehicle_odometry_pub{ORB_ID(vehicle_odometry)}; + uORB::Publication _yaw_est_pub{ORB_ID(yaw_estimator_status)}; + uORB::PublicationData _vehicle_global_position_pub{ORB_ID(vehicle_global_position)}; + uORB::PublicationData _vehicle_local_position_pub{ORB_ID(vehicle_local_position)}; + uORB::PublicationData _vehicle_visual_odometry_aligned_pub{ORB_ID(vehicle_visual_odometry_aligned)}; + uORB::PublicationMulti _wind_pub{ORB_ID(wind_estimate)}; + + Ekf _ekf; + + parameters *_params; ///< pointer to ekf parameter struct (located in _ekf class instance) + + DEFINE_PARAMETERS( + (ParamExtInt) + _param_ekf2_min_obs_dt, ///< Maximum time delay of any sensor used to increase buffer length to handle large timing jitter (mSec) + (ParamExtFloat) + _param_ekf2_mag_delay, ///< magnetometer measurement delay relative to the IMU (mSec) + (ParamExtFloat) + _param_ekf2_baro_delay, ///< barometer height measurement delay relative to the IMU (mSec) + (ParamExtFloat) + _param_ekf2_gps_delay, ///< GPS measurement delay relative to the IMU (mSec) + (ParamExtFloat) + _param_ekf2_of_delay, ///< optical flow measurement delay relative to the IMU (mSec) - this is to the middle of the optical flow integration interval + (ParamExtFloat) + _param_ekf2_rng_delay, ///< range finder measurement delay relative to the IMU (mSec) + (ParamExtFloat) + _param_ekf2_asp_delay, ///< airspeed measurement delay relative to the IMU (mSec) + (ParamExtFloat) + _param_ekf2_ev_delay, ///< off-board vision measurement delay relative to the IMU (mSec) + (ParamExtFloat) + _param_ekf2_avel_delay, ///< auxillary velocity measurement delay relative to the IMU (mSec) + + (ParamExtFloat) + _param_ekf2_gyr_noise, ///< IMU angular rate noise used for covariance prediction (rad/sec) + (ParamExtFloat) + _param_ekf2_acc_noise, ///< IMU acceleration noise use for covariance prediction (m/sec**2) + + // process noise + (ParamExtFloat) + _param_ekf2_gyr_b_noise, ///< process noise for IMU rate gyro bias prediction (rad/sec**2) + (ParamExtFloat) + _param_ekf2_acc_b_noise,///< process noise for IMU accelerometer bias prediction (m/sec**3) + (ParamExtFloat) + _param_ekf2_mag_e_noise, ///< process noise for earth magnetic field prediction (Gauss/sec) + (ParamExtFloat) + _param_ekf2_mag_b_noise, ///< process noise for body magnetic field prediction (Gauss/sec) + (ParamExtFloat) + _param_ekf2_wind_noise, ///< process noise for wind velocity prediction (m/sec**2) + (ParamExtFloat) _param_ekf2_terr_noise, ///< process noise for terrain offset (m/sec) + (ParamExtFloat) + _param_ekf2_terr_grad, ///< gradient of terrain used to estimate process noise due to changing position (m/m) + + (ParamExtFloat) + _param_ekf2_gps_v_noise, ///< minimum allowed observation noise for gps velocity fusion (m/sec) + (ParamExtFloat) + _param_ekf2_gps_p_noise, ///< minimum allowed observation noise for gps position fusion (m) + (ParamExtFloat) + _param_ekf2_noaid_noise, ///< observation noise for non-aiding position fusion (m) + (ParamExtFloat) + _param_ekf2_baro_noise, ///< observation noise for barometric height fusion (m) + (ParamExtFloat) + _param_ekf2_baro_gate, ///< barometric height innovation consistency gate size (STD) + (ParamExtFloat) + _param_ekf2_gnd_eff_dz, ///< barometric deadzone range for negative innovations (m) + (ParamExtFloat) + _param_ekf2_gnd_max_hgt, ///< maximum height above the ground level for expected negative baro innovations (m) + (ParamExtFloat) + _param_ekf2_gps_p_gate, ///< GPS horizontal position innovation consistency gate size (STD) + (ParamExtFloat) + _param_ekf2_gps_v_gate, ///< GPS velocity innovation consistency gate size (STD) + (ParamExtFloat) + _param_ekf2_tas_gate, ///< True Airspeed innovation consistency gate size (STD) + + // control of magnetometer fusion + (ParamExtFloat) + _param_ekf2_head_noise, ///< measurement noise used for simple heading fusion (rad) + (ParamExtFloat) + _param_ekf2_mag_noise, ///< measurement noise used for 3-axis magnetoemeter fusion (Gauss) + (ParamExtFloat) + _param_ekf2_eas_noise, ///< measurement noise used for airspeed fusion (m/sec) + (ParamExtFloat) + _param_ekf2_beta_gate, ///< synthetic sideslip innovation consistency gate size (STD) + (ParamExtFloat) _param_ekf2_beta_noise, ///< synthetic sideslip noise (rad) + (ParamExtFloat) _param_ekf2_mag_decl,///< magnetic declination (degrees) + (ParamExtFloat) + _param_ekf2_hdg_gate,///< heading fusion innovation consistency gate size (STD) + (ParamExtFloat) + _param_ekf2_mag_gate, ///< magnetometer fusion innovation consistency gate size (STD) + (ParamExtInt) + _param_ekf2_decl_type, ///< bitmask used to control the handling of declination data + (ParamExtInt) + _param_ekf2_mag_type, ///< integer used to specify the type of magnetometer fusion used + (ParamExtFloat) + _param_ekf2_mag_acclim, ///< integer used to specify the type of magnetometer fusion used + (ParamExtFloat) + _param_ekf2_mag_yawlim, ///< yaw rate threshold used by mode select logic (rad/sec) + + (ParamExtInt) + _param_ekf2_gps_check, ///< bitmask used to control which GPS quality checks are used + (ParamExtFloat) _param_ekf2_req_eph, ///< maximum acceptable horiz position error (m) + (ParamExtFloat) _param_ekf2_req_epv, ///< maximum acceptable vert position error (m) + (ParamExtFloat) _param_ekf2_req_sacc, ///< maximum acceptable speed error (m/s) + (ParamExtInt) _param_ekf2_req_nsats, ///< minimum acceptable satellite count + (ParamExtFloat) + _param_ekf2_req_pdop, ///< maximum acceptable position dilution of precision + (ParamExtFloat) + _param_ekf2_req_hdrift, ///< maximum acceptable horizontal drift speed (m/s) + (ParamExtFloat) _param_ekf2_req_vdrift, ///< maximum acceptable vertical drift speed (m/s) + + // measurement source control + (ParamExtInt) + _param_ekf2_aid_mask, ///< bitmasked integer that selects which of the GPS and optical flow aiding sources will be used + (ParamExtInt) _param_ekf2_hgt_mode, ///< selects the primary source for height data + (ParamExtInt) + _param_ekf2_terr_mask, ///< bitmasked integer that selects which of range finder and optical flow aiding sources will be used for terrain estimation + (ParamExtInt) + _param_ekf2_noaid_tout, ///< maximum lapsed time from last fusion of measurements that constrain drift before the EKF will report the horizontal nav solution invalid (uSec) + + // range finder fusion + (ParamExtFloat) + _param_ekf2_rng_noise, ///< observation noise for range finder measurements (m) + (ParamExtFloat) _param_ekf2_rng_sfe, ///< scale factor from range to range noise (m/m) + (ParamExtFloat) + _param_ekf2_rng_gate, ///< range finder fusion innovation consistency gate size (STD) + (ParamExtFloat) _param_ekf2_min_rng, ///< minimum valid value for range when on ground (m) + (ParamExtFloat) _param_ekf2_rng_pitch, ///< range sensor pitch offset (rad) + (ParamExtInt) + _param_ekf2_rng_aid, ///< enables use of a range finder even if primary height source is not range finder + (ParamExtFloat) + _param_ekf2_rng_a_vmax, ///< maximum allowed horizontal velocity for range aid (m/s) + (ParamExtFloat) + _param_ekf2_rng_a_hmax, ///< maximum allowed absolute altitude (AGL) for range aid (m) + (ParamExtFloat) + _param_ekf2_rng_a_igate, ///< gate size used for innovation consistency checks for range aid fusion (STD) + + // vision estimate fusion + (ParamInt) + _param_ekf2_ev_noise_md, ///< determine source of vision observation noise + (ParamFloat) + _param_ekf2_evp_noise, ///< default position observation noise for exernal vision measurements (m) + (ParamFloat) + _param_ekf2_evv_noise, ///< default velocity observation noise for exernal vision measurements (m/s) + (ParamFloat) + _param_ekf2_eva_noise, ///< default angular observation noise for exernal vision measurements (rad) + (ParamExtFloat) + _param_ekf2_evv_gate, ///< external vision velocity innovation consistency gate size (STD) + (ParamExtFloat) + _param_ekf2_evp_gate, ///< external vision position innovation consistency gate size (STD) + + // optical flow fusion + (ParamExtFloat) + _param_ekf2_of_n_min, ///< best quality observation noise for optical flow LOS rate measurements (rad/sec) + (ParamExtFloat) + _param_ekf2_of_n_max, ///< worst quality observation noise for optical flow LOS rate measurements (rad/sec) + (ParamExtInt) + _param_ekf2_of_qmin, ///< minimum acceptable quality integer from the flow sensor + (ParamExtFloat) + _param_ekf2_of_gate, ///< optical flow fusion innovation consistency gate size (STD) + + (ParamInt) _param_ekf2_imu_id, + + // sensor positions in body frame + (ParamExtFloat) _param_ekf2_imu_pos_x, ///< X position of IMU in body frame (m) + (ParamExtFloat) _param_ekf2_imu_pos_y, ///< Y position of IMU in body frame (m) + (ParamExtFloat) _param_ekf2_imu_pos_z, ///< Z position of IMU in body frame (m) + (ParamExtFloat) _param_ekf2_gps_pos_x, ///< X position of GPS antenna in body frame (m) + (ParamExtFloat) _param_ekf2_gps_pos_y, ///< Y position of GPS antenna in body frame (m) + (ParamExtFloat) _param_ekf2_gps_pos_z, ///< Z position of GPS antenna in body frame (m) + (ParamExtFloat) _param_ekf2_rng_pos_x, ///< X position of range finder in body frame (m) + (ParamExtFloat) _param_ekf2_rng_pos_y, ///< Y position of range finder in body frame (m) + (ParamExtFloat) _param_ekf2_rng_pos_z, ///< Z position of range finder in body frame (m) + (ParamExtFloat) + _param_ekf2_of_pos_x, ///< X position of optical flow sensor focal point in body frame (m) + (ParamExtFloat) + _param_ekf2_of_pos_y, ///< Y position of optical flow sensor focal point in body frame (m) + (ParamExtFloat) + _param_ekf2_of_pos_z, ///< Z position of optical flow sensor focal point in body frame (m) + (ParamExtFloat) + _param_ekf2_ev_pos_x, ///< X position of VI sensor focal point in body frame (m) + (ParamExtFloat) + _param_ekf2_ev_pos_y, ///< Y position of VI sensor focal point in body frame (m) + (ParamExtFloat) + _param_ekf2_ev_pos_z, ///< Z position of VI sensor focal point in body frame (m) + + // control of airspeed and sideslip fusion + (ParamFloat) + _param_ekf2_arsp_thr, ///< A value of zero will disabled airspeed fusion. Any positive value sets the minimum airspeed which will be used (m/sec) + (ParamInt) + _param_ekf2_fuse_beta, ///< Controls synthetic sideslip fusion, 0 disables, 1 enables + + // output predictor filter time constants + (ParamExtFloat) + _param_ekf2_tau_vel, ///< time constant used by the output velocity complementary filter (sec) + (ParamExtFloat) + _param_ekf2_tau_pos, ///< time constant used by the output position complementary filter (sec) + + // IMU switch on bias parameters + (ParamExtFloat) + _param_ekf2_gbias_init, ///< 1-sigma gyro bias uncertainty at switch on (rad/sec) + (ParamExtFloat) + _param_ekf2_abias_init, ///< 1-sigma accelerometer bias uncertainty at switch on (m/sec**2) + (ParamExtFloat) + _param_ekf2_angerr_init, ///< 1-sigma tilt error after initial alignment using gravity vector (rad) + + // EKF saved XYZ magnetometer bias values + (ParamFloat) _param_ekf2_magbias_x, ///< X magnetometer bias (mGauss) + (ParamFloat) _param_ekf2_magbias_y, ///< Y magnetometer bias (mGauss) + (ParamFloat) _param_ekf2_magbias_z, ///< Z magnetometer bias (mGauss) + (ParamInt) + _param_ekf2_magbias_id, ///< ID of the magnetometer sensor used to learn the bias values + (ParamFloat) + _param_ekf2_magb_vref, ///< Assumed error variance of previously saved magnetometer bias estimates (mGauss**2) + (ParamFloat) + _param_ekf2_magb_k, ///< maximum fraction of the learned magnetometer bias that is saved at each disarm + + // EKF accel bias learning control + (ParamExtFloat) _param_ekf2_abl_lim, ///< Accelerometer bias learning limit (m/s**2) + (ParamExtFloat) + _param_ekf2_abl_acclim, ///< Maximum IMU accel magnitude that allows IMU bias learning (m/s**2) + (ParamExtFloat) + _param_ekf2_abl_gyrlim, ///< Maximum IMU gyro angular rate magnitude that allows IMU bias learning (m/s**2) + (ParamExtFloat) + _param_ekf2_abl_tau, ///< Time constant used to inhibit IMU delta velocity bias learning (sec) + + // Multi-rotor drag specific force fusion + (ParamExtFloat) + _param_ekf2_drag_noise, ///< observation noise variance for drag specific force measurements (m/sec**2)**2 + (ParamExtFloat) _param_ekf2_bcoef_x, ///< ballistic coefficient along the X-axis (kg/m**2) + (ParamExtFloat) _param_ekf2_bcoef_y, ///< ballistic coefficient along the Y-axis (kg/m**2) + + // Corrections for static pressure position error where Ps_error = Ps_meas - Ps_truth + // Coef = Ps_error / Pdynamic, where Pdynamic = 1/2 * density * TAS**2 + (ParamExtFloat) + _param_ekf2_aspd_max, ///< upper limit on airspeed used for correction (m/s**2) + (ParamExtFloat) + _param_ekf2_pcoef_xp, ///< static pressure position error coefficient along the positive X body axis + (ParamExtFloat) + _param_ekf2_pcoef_xn, ///< static pressure position error coefficient along the negative X body axis + (ParamExtFloat) + _param_ekf2_pcoef_yp, ///< static pressure position error coefficient along the positive Y body axis + (ParamExtFloat) + _param_ekf2_pcoef_yn, ///< static pressure position error coefficient along the negative Y body axis + (ParamExtFloat) + _param_ekf2_pcoef_z, ///< static pressure position error coefficient along the Z body axis + + // GPS blending + (ParamInt) + _param_ekf2_gps_mask, ///< mask defining when GPS accuracy metrics are used to calculate the blend ratio + (ParamFloat) + _param_ekf2_gps_tau, ///< time constant controlling how rapidly the offset used to bring GPS solutions together is allowed to change (sec) + + // Test used to determine if the vehicle is static or moving + (ParamExtFloat) + _param_ekf2_move_test, ///< scaling applied to IMU data thresholds used to determine if the vehicle is static or moving. + + (ParamFloat) _param_ekf2_req_gps_h, ///< Required GPS health time + (ParamExtInt) _param_ekf2_mag_check, ///< Mag field strength check + + // Used by EKF-GSF experimental yaw estimator + (ParamExtFloat) + _param_ekf2_gsf_tas_default ///< default value of true airspeed assumed during fixed wing operation + + ) +}; diff --git a/src/modules/ekf2/Utility/CMakeLists.txt b/src/modules/ekf2/Utility/CMakeLists.txt index df1c33ebfe..7a635acebe 100644 --- a/src/modules/ekf2/Utility/CMakeLists.txt +++ b/src/modules/ekf2/Utility/CMakeLists.txt @@ -31,15 +31,15 @@ # ############################################################################# -px4_add_library(Ekf2Utility +px4_add_library(EKF2Utility PreFlightChecker.cpp ) -target_include_directories(Ekf2Utility +target_include_directories(EKF2Utility PUBLIC ${CMAKE_CURRENT_SOURCE_DIR} ) -target_link_libraries(Ekf2Utility PRIVATE mathlib) +target_link_libraries(EKF2Utility PRIVATE mathlib) -px4_add_unit_gtest(SRC PreFlightCheckerTest.cpp LINKLIBS Ekf2Utility) +px4_add_unit_gtest(SRC PreFlightCheckerTest.cpp LINKLIBS EKF2Utility)