diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index a882390eea..a991f68fa7 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -186,8 +186,7 @@ private: control::BlockParamFloat *_mag_declination_deg; // magnetic declination in degrees control::BlockParamFloat *_heading_innov_gate; // innovation gate for heading innovation test control::BlockParamFloat *_mag_innov_gate; // innovation gate for magnetometer innovation test - control::BlockParamInt - *_mag_decl_source; // bitmasked integer used to control the handling of magnetic declination + control::BlockParamInt *_mag_decl_source; // bitmasked integer used to control the handling of magnetic declination control::BlockParamInt *_mag_fuse_type; // integer ued to control the type of magnetometer fusion used control::BlockParamInt *_gps_check_mask; // bitmasked integer used to activate the different GPS quality checks @@ -200,6 +199,22 @@ private: control::BlockParamFloat *_requiredVdrift; // maximum acceptable vertical drift speed (m/s) control::BlockParamInt *_param_record_replay_msg; // indicates if we want to record ekf2 replay messages + // measurement source control + control::BlockParamInt *_fusion_mode; // bitmasked integer that selects which of the GPS and optical flow aiding sources will be used + control::BlockParamInt *_vdist_sensor_type; // selects the primary source for height data + + // range finder fusion + control::BlockParamFloat *_range_noise; // observation noise for range finder measurements (m) + control::BlockParamFloat *_range_innov_gate; // range finder fusion innovation consistency gate size (STD) + control::BlockParamFloat *_rng_gnd_clearance; // minimum valid value for range when on ground (m) + + // optical flow fusion + control::BlockParamFloat *_flow_noise; // best quality observation noise for optical flow LOS rate measurements (rad/sec) + control::BlockParamFloat *_flow_noise_qual_min; // worst quality observation noise for optical flow LOS rate measurements (rad/sec) + control::BlockParamInt *_flow_qual_min; // minimum acceptable quality integer from the flow sensor + control::BlockParamFloat *_flow_innov_gate; // optical flow fusion innovation consistency gate size (STD) + control::BlockParamFloat *_flow_rate_max; // maximum valid optical flow rate (rad/sec) + int update_subscriptions(); }; @@ -253,7 +268,17 @@ Ekf2::Ekf2(): _requiredGDoP(new control::BlockParamFloat(this, "EKF2_REQ_GDOP", false, &_params->req_gdop)), _requiredHdrift(new control::BlockParamFloat(this, "EKF2_REQ_HDRIFT", false, &_params->req_hdrift)), _requiredVdrift(new control::BlockParamFloat(this, "EKF2_REQ_VDRIFT", false, &_params->req_vdrift)), - _param_record_replay_msg(new control::BlockParamInt(this, "EKF2_REC_RPL", false, &_publish_replay_mode)) + _param_record_replay_msg(new control::BlockParamInt(this, "EKF2_REC_RPL", false, &_publish_replay_mode)), + _fusion_mode(new control::BlockParamInt(this, "EKF2_AID_MASK", false, &_params->fusion_mode)), + _vdist_sensor_type(new control::BlockParamInt(this, "EKF2_HGT_MODE", false, &_params->vdist_sensor_type)), + _range_noise(new control::BlockParamFloat(this, "EKF2_RNG_NOISE", false, &_params->range_noise)), + _range_innov_gate(new control::BlockParamFloat(this, "EKF2_RNG_GATE", false, &_params->range_innov_gate)), + _rng_gnd_clearance(new control::BlockParamFloat(this, "EKF2_MIN_RNG", false, &_params->rng_gnd_clearance)), + _flow_noise(new control::BlockParamFloat(this, "EKF2_OF_N_MIN", false, &_params->flow_noise)), + _flow_noise_qual_min(new control::BlockParamFloat(this, "EKF2_OF_N_MAX", false, &_params->flow_noise_qual_min)), + _flow_qual_min(new control::BlockParamInt(this, "EKF2_OF_QMIN", false, &_params->flow_qual_min)), + _flow_innov_gate(new control::BlockParamFloat(this, "EKF2_OF_GATE", false, &_params->flow_innov_gate)), + _flow_rate_max(new control::BlockParamFloat(this, "EKF2_OF_RMAX", false, &_params->flow_rate_max)) { } diff --git a/src/modules/ekf2/ekf2_params.c b/src/modules/ekf2/ekf2_params.c index 53bb1aa9a3..3e7465a327 100644 --- a/src/modules/ekf2/ekf2_params.c +++ b/src/modules/ekf2/ekf2_params.c @@ -327,7 +327,7 @@ PARAM_DEFINE_FLOAT(EKF2_HDG_GATE, 3.0f); PARAM_DEFINE_FLOAT(EKF2_MAG_GATE, 3.0f); /** - * Integer bitmask controlling handling of magnetic declination. Set bits to in the following positions to enable functions. + * Integer bitmask controlling handling of magnetic declination. Set bits in the following positions to enable functions. * 0 : Set to true to use the declination from the geo_lookup library when the GPS position becomes available, set to false to always use the EKF2_MAG_DECL value. * 1 : Set to true to save the EKF2_MAG_DECL parameter to the value returned by the EKF when the vehicle disarms. * 2 : Set to true to always use the declination as an observaton when 3-axis magnetometer fusion is being used. @@ -388,3 +388,99 @@ PARAM_DEFINE_FLOAT(EKF2_GPS_V_GATE, 3.0f); * @max 1 */ PARAM_DEFINE_INT32(EKF2_REC_RPL, 0); + +/** + * Integer bitmask controlling which external aiding sources will be used. Set bits in the following positions to enable: + * 0 : Set to true to use GPS data if available + * 1 : Set to true to use optical flow data if available + * + * @group EKF2 + * @min 0 + * @max 3 + */ +PARAM_DEFINE_INT32(EKF2_AID_MASK, 1); + +/** + * Determines the primary source of height data used by the EKF. + * 0 : Barometric pressure + * 1 : GPS + * 2 : Range to ground + * + * @group EKF2 + * @min 0 + * @max 2 + */ +PARAM_DEFINE_INT32(EKF2_HGT_MODE, 0); + +/** + * Measurement noise for range finder fusion + * + * @group EKF2 + * @min 0.01 + * @unit m + */ +PARAM_DEFINE_FLOAT(EKF2_RNG_NOISE, 0.1f); + +/** + * Gate size for range finder fusion + * + * @group EKF2 + * @min 1.0 + * @unit SD + */ +PARAM_DEFINE_FLOAT(EKF2_RNG_GATE, 5.0f); + +/** + * Minimum valid range for the range finder + * + * @group EKF2 + * @min 0.01 + * @unit m + */ +PARAM_DEFINE_FLOAT(EKF2_MIN_RNG, 0.1f); + +/** + * Measurement noise for the optical flow sensor when it's reported quality metric is at the maximum + * + * @group EKF2 + * @min 0.05 + * @unit rad/sec + */ +PARAM_DEFINE_FLOAT(EKF2_OF_N_MIN, 0.15f); + +/** + * Measurement noise for the optical flow sensor when it's reported quality metric is at the minimum set by EKF2_OF_QMIN + * The following condition must be met: EKF2_OF_N_MAXN >= EKF2_OF_N_MIN + * + * @group EKF2 + * @min 0.05 + * @unit rad/sec + */ +PARAM_DEFINE_FLOAT(EKF2_OF_N_MAX, 0.5f); + +/** + * Optical Flow data will only be used if the sensor reports a quality metric >= EKF2_OF_QMIN. + * + * @group EKF2 + * @min 0 + * @max 255 + */ +PARAM_DEFINE_INT32(EKF2_OF_QMIN, 1); + +/** + * Gate size for optical flow fusion + * + * @group EKF2 + * @min 1.0 + * @unit SD + */ +PARAM_DEFINE_FLOAT(EKF2_OF_GATE, 3.0f); + +/** + * Optical Flow data will not fused if the magnitude of the flow rate > EKF2_OF_RMAX + * + * @group EKF2 + * @min 1.0 + * @unit rad/sec + */ +PARAM_DEFINE_FLOAT(EKF2_OF_RMAX, 2.5f);