diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index dbdbbbc798..47ea7b44a2 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -291,220 +291,236 @@ private: DEFINE_PARAMETERS( (ParamExtInt) - _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) - _mag_delay_ms, ///< magnetometer measurement delay relative to the IMU (mSec) + _param_ekf2_mag_delay, ///< magnetometer measurement delay relative to the IMU (mSec) (ParamExtFloat) - _baro_delay_ms, ///< barometer height measurement delay relative to the IMU (mSec) - (ParamExtFloat) _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) + _param_ekf2_gps_delay, ///< GPS measurement delay relative to the IMU (mSec) (ParamExtFloat) - _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) - _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) - _airspeed_delay_ms, ///< airspeed measurement delay relative to the IMU (mSec) + _param_ekf2_asp_delay, ///< airspeed measurement delay relative to the IMU (mSec) (ParamExtFloat) - _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) - _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) - _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) - _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) - _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) - _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) - _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) - _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) - _wind_vel_p_noise, ///< process noise for wind velocity prediction (m/sec**2) - (ParamExtFloat) _terrain_p_noise, ///< process noise for terrain offset (m/sec) + _param_ekf2_wind_noise, ///< process noise for wind velocity prediction (m/sec**2) + (ParamExtFloat) _param_ekf2_terr_noise, ///< process noise for terrain offset (m/sec) (ParamExtFloat) - _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) - _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) - _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) - _pos_noaid_noise, ///< observation noise for non-aiding position fusion (m) - (ParamExtFloat) _baro_noise, ///< observation noise for barometric height fusion (m) + _param_ekf2_noaid_noise, ///< observation noise for non-aiding position fusion (m) + (ParamExtFloat) + _param_ekf2_baro_noise, ///< observation noise for barometric height fusion (m) (ParamExtFloat) - _baro_innov_gate, ///< barometric height innovation consistency gate size (STD) + _param_ekf2_baro_gate, ///< barometric height innovation consistency gate size (STD) (ParamExtFloat) - _gnd_effect_deadzone, ///< barometric deadzone range for negative innovations (m) + _param_ekf2_gnd_eff_dz, ///< barometric deadzone range for negative innovations (m) (ParamExtFloat) - _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) - _posNE_innov_gate, ///< GPS horizontal position innovation consistency gate size (STD) - (ParamExtFloat) _vel_innov_gate, ///< GPS velocity innovation consistency gate size (STD) - (ParamExtFloat) _tas_innov_gate, ///< True Airspeed innovation consistency gate size (STD) + _param_ekf2_gps_p_gate, ///< GPS horizontal position innovation consistency gate size (STD) + (ParamExtFloat) + _param_ekf2_gps_v_gate, ///< GPS velocity innovation consistency gate size (STD) + (ParamExtFloat) + _param_ekf2_tas_gate, ///< True Airspeed innovation consistency gate size (STD) // control of magnetometer fusion (ParamExtFloat) - _mag_heading_noise, ///< measurement noise used for simple heading fusion (rad) + _param_ekf2_head_noise, ///< measurement noise used for simple heading fusion (rad) (ParamExtFloat) - _mag_noise, ///< measurement noise used for 3-axis magnetoemeter fusion (Gauss) - (ParamExtFloat) _eas_noise, ///< measurement noise used for airspeed fusion (m/sec) + _param_ekf2_mag_noise, ///< measurement noise used for 3-axis magnetoemeter fusion (Gauss) + (ParamExtFloat) + _param_ekf2_eas_noise, ///< measurement noise used for airspeed fusion (m/sec) (ParamExtFloat) - _beta_innov_gate, ///< synthetic sideslip innovation consistency gate size (STD) - (ParamExtFloat) _beta_noise, ///< synthetic sideslip noise (rad) - (ParamExtFloat) _mag_declination_deg,///< magnetic declination (degrees) + _param_ekf2_beta_gate, ///< synthetic sideslip innovation consistency gate size (STD) + (ParamExtFloat) _param_ekf2_beta_noise, ///< synthetic sideslip noise (rad) + (ParamExtFloat) _param_ekf2_mag_decl,///< magnetic declination (degrees) (ParamExtFloat) - _heading_innov_gate,///< heading fusion innovation consistency gate size (STD) + _param_ekf2_hdg_gate,///< heading fusion innovation consistency gate size (STD) (ParamExtFloat) - _mag_innov_gate, ///< magnetometer fusion innovation consistency gate size (STD) + _param_ekf2_mag_gate, ///< magnetometer fusion innovation consistency gate size (STD) (ParamExtInt) - _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) - _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) - _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) - _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) - _gps_check_mask, ///< bitmask used to control which GPS quality checks are used - (ParamExtFloat) _requiredEph, ///< maximum acceptable horiz position error (m) - (ParamExtFloat) _requiredEpv, ///< maximum acceptable vert position error (m) - (ParamExtFloat) _requiredSacc, ///< maximum acceptable speed error (m/s) - (ParamExtInt) _requiredNsats, ///< minimum acceptable satellite count - (ParamExtFloat) _requiredGDoP, ///< maximum acceptable geometric dilution of precision - (ParamExtFloat) _requiredHdrift, ///< maximum acceptable horizontal drift speed (m/s) - (ParamExtFloat) _requiredVdrift, ///< maximum acceptable vertical drift speed (m/s) + _param_ekf2_gps_check, ///< bitmask used to control which GPS quality checks are used + (ParamExtFloat) _param_ekf2_req_eph, ///< maximum acceptable horiz position error (m) + (ParamExtFloat) _param_ekf2_req_epv, ///< maximum acceptable vert position error (m) + (ParamExtFloat) _param_ekf2_req_sacc, ///< maximum acceptable speed error (m/s) + (ParamExtInt) _param_ekf2_req_nsats, ///< minimum acceptable satellite count + (ParamExtFloat) + _param_ekf2_req_gdop, ///< maximum acceptable geometric dilution of precision + (ParamExtFloat) + _param_ekf2_req_hdrift, ///< maximum acceptable horizontal drift speed (m/s) + (ParamExtFloat) _param_ekf2_req_vdrift, ///< maximum acceptable vertical drift speed (m/s) // measurement source control (ParamExtInt) - _fusion_mode, ///< bitmasked integer that selects which of the GPS and optical flow aiding sources will be used - (ParamExtInt) _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) _param_ekf2_hgt_mode, ///< selects the primary source for height data (ParamExtInt) - _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) _range_noise, ///< observation noise for range finder measurements (m) - (ParamExtFloat) _range_noise_scaler, ///< scale factor from range to range noise (m/m) + (ParamExtFloat) + _param_ekf2_rng_noise, ///< observation noise for range finder measurements (m) + (ParamExtFloat) _param_ekf2_rng_sfe, ///< scale factor from range to range noise (m/m) (ParamExtFloat) - _range_innov_gate, ///< range finder fusion innovation consistency gate size (STD) - (ParamExtFloat) _rng_gnd_clearance, ///< minimum valid value for range when on ground (m) - (ParamExtFloat) _rng_pitch_offset, ///< range sensor pitch offset (rad) + _param_ekf2_rng_gate, ///< range finder fusion innovation consistency gate size (STD) + (ParamExtFloat) _param_ekf2_min_rng, ///< minimum valid value for range when on ground (m) + (ParamExtFloat) _param_ekf2_rng_pitch, ///< range sensor pitch offset (rad) (ParamExtInt) - _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) - _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) - _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) - _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) - _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) - _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) - _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) - _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) - _flow_noise_qual_min, ///< worst quality observation noise for optical flow LOS rate measurements (rad/sec) - (ParamExtInt) _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) + _param_ekf2_of_qmin, ///< minimum acceptable quality integer from the flow sensor (ParamExtFloat) - _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) _imu_pos_x, ///< X position of IMU in body frame (m) - (ParamExtFloat) _imu_pos_y, ///< Y position of IMU in body frame (m) - (ParamExtFloat) _imu_pos_z, ///< Z position of IMU in body frame (m) - (ParamExtFloat) _gps_pos_x, ///< X position of GPS antenna in body frame (m) - (ParamExtFloat) _gps_pos_y, ///< Y position of GPS antenna in body frame (m) - (ParamExtFloat) _gps_pos_z, ///< Z position of GPS antenna in body frame (m) - (ParamExtFloat) _rng_pos_x, ///< X position of range finder in body frame (m) - (ParamExtFloat) _rng_pos_y, ///< Y position of range finder in body frame (m) - (ParamExtFloat) _rng_pos_z, ///< Z position of range finder in body frame (m) + (ParamExtFloat) _param_ekf2_imu_pos_x, ///< X position of IMU in body frame (m) + (ParamExtFloat) _param_ekf2_imu_pos_y, ///< Y position of IMU in body frame (m) + (ParamExtFloat) _param_ekf2_imu_pos_z, ///< Z position of IMU in body frame (m) + (ParamExtFloat) _param_ekf2_gps_pos_x, ///< X position of GPS antenna in body frame (m) + (ParamExtFloat) _param_ekf2_gps_pos_y, ///< Y position of GPS antenna in body frame (m) + (ParamExtFloat) _param_ekf2_gps_pos_z, ///< Z position of GPS antenna in body frame (m) + (ParamExtFloat) _param_ekf2_rng_pos_x, ///< X position of range finder in body frame (m) + (ParamExtFloat) _param_ekf2_rng_pos_y, ///< Y position of range finder in body frame (m) + (ParamExtFloat) _param_ekf2_rng_pos_z, ///< Z position of range finder in body frame (m) (ParamExtFloat) - _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) - _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) - _flow_pos_z, ///< Z position of optical flow sensor focal point in body frame (m) - (ParamExtFloat) _ev_odom_x, ///< X position of VI sensor focal point in body frame (m) - (ParamExtFloat) _ev_odom_y, ///< Y position of VI sensor focal point in body frame (m) - (ParamExtFloat) _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) + _param_ekf2_ev_pos_x, ///< X position of VI sensor focal point in body frame (m) + (ParamExtFloat) + _param_ekf2_ev_pos_y, ///< Y position of VI sensor focal point in body frame (m) + (ParamExtFloat) + _param_ekf2_ev_pos_z, ///< Z position of VI sensor focal point in body frame (m) // control of airspeed and sideslip fusion (ParamFloat) - _arspFusionThreshold, ///< A value of zero will disabled airspeed fusion. Any positive value sets the minimum airspeed which will be used (m/sec) - (ParamInt) _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) + _param_ekf2_fuse_beta, ///< Controls synthetic sideslip fusion, 0 disables, 1 enables // output predictor filter time constants (ParamExtFloat) - _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) - _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) _gyr_bias_init, ///< 1-sigma gyro bias uncertainty at switch on (rad/sec) + (ParamExtFloat) + _param_ekf2_gbias_init, ///< 1-sigma gyro bias uncertainty at switch on (rad/sec) (ParamExtFloat) - _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) - _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) _mag_bias_x, ///< X magnetometer bias (mGauss) - (ParamFloat) _mag_bias_y, ///< Y magnetometer bias (mGauss) - (ParamFloat) _mag_bias_z, ///< Z magnetometer bias (mGauss) - (ParamInt) _mag_bias_id, ///< ID of the magnetometer sensor used to learn the bias values + (ParamFloat) _param_ekf2_magbias_x, ///< X magnetometer bias (mGauss) + (ParamFloat) _param_ekf2_magbias_y, ///< Y magnetometer bias (mGauss) + (ParamFloat) _param_ekf2_magbias_z, ///< Z magnetometer bias (mGauss) + (ParamInt) + _param_ekf2_magbias_id, ///< ID of the magnetometer sensor used to learn the bias values (ParamFloat) - _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) - _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) _acc_bias_lim, ///< Accelerometer bias learning limit (m/s**2) + (ParamExtFloat) _param_ekf2_abl_lim, ///< Accelerometer bias learning limit (m/s**2) (ParamExtFloat) - _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) - _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) - _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) - _drag_noise, ///< observation noise variance for drag specific force measurements (m/sec**2)**2 - (ParamExtFloat) _bcoef_x, ///< ballistic coefficient along the X-axis (kg/m**2) - (ParamExtFloat) _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) _param_ekf2_bcoef_x, ///< ballistic coefficient along the X-axis (kg/m**2) + (ParamExtFloat) _param_ekf2_bcoef_y, ///< ballistic coefficient along the Y-axis (kg/m**2) // Corrections for static pressure position error where Ps_error = Ps_meas - Ps_truth // Coef = Ps_error / Pdynamic, where Pdynamic = 1/2 * density * TAS**2 - (ParamFloat) _aspd_max, ///< upper limit on airspeed used for correction (m/s**2) + (ParamFloat) + _param_ekf2_aspd_max, ///< upper limit on airspeed used for correction (m/s**2) (ParamFloat) - _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) - _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) - _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) - _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) - _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) - _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) - _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) - _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) {