zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Parameter update - Rename variables in modules/ekf2 

using parameter_update.py followed by a make format
sbg
bresch 6 years ago committed by Matthias Grob
parent
da2deaa0b2
commit
986c1a37d1
1 changed files with 273 additions and 255 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 528
      src/modules/ekf2/ekf2_main.cpp

528
src/modules/ekf2/ekf2_main.cpp
Unescape View File

@ -291,220 +291,236 @@ private: @@ -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(): @@ -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) @@ -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() @@ -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() @@ -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() @@ -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() @@ -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() @@ -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() @@ -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() @@ -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() @@ -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() @@ -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() @@ -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() @@ -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() @@ -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() @@ -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() @@ -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() @@ -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() @@ -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() @@ -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() @@ -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() @@ -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() @@ -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() @@ -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) {

Write Preview
Loading…
Cancel
Save