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

Parameter update - Rename variables in modules/ekf2

Browse Source 
using parameter_update.py followed by a make format
This commit is contained in:
bresch
2019-04-03 15:38:50 +02:00
committed by Matthias Grob
parent da2deaa0b2
commit 986c1a37d1
1 changed files with 273 additions and 255 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/ekf2_main.cpp
+273 -255
View File
@@ -291,220 +291,236 @@ private:
DEFINE_PARAMETERS(
(ParamExtInt<px4::params::EKF2_MIN_OBS_DT>)
_obs_dt_min_ms, ///< Maximum time delay of any sensor used to increase buffer length to handle large timing jitter (mSec)
_param_ekf2_min_obs_dt, ///< Maximum time delay of any sensor used to increase buffer length to handle large timing jitter (mSec)
(ParamExtFloat<px4::params::EKF2_MAG_DELAY>)
_mag_delay_ms, ///< magnetometer measurement delay relative to the IMU (mSec)
_param_ekf2_mag_delay, ///< magnetometer measurement delay relative to the IMU (mSec)
(ParamExtFloat<px4::params::EKF2_BARO_DELAY>)
_baro_delay_ms, ///< barometer height measurement delay relative to the IMU (mSec)
(ParamExtFloat<px4::params::EKF2_GPS_DELAY>) _gps_delay_ms, ///< GPS measurement delay relative to the IMU (mSec)
_param_ekf2_baro_delay, ///< barometer height measurement delay relative to the IMU (mSec)
(ParamExtFloat<px4::params::EKF2_GPS_DELAY>)
_param_ekf2_gps_delay, ///< GPS measurement delay relative to the IMU (mSec)
(ParamExtFloat<px4::params::EKF2_OF_DELAY>)
_flow_delay_ms, ///< optical flow measurement delay relative to the IMU (mSec) - this is to the middle of the optical flow integration interval
_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<px4::params::EKF2_RNG_DELAY>)
_rng_delay_ms, ///< range finder measurement delay relative to the IMU (mSec)
_param_ekf2_rng_delay, ///< range finder measurement delay relative to the IMU (mSec)
(ParamExtFloat<px4::params::EKF2_ASP_DELAY>)
_airspeed_delay_ms, ///< airspeed measurement delay relative to the IMU (mSec)
_param_ekf2_asp_delay, ///< airspeed measurement delay relative to the IMU (mSec)
(ParamExtFloat<px4::params::EKF2_EV_DELAY>)
_ev_delay_ms, ///< off-board vision measurement delay relative to the IMU (mSec)
_param_ekf2_ev_delay, ///< off-board vision measurement delay relative to the IMU (mSec)
(ParamExtFloat<px4::params::EKF2_AVEL_DELAY>)
_auxvel_delay_ms, ///< auxillary velocity measurement delay relative to the IMU (mSec)
_param_ekf2_avel_delay, ///< auxillary velocity measurement delay relative to the IMU (mSec)
(ParamExtFloat<px4::params::EKF2_GYR_NOISE>)
_gyro_noise, ///< IMU angular rate noise used for covariance prediction (rad/sec)
_param_ekf2_gyr_noise, ///< IMU angular rate noise used for covariance prediction (rad/sec)
(ParamExtFloat<px4::params::EKF2_ACC_NOISE>)
_accel_noise, ///< IMU acceleration noise use for covariance prediction (m/sec**2)
_param_ekf2_acc_noise, ///< IMU acceleration noise use for covariance prediction (m/sec**2)
// process noise
(ParamExtFloat<px4::params::EKF2_GYR_B_NOISE>)
_gyro_bias_p_noise, ///< process noise for IMU rate gyro bias prediction (rad/sec**2)
_param_ekf2_gyr_b_noise, ///< process noise for IMU rate gyro bias prediction (rad/sec**2)
(ParamExtFloat<px4::params::EKF2_ACC_B_NOISE>)
_accel_bias_p_noise,///< process noise for IMU accelerometer bias prediction (m/sec**3)
_param_ekf2_acc_b_noise,///< process noise for IMU accelerometer bias prediction (m/sec**3)
(ParamExtFloat<px4::params::EKF2_MAG_E_NOISE>)
_mage_p_noise, ///< process noise for earth magnetic field prediction (Gauss/sec)
_param_ekf2_mag_e_noise, ///< process noise for earth magnetic field prediction (Gauss/sec)
(ParamExtFloat<px4::params::EKF2_MAG_B_NOISE>)
_magb_p_noise, ///< process noise for body magnetic field prediction (Gauss/sec)
_param_ekf2_mag_b_noise, ///< process noise for body magnetic field prediction (Gauss/sec)
(ParamExtFloat<px4::params::EKF2_WIND_NOISE>)
_wind_vel_p_noise, ///< process noise for wind velocity prediction (m/sec**2)
(ParamExtFloat<px4::params::EKF2_TERR_NOISE>) _terrain_p_noise, ///< process noise for terrain offset (m/sec)
_param_ekf2_wind_noise, ///< process noise for wind velocity prediction (m/sec**2)
(ParamExtFloat<px4::params::EKF2_TERR_NOISE>) _param_ekf2_terr_noise, ///< process noise for terrain offset (m/sec)
(ParamExtFloat<px4::params::EKF2_TERR_GRAD>)
_terrain_gradient, ///< gradient of terrain used to estimate process noise due to changing position (m/m)
_param_ekf2_terr_grad, ///< gradient of terrain used to estimate process noise due to changing position (m/m)
(ParamExtFloat<px4::params::EKF2_GPS_V_NOISE>)
_gps_vel_noise, ///< minimum allowed observation noise for gps velocity fusion (m/sec)
_param_ekf2_gps_v_noise, ///< minimum allowed observation noise for gps velocity fusion (m/sec)
(ParamExtFloat<px4::params::EKF2_GPS_P_NOISE>)
_gps_pos_noise, ///< minimum allowed observation noise for gps position fusion (m)
_param_ekf2_gps_p_noise, ///< minimum allowed observation noise for gps position fusion (m)
(ParamExtFloat<px4::params::EKF2_NOAID_NOISE>)
_pos_noaid_noise, ///< observation noise for non-aiding position fusion (m)
(ParamExtFloat<px4::params::EKF2_BARO_NOISE>) _baro_noise, ///< observation noise for barometric height fusion (m)
_param_ekf2_noaid_noise, ///< observation noise for non-aiding position fusion (m)
(ParamExtFloat<px4::params::EKF2_BARO_NOISE>)
_param_ekf2_baro_noise, ///< observation noise for barometric height fusion (m)
(ParamExtFloat<px4::params::EKF2_BARO_GATE>)
_baro_innov_gate, ///< barometric height innovation consistency gate size (STD)
_param_ekf2_baro_gate, ///< barometric height innovation consistency gate size (STD)
(ParamExtFloat<px4::params::EKF2_GND_EFF_DZ>)
_gnd_effect_deadzone, ///< barometric deadzone range for negative innovations (m)
_param_ekf2_gnd_eff_dz, ///< barometric deadzone range for negative innovations (m)
(ParamExtFloat<px4::params::EKF2_GND_MAX_HGT>)
_gnd_effect_max_hgt, ///< maximum height above the ground level for expected negative baro innovations (m)
_param_ekf2_gnd_max_hgt, ///< maximum height above the ground level for expected negative baro innovations (m)
(ParamExtFloat<px4::params::EKF2_GPS_P_GATE>)
_posNE_innov_gate, ///< GPS horizontal position innovation consistency gate size (STD)
(ParamExtFloat<px4::params::EKF2_GPS_V_GATE>) _vel_innov_gate, ///< GPS velocity innovation consistency gate size (STD)
(ParamExtFloat<px4::params::EKF2_TAS_GATE>) _tas_innov_gate, ///< True Airspeed innovation consistency gate size (STD)
_param_ekf2_gps_p_gate, ///< GPS horizontal position innovation consistency gate size (STD)
(ParamExtFloat<px4::params::EKF2_GPS_V_GATE>)
_param_ekf2_gps_v_gate, ///< GPS velocity innovation consistency gate size (STD)
(ParamExtFloat<px4::params::EKF2_TAS_GATE>)
_param_ekf2_tas_gate, ///< True Airspeed innovation consistency gate size (STD)
// control of magnetometer fusion
(ParamExtFloat<px4::params::EKF2_HEAD_NOISE>)
_mag_heading_noise, ///< measurement noise used for simple heading fusion (rad)
_param_ekf2_head_noise, ///< measurement noise used for simple heading fusion (rad)
(ParamExtFloat<px4::params::EKF2_MAG_NOISE>)
_mag_noise, ///< measurement noise used for 3-axis magnetoemeter fusion (Gauss)
(ParamExtFloat<px4::params::EKF2_EAS_NOISE>) _eas_noise, ///< measurement noise used for airspeed fusion (m/sec)
_param_ekf2_mag_noise, ///< measurement noise used for 3-axis magnetoemeter fusion (Gauss)
(ParamExtFloat<px4::params::EKF2_EAS_NOISE>)
_param_ekf2_eas_noise, ///< measurement noise used for airspeed fusion (m/sec)
(ParamExtFloat<px4::params::EKF2_BETA_GATE>)
_beta_innov_gate, ///< synthetic sideslip innovation consistency gate size (STD)
(ParamExtFloat<px4::params::EKF2_BETA_NOISE>) _beta_noise, ///< synthetic sideslip noise (rad)
(ParamExtFloat<px4::params::EKF2_MAG_DECL>) _mag_declination_deg,///< magnetic declination (degrees)
_param_ekf2_beta_gate, ///< synthetic sideslip innovation consistency gate size (STD)
(ParamExtFloat<px4::params::EKF2_BETA_NOISE>) _param_ekf2_beta_noise, ///< synthetic sideslip noise (rad)
(ParamExtFloat<px4::params::EKF2_MAG_DECL>) _param_ekf2_mag_decl,///< magnetic declination (degrees)
(ParamExtFloat<px4::params::EKF2_HDG_GATE>)
_heading_innov_gate,///< heading fusion innovation consistency gate size (STD)
_param_ekf2_hdg_gate,///< heading fusion innovation consistency gate size (STD)
(ParamExtFloat<px4::params::EKF2_MAG_GATE>)
_mag_innov_gate, ///< magnetometer fusion innovation consistency gate size (STD)
_param_ekf2_mag_gate, ///< magnetometer fusion innovation consistency gate size (STD)
(ParamExtInt<px4::params::EKF2_DECL_TYPE>)
_mag_decl_source, ///< bitmask used to control the handling of declination data
_param_ekf2_decl_type, ///< bitmask used to control the handling of declination data
(ParamExtInt<px4::params::EKF2_MAG_TYPE>)
_mag_fuse_type, ///< integer used to specify the type of magnetometer fusion used
_param_ekf2_mag_type, ///< integer used to specify the type of magnetometer fusion used
(ParamExtFloat<px4::params::EKF2_MAG_ACCLIM>)
_mag_acc_gate, ///< integer used to specify the type of magnetometer fusion used
_param_ekf2_mag_acclim, ///< integer used to specify the type of magnetometer fusion used
(ParamExtFloat<px4::params::EKF2_MAG_YAWLIM>)
_mag_yaw_rate_gate, ///< yaw rate threshold used by mode select logic (rad/sec)
_param_ekf2_mag_yawlim, ///< yaw rate threshold used by mode select logic (rad/sec)
(ParamExtInt<px4::params::EKF2_GPS_CHECK>)
_gps_check_mask, ///< bitmask used to control which GPS quality checks are used
(ParamExtFloat<px4::params::EKF2_REQ_EPH>) _requiredEph, ///< maximum acceptable horiz position error (m)
(ParamExtFloat<px4::params::EKF2_REQ_EPV>) _requiredEpv, ///< maximum acceptable vert position error (m)
(ParamExtFloat<px4::params::EKF2_REQ_SACC>) _requiredSacc, ///< maximum acceptable speed error (m/s)
(ParamExtInt<px4::params::EKF2_REQ_NSATS>) _requiredNsats, ///< minimum acceptable satellite count
(ParamExtFloat<px4::params::EKF2_REQ_GDOP>) _requiredGDoP, ///< maximum acceptable geometric dilution of precision
(ParamExtFloat<px4::params::EKF2_REQ_HDRIFT>) _requiredHdrift, ///< maximum acceptable horizontal drift speed (m/s)
(ParamExtFloat<px4::params::EKF2_REQ_VDRIFT>) _requiredVdrift, ///< maximum acceptable vertical drift speed (m/s)
_param_ekf2_gps_check, ///< bitmask used to control which GPS quality checks are used
(ParamExtFloat<px4::params::EKF2_REQ_EPH>) _param_ekf2_req_eph, ///< maximum acceptable horiz position error (m)
(ParamExtFloat<px4::params::EKF2_REQ_EPV>) _param_ekf2_req_epv, ///< maximum acceptable vert position error (m)
(ParamExtFloat<px4::params::EKF2_REQ_SACC>) _param_ekf2_req_sacc, ///< maximum acceptable speed error (m/s)
(ParamExtInt<px4::params::EKF2_REQ_NSATS>) _param_ekf2_req_nsats, ///< minimum acceptable satellite count
(ParamExtFloat<px4::params::EKF2_REQ_GDOP>)
_param_ekf2_req_gdop, ///< maximum acceptable geometric dilution of precision
(ParamExtFloat<px4::params::EKF2_REQ_HDRIFT>)
_param_ekf2_req_hdrift, ///< maximum acceptable horizontal drift speed (m/s)
(ParamExtFloat<px4::params::EKF2_REQ_VDRIFT>) _param_ekf2_req_vdrift, ///< maximum acceptable vertical drift speed (m/s)
// measurement source control
(ParamExtInt<px4::params::EKF2_AID_MASK>)
_fusion_mode, ///< bitmasked integer that selects which of the GPS and optical flow aiding sources will be used
(ParamExtInt<px4::params::EKF2_HGT_MODE>) _vdist_sensor_type, ///< selects the primary source for height data
_param_ekf2_aid_mask, ///< bitmasked integer that selects which of the GPS and optical flow aiding sources will be used
(ParamExtInt<px4::params::EKF2_HGT_MODE>) _param_ekf2_hgt_mode, ///< selects the primary source for height data
(ParamExtInt<px4::params::EKF2_NOAID_TOUT>)
_valid_timeout_max, ///< maximum lapsed time from last fusion of measurements that constrain drift before the EKF will report the horizontal nav solution invalid (uSec)
_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<px4::params::EKF2_RNG_NOISE>) _range_noise, ///< observation noise for range finder measurements (m)
(ParamExtFloat<px4::params::EKF2_RNG_SFE>) _range_noise_scaler, ///< scale factor from range to range noise (m/m)
(ParamExtFloat<px4::params::EKF2_RNG_NOISE>)
_param_ekf2_rng_noise, ///< observation noise for range finder measurements (m)
(ParamExtFloat<px4::params::EKF2_RNG_SFE>) _param_ekf2_rng_sfe, ///< scale factor from range to range noise (m/m)
(ParamExtFloat<px4::params::EKF2_RNG_GATE>)
_range_innov_gate, ///< range finder fusion innovation consistency gate size (STD)
(ParamExtFloat<px4::params::EKF2_MIN_RNG>) _rng_gnd_clearance, ///< minimum valid value for range when on ground (m)
(ParamExtFloat<px4::params::EKF2_RNG_PITCH>) _rng_pitch_offset, ///< range sensor pitch offset (rad)
_param_ekf2_rng_gate, ///< range finder fusion innovation consistency gate size (STD)
(ParamExtFloat<px4::params::EKF2_MIN_RNG>) _param_ekf2_min_rng, ///< minimum valid value for range when on ground (m)
(ParamExtFloat<px4::params::EKF2_RNG_PITCH>) _param_ekf2_rng_pitch, ///< range sensor pitch offset (rad)
(ParamExtInt<px4::params::EKF2_RNG_AID>)
_rng_aid, ///< enables use of a range finder even if primary height source is not range finder
_param_ekf2_rng_aid, ///< enables use of a range finder even if primary height source is not range finder
(ParamExtFloat<px4::params::EKF2_RNG_A_VMAX>)
_rng_aid_hor_vel_max, ///< maximum allowed horizontal velocity for range aid (m/s)
_param_ekf2_rng_a_vmax, ///< maximum allowed horizontal velocity for range aid (m/s)
(ParamExtFloat<px4::params::EKF2_RNG_A_HMAX>)
_rng_aid_height_max, ///< maximum allowed absolute altitude (AGL) for range aid (m)
_param_ekf2_rng_a_hmax, ///< maximum allowed absolute altitude (AGL) for range aid (m)
(ParamExtFloat<px4::params::EKF2_RNG_A_IGATE>)
_rng_aid_innov_gate, ///< gate size used for innovation consistency checks for range aid fusion (STD)
_param_ekf2_rng_a_igate, ///< gate size used for innovation consistency checks for range aid fusion (STD)
// vision estimate fusion
(ParamFloat<px4::params::EKF2_EVP_NOISE>)
_ev_pos_noise, ///< default position observation noise for exernal vision measurements (m)
_param_ekf2_evp_noise, ///< default position observation noise for exernal vision measurements (m)
(ParamFloat<px4::params::EKF2_EVA_NOISE>)
_ev_ang_noise, ///< default angular observation noise for exernal vision measurements (rad)
_param_ekf2_eva_noise, ///< default angular observation noise for exernal vision measurements (rad)
(ParamExtFloat<px4::params::EKF2_EV_GATE>)
_ev_innov_gate, ///< external vision position innovation consistency gate size (STD)
_param_ekf2_ev_gate, ///< external vision position innovation consistency gate size (STD)
// optical flow fusion
(ParamExtFloat<px4::params::EKF2_OF_N_MIN>)
_flow_noise, ///< best quality observation noise for optical flow LOS rate measurements (rad/sec)
_param_ekf2_of_n_min, ///< best quality observation noise for optical flow LOS rate measurements (rad/sec)
(ParamExtFloat<px4::params::EKF2_OF_N_MAX>)
_flow_noise_qual_min, ///< worst quality observation noise for optical flow LOS rate measurements (rad/sec)
(ParamExtInt<px4::params::EKF2_OF_QMIN>) _flow_qual_min, ///< minimum acceptable quality integer from the flow sensor
_param_ekf2_of_n_max, ///< worst quality observation noise for optical flow LOS rate measurements (rad/sec)
(ParamExtInt<px4::params::EKF2_OF_QMIN>)
_param_ekf2_of_qmin, ///< minimum acceptable quality integer from the flow sensor
(ParamExtFloat<px4::params::EKF2_OF_GATE>)
_flow_innov_gate, ///< optical flow fusion innovation consistency gate size (STD)
_param_ekf2_of_gate, ///< optical flow fusion innovation consistency gate size (STD)
// sensor positions in body frame
(ParamExtFloat<px4::params::EKF2_IMU_POS_X>) _imu_pos_x, ///< X position of IMU in body frame (m)
(ParamExtFloat<px4::params::EKF2_IMU_POS_Y>) _imu_pos_y, ///< Y position of IMU in body frame (m)
(ParamExtFloat<px4::params::EKF2_IMU_POS_Z>) _imu_pos_z, ///< Z position of IMU in body frame (m)
(ParamExtFloat<px4::params::EKF2_GPS_POS_X>) _gps_pos_x, ///< X position of GPS antenna in body frame (m)
(ParamExtFloat<px4::params::EKF2_GPS_POS_Y>) _gps_pos_y, ///< Y position of GPS antenna in body frame (m)
(ParamExtFloat<px4::params::EKF2_GPS_POS_Z>) _gps_pos_z, ///< Z position of GPS antenna in body frame (m)
(ParamExtFloat<px4::params::EKF2_RNG_POS_X>) _rng_pos_x, ///< X position of range finder in body frame (m)
(ParamExtFloat<px4::params::EKF2_RNG_POS_Y>) _rng_pos_y, ///< Y position of range finder in body frame (m)
(ParamExtFloat<px4::params::EKF2_RNG_POS_Z>) _rng_pos_z, ///< Z position of range finder in body frame (m)
(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)
(ParamExtFloat<px4::params::EKF2_IMU_POS_Z>) _param_ekf2_imu_pos_z, ///< Z position of IMU in body frame (m)
(ParamExtFloat<px4::params::EKF2_GPS_POS_X>) _param_ekf2_gps_pos_x, ///< X position of GPS antenna in body frame (m)
(ParamExtFloat<px4::params::EKF2_GPS_POS_Y>) _param_ekf2_gps_pos_y, ///< Y position of GPS antenna in body frame (m)
(ParamExtFloat<px4::params::EKF2_GPS_POS_Z>) _param_ekf2_gps_pos_z, ///< Z position of GPS antenna in body frame (m)
(ParamExtFloat<px4::params::EKF2_RNG_POS_X>) _param_ekf2_rng_pos_x, ///< X position of range finder in body frame (m)
(ParamExtFloat<px4::params::EKF2_RNG_POS_Y>) _param_ekf2_rng_pos_y, ///< Y position of range finder in body frame (m)
(ParamExtFloat<px4::params::EKF2_RNG_POS_Z>) _param_ekf2_rng_pos_z, ///< Z position of range finder in body frame (m)
(ParamExtFloat<px4::params::EKF2_OF_POS_X>)
_flow_pos_x, ///< X position of optical flow sensor focal point in body frame (m)
_param_ekf2_of_pos_x, ///< X position of optical flow sensor focal point in body frame (m)
(ParamExtFloat<px4::params::EKF2_OF_POS_Y>)
_flow_pos_y, ///< Y position of optical flow sensor focal point in body frame (m)
_param_ekf2_of_pos_y, ///< Y position of optical flow sensor focal point in body frame (m)
(ParamExtFloat<px4::params::EKF2_OF_POS_Z>)
_flow_pos_z, ///< Z position of optical flow sensor focal point in body frame (m)
(ParamExtFloat<px4::params::EKF2_EV_POS_X>) _ev_odom_x, ///< X position of VI sensor focal point in body frame (m)
(ParamExtFloat<px4::params::EKF2_EV_POS_Y>) _ev_odom_y, ///< Y position of VI sensor focal point in body frame (m)
(ParamExtFloat<px4::params::EKF2_EV_POS_Z>) _ev_odom_z, ///< Z position of VI sensor focal point in body frame (m)
_param_ekf2_of_pos_z, ///< Z position of optical flow sensor focal point in body frame (m)
(ParamExtFloat<px4::params::EKF2_EV_POS_X>)
_param_ekf2_ev_pos_x, ///< X position of VI sensor focal point in body frame (m)
(ParamExtFloat<px4::params::EKF2_EV_POS_Y>)
_param_ekf2_ev_pos_y, ///< Y position of VI sensor focal point in body frame (m)
(ParamExtFloat<px4::params::EKF2_EV_POS_Z>)
_param_ekf2_ev_pos_z, ///< Z position of VI sensor focal point in body frame (m)
// control of airspeed and sideslip fusion
(ParamFloat<px4::params::EKF2_ARSP_THR>)
_arspFusionThreshold, ///< A value of zero will disabled airspeed fusion. Any positive value sets the minimum airspeed which will be used (m/sec)
(ParamInt<px4::params::EKF2_FUSE_BETA>) _fuseBeta, ///< Controls synthetic sideslip fusion, 0 disables, 1 enables
_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<px4::params::EKF2_FUSE_BETA>)
_param_ekf2_fuse_beta, ///< Controls synthetic sideslip fusion, 0 disables, 1 enables
// output predictor filter time constants
(ParamExtFloat<px4::params::EKF2_TAU_VEL>)
_tau_vel, ///< time constant used by the output velocity complementary filter (sec)
_param_ekf2_tau_vel, ///< time constant used by the output velocity complementary filter (sec)
(ParamExtFloat<px4::params::EKF2_TAU_POS>)
_tau_pos, ///< time constant used by the output position complementary filter (sec)
_param_ekf2_tau_pos, ///< time constant used by the output position complementary filter (sec)
// IMU switch on bias parameters
(ParamExtFloat<px4::params::EKF2_GBIAS_INIT>) _gyr_bias_init, ///< 1-sigma gyro bias uncertainty at switch on (rad/sec)
(ParamExtFloat<px4::params::EKF2_GBIAS_INIT>)
_param_ekf2_gbias_init, ///< 1-sigma gyro bias uncertainty at switch on (rad/sec)
(ParamExtFloat<px4::params::EKF2_ABIAS_INIT>)
_acc_bias_init, ///< 1-sigma accelerometer bias uncertainty at switch on (m/sec**2)
_param_ekf2_abias_init, ///< 1-sigma accelerometer bias uncertainty at switch on (m/sec**2)
(ParamExtFloat<px4::params::EKF2_ANGERR_INIT>)
_ang_err_init, ///< 1-sigma tilt error after initial alignment using gravity vector (rad)
_param_ekf2_angerr_init, ///< 1-sigma tilt error after initial alignment using gravity vector (rad)
// EKF saved XYZ magnetometer bias values
(ParamFloat<px4::params::EKF2_MAGBIAS_X>) _mag_bias_x, ///< X magnetometer bias (mGauss)
(ParamFloat<px4::params::EKF2_MAGBIAS_Y>) _mag_bias_y, ///< Y magnetometer bias (mGauss)
(ParamFloat<px4::params::EKF2_MAGBIAS_Z>) _mag_bias_z, ///< Z magnetometer bias (mGauss)
(ParamInt<px4::params::EKF2_MAGBIAS_ID>) _mag_bias_id, ///< ID of the magnetometer sensor used to learn the bias values
(ParamFloat<px4::params::EKF2_MAGBIAS_X>) _param_ekf2_magbias_x, ///< X magnetometer bias (mGauss)
(ParamFloat<px4::params::EKF2_MAGBIAS_Y>) _param_ekf2_magbias_y, ///< Y magnetometer bias (mGauss)
(ParamFloat<px4::params::EKF2_MAGBIAS_Z>) _param_ekf2_magbias_z, ///< Z magnetometer bias (mGauss)
(ParamInt<px4::params::EKF2_MAGBIAS_ID>)
_param_ekf2_magbias_id, ///< ID of the magnetometer sensor used to learn the bias values
(ParamFloat<px4::params::EKF2_MAGB_VREF>)
_mag_bias_saved_variance, ///< Assumed error variance of previously saved magnetometer bias estimates (mGauss**2)
_param_ekf2_magb_vref, ///< Assumed error variance of previously saved magnetometer bias estimates (mGauss**2)
(ParamFloat<px4::params::EKF2_MAGB_K>)
_mag_bias_alpha, ///< maximum fraction of the learned magnetometer bias that is saved at each disarm
_param_ekf2_magb_k, ///< maximum fraction of the learned magnetometer bias that is saved at each disarm
// EKF accel bias learning control
(ParamExtFloat<px4::params::EKF2_ABL_LIM>) _acc_bias_lim, ///< Accelerometer bias learning limit (m/s**2)
(ParamExtFloat<px4::params::EKF2_ABL_LIM>) _param_ekf2_abl_lim, ///< Accelerometer bias learning limit (m/s**2)
(ParamExtFloat<px4::params::EKF2_ABL_ACCLIM>)
_acc_bias_learn_acc_lim, ///< Maximum IMU accel magnitude that allows IMU bias learning (m/s**2)
_param_ekf2_abl_acclim, ///< Maximum IMU accel magnitude that allows IMU bias learning (m/s**2)
(ParamExtFloat<px4::params::EKF2_ABL_GYRLIM>)
_acc_bias_learn_gyr_lim, ///< Maximum IMU gyro angular rate magnitude that allows IMU bias learning (m/s**2)
_param_ekf2_abl_gyrlim, ///< Maximum IMU gyro angular rate magnitude that allows IMU bias learning (m/s**2)
(ParamExtFloat<px4::params::EKF2_ABL_TAU>)
_acc_bias_learn_tc, ///< Time constant used to inhibit IMU delta velocity bias learning (sec)
_param_ekf2_abl_tau, ///< Time constant used to inhibit IMU delta velocity bias learning (sec)
// Multi-rotor drag specific force fusion
(ParamExtFloat<px4::params::EKF2_DRAG_NOISE>)
_drag_noise, ///< observation noise variance for drag specific force measurements (m/sec**2)**2
(ParamExtFloat<px4::params::EKF2_BCOEF_X>) _bcoef_x, ///< ballistic coefficient along the X-axis (kg/m**2)
(ParamExtFloat<px4::params::EKF2_BCOEF_Y>) _bcoef_y, ///< ballistic coefficient along the Y-axis (kg/m**2)
_param_ekf2_drag_noise, ///< observation noise variance for drag specific force measurements (m/sec**2)**2
(ParamExtFloat<px4::params::EKF2_BCOEF_X>) _param_ekf2_bcoef_x, ///< ballistic coefficient along the X-axis (kg/m**2)
(ParamExtFloat<px4::params::EKF2_BCOEF_Y>) _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
(ParamFloat<px4::params::EKF2_ASPD_MAX>) _aspd_max, ///< upper limit on airspeed used for correction (m/s**2)
(ParamFloat<px4::params::EKF2_ASPD_MAX>)
_param_ekf2_aspd_max, ///< upper limit on airspeed used for correction (m/s**2)
(ParamFloat<px4::params::EKF2_PCOEF_XP>)
_K_pstatic_coef_xp, ///< static pressure position error coefficient along the positive X body axis
_param_ekf2_pcoef_xp, ///< static pressure position error coefficient along the positive X body axis
(ParamFloat<px4::params::EKF2_PCOEF_XN>)
_K_pstatic_coef_xn, ///< static pressure position error coefficient along the negative X body axis
_param_ekf2_pcoef_xn, ///< static pressure position error coefficient along the negative X body axis
(ParamFloat<px4::params::EKF2_PCOEF_YP>)
_K_pstatic_coef_yp, ///< static pressure position error coefficient along the positive Y body axis
_param_ekf2_pcoef_yp, ///< static pressure position error coefficient along the positive Y body axis
(ParamFloat<px4::params::EKF2_PCOEF_YN>)
_K_pstatic_coef_yn, ///< static pressure position error coefficient along the negative Y body axis
_param_ekf2_pcoef_yn, ///< static pressure position error coefficient along the negative Y body axis
(ParamFloat<px4::params::EKF2_PCOEF_Z>)
_K_pstatic_coef_z, ///< static pressure position error coefficient along the Z body axis
_param_ekf2_pcoef_z, ///< static pressure position error coefficient along the Z body axis
// GPS blending
(ParamInt<px4::params::EKF2_GPS_MASK>)
_gps_blend_mask, ///< mask defining when GPS accuracy metrics are used to calculate the blend ratio
_param_ekf2_gps_mask, ///< mask defining when GPS accuracy metrics are used to calculate the blend ratio
(ParamFloat<px4::params::EKF2_GPS_TAU>)
_gps_blend_tau, ///< time constant controlling how rapidly the offset used to bring GPS solutions together is allowed to change (sec)
_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<px4::params::EKF2_MOVE_TEST>)
_is_moving_scaler ///< scaling applied to IMU data thresholds used to determine if the vehicle is static or moving.
_param_ekf2_move_test ///< scaling applied to IMU data thresholds used to determine if the vehicle is static or moving.
)
@@ -518,99 +534,99 @@ Ekf2::Ekf2():
_vehicle_global_position_pub(ORB_ID(vehicle_global_position)),
_vehicle_odometry_pub(ORB_ID(vehicle_odometry)),
_params(_ekf.getParamHandle()),
_obs_dt_min_ms(_params->sensor_interval_min_ms),
_mag_delay_ms(_params->mag_delay_ms),
_baro_delay_ms(_params->baro_delay_ms),
_gps_delay_ms(_params->gps_delay_ms),
_flow_delay_ms(_params->flow_delay_ms),
_rng_delay_ms(_params->range_delay_ms),
_airspeed_delay_ms(_params->airspeed_delay_ms),
_ev_delay_ms(_params->ev_delay_ms),
_auxvel_delay_ms(_params->auxvel_delay_ms),
_gyro_noise(_params->gyro_noise),
_accel_noise(_params->accel_noise),
_gyro_bias_p_noise(_params->gyro_bias_p_noise),
_accel_bias_p_noise(_params->accel_bias_p_noise),
_mage_p_noise(_params->mage_p_noise),
_magb_p_noise(_params->magb_p_noise),
_wind_vel_p_noise(_params->wind_vel_p_noise),
_terrain_p_noise(_params->terrain_p_noise),
_terrain_gradient(_params->terrain_gradient),
_gps_vel_noise(_params->gps_vel_noise),
_gps_pos_noise(_params->gps_pos_noise),
_pos_noaid_noise(_params->pos_noaid_noise),
_baro_noise(_params->baro_noise),
_baro_innov_gate(_params->baro_innov_gate),
_gnd_effect_deadzone(_params->gnd_effect_deadzone),
_gnd_effect_max_hgt(_params->gnd_effect_max_hgt),
_posNE_innov_gate(_params->posNE_innov_gate),
_vel_innov_gate(_params->vel_innov_gate),
_tas_innov_gate(_params->tas_innov_gate),
_mag_heading_noise(_params->mag_heading_noise),
_mag_noise(_params->mag_noise),
_eas_noise(_params->eas_noise),
_beta_innov_gate(_params->beta_innov_gate),
_beta_noise(_params->beta_noise),
_mag_declination_deg(_params->mag_declination_deg),
_heading_innov_gate(_params->heading_innov_gate),
_mag_innov_gate(_params->mag_innov_gate),
_mag_decl_source(_params->mag_declination_source),
_mag_fuse_type(_params->mag_fusion_type),
_mag_acc_gate(_params->mag_acc_gate),
_mag_yaw_rate_gate(_params->mag_yaw_rate_gate),
_gps_check_mask(_params->gps_check_mask),
_requiredEph(_params->req_hacc),
_requiredEpv(_params->req_vacc),
_requiredSacc(_params->req_sacc),
_requiredNsats(_params->req_nsats),
_requiredGDoP(_params->req_gdop),
_requiredHdrift(_params->req_hdrift),
_requiredVdrift(_params->req_vdrift),
_fusion_mode(_params->fusion_mode),
_vdist_sensor_type(_params->vdist_sensor_type),
_valid_timeout_max(_params->valid_timeout_max),
_range_noise(_params->range_noise),
_range_noise_scaler(_params->range_noise_scaler),
_range_innov_gate(_params->range_innov_gate),
_rng_gnd_clearance(_params->rng_gnd_clearance),
_rng_pitch_offset(_params->rng_sens_pitch),
_rng_aid(_params->range_aid),
_rng_aid_hor_vel_max(_params->max_vel_for_range_aid),
_rng_aid_height_max(_params->max_hagl_for_range_aid),
_rng_aid_innov_gate(_params->range_aid_innov_gate),
_ev_innov_gate(_params->ev_innov_gate),
_flow_noise(_params->flow_noise),
_flow_noise_qual_min(_params->flow_noise_qual_min),
_flow_qual_min(_params->flow_qual_min),
_flow_innov_gate(_params->flow_innov_gate),
_imu_pos_x(_params->imu_pos_body(0)),
_imu_pos_y(_params->imu_pos_body(1)),
_imu_pos_z(_params->imu_pos_body(2)),
_gps_pos_x(_params->gps_pos_body(0)),
_gps_pos_y(_params->gps_pos_body(1)),
_gps_pos_z(_params->gps_pos_body(2)),
_rng_pos_x(_params->rng_pos_body(0)),
_rng_pos_y(_params->rng_pos_body(1)),
_rng_pos_z(_params->rng_pos_body(2)),
_flow_pos_x(_params->flow_pos_body(0)),
_flow_pos_y(_params->flow_pos_body(1)),
_flow_pos_z(_params->flow_pos_body(2)),
_ev_odom_x(_params->ev_pos_body(0)),
_ev_odom_y(_params->ev_pos_body(1)),
_ev_odom_z(_params->ev_pos_body(2)),
_tau_vel(_params->vel_Tau),
_tau_pos(_params->pos_Tau),
_gyr_bias_init(_params->switch_on_gyro_bias),
_acc_bias_init(_params->switch_on_accel_bias),
_ang_err_init(_params->initial_tilt_err),
_acc_bias_lim(_params->acc_bias_lim),
_acc_bias_learn_acc_lim(_params->acc_bias_learn_acc_lim),
_acc_bias_learn_gyr_lim(_params->acc_bias_learn_gyr_lim),
_acc_bias_learn_tc(_params->acc_bias_learn_tc),
_drag_noise(_params->drag_noise),
_bcoef_x(_params->bcoef_x),
_bcoef_y(_params->bcoef_y),
_is_moving_scaler(_params->is_moving_scaler)
_param_ekf2_min_obs_dt(_params->sensor_interval_min_ms),
_param_ekf2_mag_delay(_params->mag_delay_ms),
_param_ekf2_baro_delay(_params->baro_delay_ms),
_param_ekf2_gps_delay(_params->gps_delay_ms),
_param_ekf2_of_delay(_params->flow_delay_ms),
_param_ekf2_rng_delay(_params->range_delay_ms),
_param_ekf2_asp_delay(_params->airspeed_delay_ms),
_param_ekf2_ev_delay(_params->ev_delay_ms),
_param_ekf2_avel_delay(_params->auxvel_delay_ms),
_param_ekf2_gyr_noise(_params->gyro_noise),
_param_ekf2_acc_noise(_params->accel_noise),
_param_ekf2_gyr_b_noise(_params->gyro_bias_p_noise),
_param_ekf2_acc_b_noise(_params->accel_bias_p_noise),
_param_ekf2_mag_e_noise(_params->mage_p_noise),
_param_ekf2_mag_b_noise(_params->magb_p_noise),
_param_ekf2_wind_noise(_params->wind_vel_p_noise),
_param_ekf2_terr_noise(_params->terrain_p_noise),
_param_ekf2_terr_grad(_params->terrain_gradient),
_param_ekf2_gps_v_noise(_params->gps_vel_noise),
_param_ekf2_gps_p_noise(_params->gps_pos_noise),
_param_ekf2_noaid_noise(_params->pos_noaid_noise),
_param_ekf2_baro_noise(_params->baro_noise),
_param_ekf2_baro_gate(_params->baro_innov_gate),
_param_ekf2_gnd_eff_dz(_params->gnd_effect_deadzone),
_param_ekf2_gnd_max_hgt(_params->gnd_effect_max_hgt),
_param_ekf2_gps_p_gate(_params->posNE_innov_gate),
_param_ekf2_gps_v_gate(_params->vel_innov_gate),
_param_ekf2_tas_gate(_params->tas_innov_gate),
_param_ekf2_head_noise(_params->mag_heading_noise),
_param_ekf2_mag_noise(_params->mag_noise),
_param_ekf2_eas_noise(_params->eas_noise),
_param_ekf2_beta_gate(_params->beta_innov_gate),
_param_ekf2_beta_noise(_params->beta_noise),
_param_ekf2_mag_decl(_params->mag_declination_deg),
_param_ekf2_hdg_gate(_params->heading_innov_gate),
_param_ekf2_mag_gate(_params->mag_innov_gate),
_param_ekf2_decl_type(_params->mag_declination_source),
_param_ekf2_mag_type(_params->mag_fusion_type),
_param_ekf2_mag_acclim(_params->mag_acc_gate),
_param_ekf2_mag_yawlim(_params->mag_yaw_rate_gate),
_param_ekf2_gps_check(_params->gps_check_mask),
_param_ekf2_req_eph(_params->req_hacc),
_param_ekf2_req_epv(_params->req_vacc),
_param_ekf2_req_sacc(_params->req_sacc),
_param_ekf2_req_nsats(_params->req_nsats),
_param_ekf2_req_gdop(_params->req_gdop),
_param_ekf2_req_hdrift(_params->req_hdrift),
_param_ekf2_req_vdrift(_params->req_vdrift),
_param_ekf2_aid_mask(_params->fusion_mode),
_param_ekf2_hgt_mode(_params->vdist_sensor_type),
_param_ekf2_noaid_tout(_params->valid_timeout_max),
_param_ekf2_rng_noise(_params->range_noise),
_param_ekf2_rng_sfe(_params->range_noise_scaler),
_param_ekf2_rng_gate(_params->range_innov_gate),
_param_ekf2_min_rng(_params->rng_gnd_clearance),
_param_ekf2_rng_pitch(_params->rng_sens_pitch),
_param_ekf2_rng_aid(_params->range_aid),
_param_ekf2_rng_a_vmax(_params->max_vel_for_range_aid),
_param_ekf2_rng_a_hmax(_params->max_hagl_for_range_aid),
_param_ekf2_rng_a_igate(_params->range_aid_innov_gate),
_param_ekf2_ev_gate(_params->ev_innov_gate),
_param_ekf2_of_n_min(_params->flow_noise),
_param_ekf2_of_n_max(_params->flow_noise_qual_min),
_param_ekf2_of_qmin(_params->flow_qual_min),
_param_ekf2_of_gate(_params->flow_innov_gate),
_param_ekf2_imu_pos_x(_params->imu_pos_body(0)),
_param_ekf2_imu_pos_y(_params->imu_pos_body(1)),
_param_ekf2_imu_pos_z(_params->imu_pos_body(2)),
_param_ekf2_gps_pos_x(_params->gps_pos_body(0)),
_param_ekf2_gps_pos_y(_params->gps_pos_body(1)),
_param_ekf2_gps_pos_z(_params->gps_pos_body(2)),
_param_ekf2_rng_pos_x(_params->rng_pos_body(0)),
_param_ekf2_rng_pos_y(_params->rng_pos_body(1)),
_param_ekf2_rng_pos_z(_params->rng_pos_body(2)),
_param_ekf2_of_pos_x(_params->flow_pos_body(0)),
_param_ekf2_of_pos_y(_params->flow_pos_body(1)),
_param_ekf2_of_pos_z(_params->flow_pos_body(2)),
_param_ekf2_ev_pos_x(_params->ev_pos_body(0)),
_param_ekf2_ev_pos_y(_params->ev_pos_body(1)),
_param_ekf2_ev_pos_z(_params->ev_pos_body(2)),
_param_ekf2_tau_vel(_params->vel_Tau),
_param_ekf2_tau_pos(_params->pos_Tau),
_param_ekf2_gbias_init(_params->switch_on_gyro_bias),
_param_ekf2_abias_init(_params->switch_on_accel_bias),
_param_ekf2_angerr_init(_params->initial_tilt_err),
_param_ekf2_abl_lim(_params->acc_bias_lim),
_param_ekf2_abl_acclim(_params->acc_bias_learn_acc_lim),
_param_ekf2_abl_gyrlim(_params->acc_bias_learn_gyr_lim),
_param_ekf2_abl_tau(_params->acc_bias_learn_tc),
_param_ekf2_drag_noise(_params->drag_noise),
_param_ekf2_bcoef_x(_params->bcoef_x),
_param_ekf2_bcoef_y(_params->bcoef_y),
_param_ekf2_move_test(_params->is_moving_scaler)
{
_airdata_sub = orb_subscribe(ORB_ID(vehicle_air_data));
_airspeed_sub = orb_subscribe(ORB_ID(airspeed));
@@ -681,8 +697,8 @@ void Ekf2::update_mag_bias(Param &mag_bias_param, int axis_index)
if (_valid_cal_available[axis_index]) {
// calculate weighting using ratio of variances and update stored bias values
const float weighting = constrain(_mag_bias_saved_variance.get() / (_mag_bias_saved_variance.get() +
_last_valid_variance[axis_index]), 0.0f, _mag_bias_alpha.get());
const float weighting = constrain(_param_ekf2_magb_vref.get() / (_param_ekf2_magb_vref.get() +
_last_valid_variance[axis_index]), 0.0f, _param_ekf2_magb_k.get());
const float mag_bias_saved = mag_bias_param.get();
_last_valid_mag_cal[axis_index] = weighting * _last_valid_mag_cal[axis_index] + mag_bias_saved;
@@ -778,7 +794,7 @@ void Ekf2::run()
if (vehicle_status_updated) {
if (orb_copy(ORB_ID(vehicle_status), _status_sub, &vehicle_status) == PX4_OK) {
// only fuse synthetic sideslip measurements if conditions are met
_ekf.set_fuse_beta_flag(!vehicle_status.is_rotary_wing && (_fuseBeta.get() == 1));
_ekf.set_fuse_beta_flag(!vehicle_status.is_rotary_wing && (_param_ekf2_fuse_beta.get() == 1));
// let the EKF know if the vehicle motion is that of a fixed wing (forward flight only relative to wind)
_ekf.set_is_fixed_wing(!vehicle_status.is_rotary_wing);
@@ -840,7 +856,7 @@ void Ekf2::run()
// Do not reset parmameters when armed to prevent potential time slips casued by parameter set
// and notification events
// Check if there has been a persistant change in magnetometer ID
if (sensor_selection.mag_device_id != 0 && sensor_selection.mag_device_id != (uint32_t)_mag_bias_id.get()) {
if (sensor_selection.mag_device_id != 0 && sensor_selection.mag_device_id != (uint32_t)_param_ekf2_magbias_id.get()) {
if (_invalid_mag_id_count < 200) {
_invalid_mag_id_count++;
}
@@ -854,18 +870,18 @@ void Ekf2::run()
if ((vehicle_status.arming_state != vehicle_status_s::ARMING_STATE_ARMED) && (_invalid_mag_id_count > 100)) {
// the sensor ID used for the last saved mag bias is not confirmed to be the same as the current sensor ID
// this means we need to reset the learned bias values to zero
_mag_bias_x.set(0.f);
_mag_bias_x.commit_no_notification();
_mag_bias_y.set(0.f);
_mag_bias_y.commit_no_notification();
_mag_bias_z.set(0.f);
_mag_bias_z.commit_no_notification();
_mag_bias_id.set(sensor_selection.mag_device_id);
_mag_bias_id.commit();
_param_ekf2_magbias_x.set(0.f);
_param_ekf2_magbias_x.commit_no_notification();
_param_ekf2_magbias_y.set(0.f);
_param_ekf2_magbias_y.commit_no_notification();
_param_ekf2_magbias_z.set(0.f);
_param_ekf2_magbias_z.commit_no_notification();
_param_ekf2_magbias_id.set(sensor_selection.mag_device_id);
_param_ekf2_magbias_id.commit();
_invalid_mag_id_count = 0;
PX4_INFO("Mag sensor ID changed to %i", _mag_bias_id.get());
PX4_INFO("Mag sensor ID changed to %i", _param_ekf2_magbias_id.get());
}
// If the time last used by the EKF is less than specified, then accumulate the
@@ -880,9 +896,9 @@ void Ekf2::run()
if ((mag_time_ms - _mag_time_ms_last_used) > _params->sensor_interval_min_ms) {
const float mag_sample_count_inv = 1.0f / _mag_sample_count;
// calculate mean of measurements and correct for learned bias offsets
float mag_data_avg_ga[3] = {_mag_data_sum[0] *mag_sample_count_inv - _mag_bias_x.get(),
_mag_data_sum[1] *mag_sample_count_inv - _mag_bias_y.get(),
_mag_data_sum[2] *mag_sample_count_inv - _mag_bias_z.get()
float mag_data_avg_ga[3] = {_mag_data_sum[0] *mag_sample_count_inv - _param_ekf2_magbias_x.get(),
_mag_data_sum[1] *mag_sample_count_inv - _param_ekf2_magbias_y.get(),
_mag_data_sum[2] *mag_sample_count_inv - _param_ekf2_magbias_z.get()
};
_ekf.setMagData(1000 * (uint64_t)mag_time_ms, mag_data_avg_ga);
@@ -929,28 +945,28 @@ void Ekf2::run()
float K_pstatic_coef_x;
if (vel_body_wind(0) >= 0.0f) {
K_pstatic_coef_x = _K_pstatic_coef_xp.get();
K_pstatic_coef_x = _param_ekf2_pcoef_xp.get();
} else {
K_pstatic_coef_x = _K_pstatic_coef_xn.get();
K_pstatic_coef_x = _param_ekf2_pcoef_xn.get();
}
float K_pstatic_coef_y;
if (vel_body_wind(1) >= 0.0f) {
K_pstatic_coef_y = _K_pstatic_coef_yp.get();
K_pstatic_coef_y = _param_ekf2_pcoef_yp.get();
} else {
K_pstatic_coef_y = _K_pstatic_coef_yn.get();
K_pstatic_coef_y = _param_ekf2_pcoef_yn.get();
}
const float max_airspeed_sq = _aspd_max.get() * _aspd_max.get();
const float max_airspeed_sq = _param_ekf2_aspd_max.get() * _param_ekf2_aspd_max.get();
const float x_v2 = fminf(vel_body_wind(0) * vel_body_wind(0), max_airspeed_sq);
const float y_v2 = fminf(vel_body_wind(1) * vel_body_wind(1), max_airspeed_sq);
const float z_v2 = fminf(vel_body_wind(2) * vel_body_wind(2), max_airspeed_sq);
const float pstatic_err = 0.5f * airdata.rho *
(K_pstatic_coef_x * x_v2) + (K_pstatic_coef_y * y_v2) + (_K_pstatic_coef_z.get() * z_v2);
(K_pstatic_coef_x * x_v2) + (K_pstatic_coef_y * y_v2) + (_param_ekf2_pcoef_z.get() * z_v2);
// correct baro measurement using pressure error estimate and assuming sea level gravity
balt_data_avg += pstatic_err / (airdata.rho * CONSTANTS_ONE_G);
@@ -1031,11 +1047,11 @@ void Ekf2::run()
}
}
if ((_gps_blend_mask.get() == 0) && gps1_updated) {
if ((_param_ekf2_gps_mask.get() == 0) && gps1_updated) {
// When GPS blending is disabled we always use the first receiver instance
_ekf.setGpsData(_gps_state[0].time_usec, _gps_state[0]);
} else if ((_gps_blend_mask.get() > 0) && (gps1_updated || gps2_updated)) {
} else if ((_param_ekf2_gps_mask.get() > 0) && (gps1_updated || gps2_updated)) {
// blend dual receivers if available
// calculate blending weights
@@ -1116,7 +1132,7 @@ void Ekf2::run()
if (orb_copy(ORB_ID(airspeed), _airspeed_sub, &airspeed) == PX4_OK) {
// only set airspeed data if condition for airspeed fusion are met
if ((_arspFusionThreshold.get() > FLT_EPSILON) && (airspeed.true_airspeed_m_s > _arspFusionThreshold.get())) {
if ((_param_ekf2_arsp_thr.get() > FLT_EPSILON) && (airspeed.true_airspeed_m_s > _param_ekf2_arsp_thr.get())) {
const float eas2tas = airspeed.true_airspeed_m_s / airspeed.indicated_airspeed_m_s;
_ekf.setAirspeedData(airspeed.timestamp, airspeed.true_airspeed_m_s, eas2tas);
@@ -1176,7 +1192,7 @@ void Ekf2::run()
range_finder_updated = false;
} else {
range_finder.current_distance = _rng_gnd_clearance.get();
range_finder.current_distance = _param_ekf2_min_rng.get();
}
}
@@ -1214,13 +1230,15 @@ void Ekf2::run()
// position measurement error from parameters
if (PX4_ISFINITE(ev_odom.pose_covariance[ev_odom.COVARIANCE_MATRIX_X_VARIANCE])) {
ev_data.posErr = fmaxf(_ev_pos_noise.get(), sqrtf(fmaxf(ev_odom.pose_covariance[ev_odom.COVARIANCE_MATRIX_X_VARIANCE],
ev_odom.pose_covariance[ev_odom.COVARIANCE_MATRIX_Y_VARIANCE])));
ev_data.hgtErr = fmaxf(_ev_pos_noise.get(), sqrtf(ev_odom.pose_covariance[ev_odom.COVARIANCE_MATRIX_Z_VARIANCE]));
ev_data.posErr = fmaxf(_param_ekf2_evp_noise.get(),
sqrtf(fmaxf(ev_odom.pose_covariance[ev_odom.COVARIANCE_MATRIX_X_VARIANCE],
ev_odom.pose_covariance[ev_odom.COVARIANCE_MATRIX_Y_VARIANCE])));
ev_data.hgtErr = fmaxf(_param_ekf2_evp_noise.get(),
sqrtf(ev_odom.pose_covariance[ev_odom.COVARIANCE_MATRIX_Z_VARIANCE]));
} else {
ev_data.posErr = _ev_pos_noise.get();
ev_data.hgtErr = _ev_pos_noise.get();
ev_data.posErr = _param_ekf2_evp_noise.get();
ev_data.hgtErr = _param_ekf2_evp_noise.get();
}
}
@@ -1230,13 +1248,13 @@ void Ekf2::run()
// orientation measurement error from parameters
if (PX4_ISFINITE(ev_odom.pose_covariance[ev_odom.COVARIANCE_MATRIX_ROLL_VARIANCE])) {
ev_data.angErr = fmaxf(_ev_ang_noise.get(),
ev_data.angErr = fmaxf(_param_ekf2_eva_noise.get(),
sqrtf(fmaxf(ev_odom.pose_covariance[ev_odom.COVARIANCE_MATRIX_ROLL_VARIANCE],
fmaxf(ev_odom.pose_covariance[ev_odom.COVARIANCE_MATRIX_PITCH_VARIANCE],
ev_odom.pose_covariance[ev_odom.COVARIANCE_MATRIX_YAW_VARIANCE]))));
} else {
ev_data.angErr = _ev_ang_noise.get();
ev_data.angErr = _param_ekf2_eva_noise.get();
}
}
@@ -1392,17 +1410,17 @@ void Ekf2::run()
lpos.dist_bottom = terrain_vpos - lpos.z; // Distance to bottom surface (ground) in meters
// constrain the distance to ground to _rng_gnd_clearance
if (lpos.dist_bottom < _rng_gnd_clearance.get()) {
lpos.dist_bottom = _rng_gnd_clearance.get();
if (lpos.dist_bottom < _param_ekf2_min_rng.get()) {
lpos.dist_bottom = _param_ekf2_min_rng.get();
}
// update ground effect flag based on terrain estimation
if (lpos.dist_bottom_valid && lpos.dist_bottom < _gnd_effect_max_hgt.get()) {
if (lpos.dist_bottom_valid && lpos.dist_bottom < _param_ekf2_gnd_max_hgt.get()) {
_ekf.set_gnd_effect_flag(true);
}
// update ground effect flag based on land detector state
else if (vehicle_land_detected_updated && _gnd_effect_deadzone.get() > 0.0f) {
else if (vehicle_land_detected_updated && _param_ekf2_gnd_eff_dz.get() > 0.0f) {
_ekf.set_gnd_effect_flag(vehicle_land_detected.in_ground_effect);
}
@@ -1635,8 +1653,8 @@ void Ekf2::run()
if (_total_cal_time_us > 120_s) {
// we have sufficient accumulated valid flight time to form a reliable bias estimate
// check that the state variance for each axis is within a range indicating filter convergence
const float max_var_allowed = 100.0f * _mag_bias_saved_variance.get();
const float min_var_allowed = 0.01f * _mag_bias_saved_variance.get();
const float max_var_allowed = 100.0f * _param_ekf2_magb_vref.get();
const float min_var_allowed = 0.01f * _param_ekf2_magb_vref.get();
// Declare all bias estimates invalid if any variances are out of range
bool all_estimates_invalid = false;
@@ -1661,11 +1679,11 @@ void Ekf2::run()
// Check and save the last valid calibration when we are disarmed
if ((vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_STANDBY)
&& (status.filter_fault_flags == 0)
&& (sensor_selection.mag_device_id == (uint32_t)_mag_bias_id.get())) {
&& (sensor_selection.mag_device_id == (uint32_t)_param_ekf2_magbias_id.get())) {
update_mag_bias(_mag_bias_x, 0);
update_mag_bias(_mag_bias_y, 1);
update_mag_bias(_mag_bias_z, 2);
update_mag_bias(_param_ekf2_magbias_x, 0);
update_mag_bias(_param_ekf2_magbias_y, 1);
update_mag_bias(_param_ekf2_magbias_z, 2);
// reset to prevent data being saved too frequently
_total_cal_time_us = 0;
@@ -1676,7 +1694,7 @@ void Ekf2::run()
publish_wind_estimate(now);
if (!_mag_decl_saved && (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_STANDBY)) {
_mag_decl_saved = update_mag_decl(_mag_declination_deg);
_mag_decl_saved = update_mag_decl(_param_ekf2_mag_decl);
}
{
@@ -1979,7 +1997,7 @@ bool Ekf2::blend_gps_data()
// calculate the sum squared speed accuracy across all GPS sensors
float speed_accuracy_sum_sq = 0.0f;
if (_gps_blend_mask.get() & BLEND_MASK_USE_SPD_ACC) {
if (_param_ekf2_gps_mask.get() & BLEND_MASK_USE_SPD_ACC) {
for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
if (_gps_state[i].fix_type >= 3 && _gps_state[i].sacc > 0.0f) {
speed_accuracy_sum_sq += _gps_state[i].sacc * _gps_state[i].sacc;
@@ -1995,7 +2013,7 @@ bool Ekf2::blend_gps_data()
// calculate the sum squared horizontal position accuracy across all GPS sensors
float horizontal_accuracy_sum_sq = 0.0f;
if (_gps_blend_mask.get() & BLEND_MASK_USE_HPOS_ACC) {
if (_param_ekf2_gps_mask.get() & BLEND_MASK_USE_HPOS_ACC) {
for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
if (_gps_state[i].fix_type >= 2 && _gps_state[i].eph > 0.0f) {
horizontal_accuracy_sum_sq += _gps_state[i].eph * _gps_state[i].eph;
@@ -2011,7 +2029,7 @@ bool Ekf2::blend_gps_data()
// calculate the sum squared vertical position accuracy across all GPS sensors
float vertical_accuracy_sum_sq = 0.0f;
if (_gps_blend_mask.get() & BLEND_MASK_USE_VPOS_ACC) {
if (_param_ekf2_gps_mask.get() & BLEND_MASK_USE_VPOS_ACC) {
for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
if (_gps_state[i].fix_type >= 3 && _gps_state[i].epv > 0.0f) {
vertical_accuracy_sum_sq += _gps_state[i].epv * _gps_state[i].epv;
@@ -2038,7 +2056,7 @@ bool Ekf2::blend_gps_data()
// calculate a weighting using the reported speed accuracy
float spd_blend_weights[GPS_MAX_RECEIVERS] = {};
if (speed_accuracy_sum_sq > 0.0f && (_gps_blend_mask.get() & BLEND_MASK_USE_SPD_ACC)) {
if (speed_accuracy_sum_sq > 0.0f && (_param_ekf2_gps_mask.get() & BLEND_MASK_USE_SPD_ACC)) {
// calculate the weights using the inverse of the variances
float sum_of_spd_weights = 0.0f;
@@ -2062,7 +2080,7 @@ bool Ekf2::blend_gps_data()
// calculate a weighting using the reported horizontal position
float hpos_blend_weights[GPS_MAX_RECEIVERS] = {};
if (horizontal_accuracy_sum_sq > 0.0f && (_gps_blend_mask.get() & BLEND_MASK_USE_HPOS_ACC)) {
if (horizontal_accuracy_sum_sq > 0.0f && (_param_ekf2_gps_mask.get() & BLEND_MASK_USE_HPOS_ACC)) {
// calculate the weights using the inverse of the variances
float sum_of_hpos_weights = 0.0f;
@@ -2086,7 +2104,7 @@ bool Ekf2::blend_gps_data()
// calculate a weighting using the reported vertical position accuracy
float vpos_blend_weights[GPS_MAX_RECEIVERS] = {};
if (vertical_accuracy_sum_sq > 0.0f && (_gps_blend_mask.get() & BLEND_MASK_USE_VPOS_ACC)) {
if (vertical_accuracy_sum_sq > 0.0f && (_param_ekf2_gps_mask.get() & BLEND_MASK_USE_VPOS_ACC)) {
// calculate the weights using the inverse of the variances
float sum_of_vpos_weights = 0.0f;
@@ -2287,7 +2305,7 @@ void Ekf2::update_gps_offsets()
// Increase the filter time constant proportional to the inverse of the weighting
// A weighting of 1 will make the offset adjust the slowest, a weighting of 0 will make it adjust with zero filtering
float alpha[GPS_MAX_RECEIVERS] = {};
float omega_lpf = 1.0f / fmaxf(_gps_blend_tau.get(), 1.0f);
float omega_lpf = 1.0f / fmaxf(_param_ekf2_gps_tau.get(), 1.0f);
for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
if (_gps_state[i].time_usec - _time_prev_us[i] > 0) {