diff --git a/msg/EstimatorStatusFlags.msg b/msg/EstimatorStatusFlags.msg index 4d747d8268..868238ced6 100644 --- a/msg/EstimatorStatusFlags.msg +++ b/msg/EstimatorStatusFlags.msg @@ -42,6 +42,7 @@ bool cs_gravity_vector # 34 - true when gravity vector measurements are bool cs_mag # 35 - true if 3-axis magnetometer measurement fusion (mag states only) is intended bool cs_ev_yaw_fault # 36 - true when the EV heading has been declared faulty and is no longer being used bool cs_mag_heading_consistent # 37 - true when the heading obtained from mag data is declared consistent with the filter +bool cs_mocap # 38 - true when motion capture (mocap) fusion is intended # fault status uint32 fault_status_changes # number of filter fault status (fs) changes diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index c7f75b35e2..554c1d9386 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -113,11 +113,12 @@ enum TerrainFusionMask : uint8_t { #endif // CONFIG_EKF2_RANGE_FINDER enum HeightSensor : uint8_t { - BARO = 0, - GNSS = 1, - RANGE = 2, - EV = 3, - UNKNOWN = 4 + BARO = 0, + GNSS = 1, + RANGE = 2, + EV = 3, + MOCAP = 4, + UNKNOWN = 5 }; enum class PositionSensor : uint8_t { @@ -601,7 +602,7 @@ union filter_control_status_u { uint64_t mag : 1; ///< 35 - true if 3-axis magnetometer measurement fusion (mag states only) is intended uint64_t ev_yaw_fault : 1; ///< 36 - true when the EV heading has been declared faulty and is no longer being used uint64_t mag_heading_consistent : 1; ///< 37 - true when the heading obtained from mag data is declared consistent with the filter - + uint64_t mocap : 1; ///< 38 - true when motion capture (mocap) fusion is intended } flags; uint64_t value; }; diff --git a/src/modules/ekf2/EKF/estimator_interface.h b/src/modules/ekf2/EKF/estimator_interface.h index f9c62967c6..b0e149194f 100644 --- a/src/modules/ekf2/EKF/estimator_interface.h +++ b/src/modules/ekf2/EKF/estimator_interface.h @@ -270,6 +270,10 @@ public: const filter_control_status_u &control_status_prev() const { return _control_status_prev; } const decltype(filter_control_status_u::flags) &control_status_prev_flags() const { return _control_status_prev.flags; } + // TODO + void enableControlStatusMocap() { _control_status.flags.mocap = true; } + void disableControlStatusMocap() { _control_status.flags.mocap = false; } + // get EKF internal fault status const fault_status_u &fault_status() const { return _fault_status; } const decltype(fault_status_u::flags) &fault_status_flags() const { return _fault_status.flags; } diff --git a/src/modules/ekf2/EKF/ev_pos_control.cpp b/src/modules/ekf2/EKF/ev_pos_control.cpp index fc4e2db633..dc4de8da74 100644 --- a/src/modules/ekf2/EKF/ev_pos_control.cpp +++ b/src/modules/ekf2/EKF/ev_pos_control.cpp @@ -69,7 +69,7 @@ void Ekf::controlEvPosFusion(const extVisionSample &ev_sample, const bool common pos = ev_sample.pos - pos_offset_earth; pos_cov = matrix::diag(ev_sample.position_var); - if (_control_status.flags.gps) { + if (_control_status.flags.gps || _control_status.flags.mocap) { _ev_pos_b_est.setFusionActive(); } else { @@ -108,7 +108,7 @@ void Ekf::controlEvPosFusion(const extVisionSample &ev_sample, const bool common pos_cov(i, i) = math::max(pos_cov(i, i), orientation_var_max); } - if (_control_status.flags.gps) { + if (_control_status.flags.gps || _control_status.flags.mocap) { _ev_pos_b_est.setFusionActive(); } else { @@ -125,13 +125,6 @@ void Ekf::controlEvPosFusion(const extVisionSample &ev_sample, const bool common break; } - // increase minimum variance if GPS active (position reference) - if (_control_status.flags.gps) { - for (int i = 0; i < 2; i++) { - pos_cov(i, i) = math::max(pos_cov(i, i), sq(_params.gps_pos_noise)); - } - } - const Vector2f measurement{pos(0), pos(1)}; const Vector2f measurement_var{ @@ -200,7 +193,7 @@ void Ekf::startEvPosFusion(const Vector2f &measurement, const Vector2f &measurem { // activate fusion // TODO: (_params.position_sensor_ref == PositionSensor::EV) - if (_control_status.flags.gps) { + if (_control_status.flags.gps || _control_status.flags.mocap) { ECL_INFO("starting %s fusion", EV_AID_SRC_NAME); _ev_pos_b_est.setBias(-Vector2f(_state.pos.xy()) + measurement); _ev_pos_b_est.setFusionActive(); @@ -226,7 +219,7 @@ void Ekf::updateEvPosFusion(const Vector2f &measurement, const Vector2f &measure if (quality_sufficient) { - if (!_control_status.flags.gps) { + if (!_control_status.flags.gps && !_control_status.flags.mocap) { ECL_INFO("reset to %s", EV_AID_SRC_NAME); _information_events.flags.reset_pos_to_vision = true; resetHorizontalPositionTo(measurement, measurement_var); @@ -261,14 +254,14 @@ void Ekf::updateEvPosFusion(const Vector2f &measurement, const Vector2f &measure // Data seems good, attempt a reset ECL_WARN("%s fusion failing, resetting", EV_AID_SRC_NAME); - if (_control_status.flags.gps && !pos_xy_fusion_failing) { + if ((_control_status.flags.gps || _control_status.flags.mocap) && !pos_xy_fusion_failing) { // reset EV position bias _ev_pos_b_est.setBias(-Vector2f(_state.pos.xy()) + measurement); } else { _information_events.flags.reset_pos_to_vision = true; - if (_control_status.flags.gps) { + if (_control_status.flags.gps || _control_status.flags.mocap) { resetHorizontalPositionTo(measurement - _ev_pos_b_est.getBias(), measurement_var + _ev_pos_b_est.getBiasVar()); _ev_pos_b_est.setBias(-Vector2f(_state.pos.xy()) + measurement); diff --git a/src/modules/ekf2/EKF/ev_yaw_control.cpp b/src/modules/ekf2/EKF/ev_yaw_control.cpp index 4d0472802f..7408e8a73f 100644 --- a/src/modules/ekf2/EKF/ev_yaw_control.cpp +++ b/src/modules/ekf2/EKF/ev_yaw_control.cpp @@ -57,6 +57,7 @@ void Ekf::controlEvYawFusion(const extVisionSample &ev_sample, const bool common bool continuing_conditions_passing = (_params.ev_ctrl & static_cast(EvCtrl::YAW)) && _control_status.flags.tilt_align && !_control_status.flags.ev_yaw_fault + && !_control_status.flags.mocap && PX4_ISFINITE(aid_src.observation) && PX4_ISFINITE(aid_src.observation_variance); diff --git a/src/modules/ekf2/EKF/mocap.cpp b/src/modules/ekf2/EKF/mocap.cpp index 91da9add59..2a41a3d51f 100644 --- a/src/modules/ekf2/EKF/mocap.cpp +++ b/src/modules/ekf2/EKF/mocap.cpp @@ -62,12 +62,20 @@ void Mocap::update(Ekf &ekf, const estimator::imuSample &imu_delayed) if (_mocap_buffer.pop_first_older_than(imu_delayed.time_us, &mocap_sample)) { - if (!ekf.global_origin_valid()) { - return; - } - // position - { + bool pos_valid = mocap_sample.pos.isAllFinite(); + //bool q_valid = mocap_sample.q.isAllFinite() && (q.min() > 0.f); + + // yaw_align = false; + // _height_sensor_ref = HeightSensor::MOCAP; + + //_control_status.flags.mocap = true; + + // reset position and yaw on init + + + // position xy + if (pos_valid) { estimator_aid_source2d_s aid_src{}; const Vector2f pos_obs_var{ @@ -82,10 +90,7 @@ void Mocap::update(Ekf &ekf, const estimator::imuSample &imu_delayed) aid_src); aid_src.fusion_enabled = _param_ekf2_mocap_ctrl.get() & 0b001; // bit 0 Horizontal position - // TODO: check frame - if (ekf.control_status_flags().yaw_align) { - ekf.fuseHorizontalPosition(aid_src); - } + ekf.fuseHorizontalPosition(aid_src); #if defined(MODULE_NAME) aid_src.timestamp = hrt_absolute_time(); @@ -93,6 +98,32 @@ void Mocap::update(Ekf &ekf, const estimator::imuSample &imu_delayed) #endif // MODULE_NAME } + // position z + { + estimator_aid_source1d_s aid_src{}; + + const float pos_obs_var = math::max(sq(mocap_sample.position_var(2)), sq(_param_ekf2_mocap_noise.get())); + + ekf.updateVerticalPositionAidSrcStatus(mocap_sample.time_us, + mocap_sample.pos(2), // observation + pos_obs_var, // observation variance + math::max(_param_ekf2_mocap_gate.get(), 1.f), // innovation gate + aid_src); + + if (_param_ekf2_mocap_ctrl.get()) { + aid_src.fusion_enabled = true; + + ekf.fuseVerticalPosition(aid_src); + + //const bool is_fusion_failing = isTimedOut(aid_src.time_last_fuse, _params.hgt_fusion_timeout_max); + } + +#if defined(MODULE_NAME) + aid_src.timestamp = hrt_absolute_time(); + _estimator_aid_src_mocap_z_pub.publish(aid_src); +#endif // MODULE_NAME + } + } } diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 788a01ce1f..dbdb73398b 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -1774,6 +1774,7 @@ void EKF2::PublishStatusFlags(const hrt_abstime ×tamp) status_flags.cs_mag = _ekf.control_status_flags().mag; status_flags.cs_ev_yaw_fault = _ekf.control_status_flags().ev_yaw_fault; status_flags.cs_mag_heading_consistent = _ekf.control_status_flags().mag_heading_consistent; + status_flags.cs_mocap = _ekf.control_status_flags().mocap; status_flags.fault_status_changes = _filter_fault_status_changes; status_flags.fs_bad_mag_x = _ekf.fault_status_flags().bad_mag_x; diff --git a/src/modules/ekf2/ekf2_params.c b/src/modules/ekf2/ekf2_params.c index d576ba1ab0..1c537f55ac 100644 --- a/src/modules/ekf2/ekf2_params.c +++ b/src/modules/ekf2/ekf2_params.c @@ -1572,17 +1572,8 @@ PARAM_DEFINE_FLOAT(EKF2_GSF_TAS, 15.0f); /** * Motion capture (mocap) aiding * - * Set bits in the following positions to enable: - * 0 : Horizontal position fusion - * 1 : Vertical position fusion - * 2 : Yaw - * * @group EKF2 - * @min 0 - * @max 15 - * @bit 0 Horizontal position - * @bit 1 Vertical position - * @bit 2 Yaw + * @boolean */ PARAM_DEFINE_INT32(EKF2_MOCAP_CTRL, 1);