diff --git a/msg/EstimatorStatusFlags.msg b/msg/EstimatorStatusFlags.msg index c6e0504f15..2d6a7c39a1 100644 --- a/msg/EstimatorStatusFlags.msg +++ b/msg/EstimatorStatusFlags.msg @@ -43,6 +43,8 @@ bool cs_mag # 35 - true if 3-axis magnetometer measurement f 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_aux_gpos # 38 - true if auxiliary global position measurement fusion is intended +bool cs_rng_terrain # 39 - true if we are fusing range finder data for terrain +bool cs_opt_flow_terrain # 40 - true if we are fusing flow data for terrain # fault status uint32 fault_status_changes # number of filter fault status (fs) changes diff --git a/src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_control.cpp b/src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_control.cpp index 85e4e5ebe1..0e4b8b668e 100644 --- a/src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_control.cpp @@ -131,13 +131,13 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed) && isTimedOut(_aid_src_optical_flow.time_last_fuse, (uint64_t)2e6); // Prevent rapid switching // If the height is relative to the ground, terrain height cannot be observed. - _hagl_sensor_status.flags.flow = _control_status.flags.opt_flow && !(_height_sensor_ref == HeightSensor::RANGE); + _control_status.flags.opt_flow_terrain = _control_status.flags.opt_flow && !(_height_sensor_ref == HeightSensor::RANGE); if (_control_status.flags.opt_flow) { if (continuing_conditions_passing) { if (is_quality_good && is_magnitude_good && is_tilt_good) { - fuseOptFlow(H, _hagl_sensor_status.flags.flow); + fuseOptFlow(H, _control_status.flags.opt_flow_terrain); } // handle the case when we have optical flow, are reliant on it, but have not been using it for an extended period @@ -145,7 +145,7 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed) if (is_flow_required && is_quality_good) { resetFlowFusion(); - if (_hagl_sensor_status.flags.flow && !isTerrainEstimateValid()) { + if (_control_status.flags.opt_flow_terrain && !isTerrainEstimateValid()) { resetTerrainToFlow(); } @@ -161,14 +161,14 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed) } else { if (starting_conditions_passing) { // If the height is relative to the ground, terrain height cannot be observed. - _hagl_sensor_status.flags.flow = (_height_sensor_ref != HeightSensor::RANGE); + _control_status.flags.opt_flow_terrain = (_height_sensor_ref != HeightSensor::RANGE); if (isHorizontalAidingActive()) { - if (fuseOptFlow(H, _hagl_sensor_status.flags.flow)) { + if (fuseOptFlow(H, _control_status.flags.opt_flow_terrain)) { ECL_INFO("starting optical flow"); _control_status.flags.opt_flow = true; - } else if (_hagl_sensor_status.flags.flow && !_hagl_sensor_status.flags.range_finder) { + } else if (_control_status.flags.opt_flow_terrain && !_control_status.flags.rng_terrain) { ECL_INFO("starting optical flow, resetting terrain"); resetTerrainToFlow(); _control_status.flags.opt_flow = true; @@ -180,14 +180,14 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed) resetFlowFusion(); _control_status.flags.opt_flow = true; - } else if (_hagl_sensor_status.flags.flow) { + } else if (_control_status.flags.opt_flow_terrain) { ECL_INFO("starting optical flow, resetting terrain"); resetTerrainToFlow(); _control_status.flags.opt_flow = true; } } - _hagl_sensor_status.flags.flow = _control_status.flags.opt_flow && !(_height_sensor_ref == HeightSensor::RANGE); + _control_status.flags.opt_flow_terrain = _control_status.flags.opt_flow && !(_height_sensor_ref == HeightSensor::RANGE); } } @@ -233,7 +233,7 @@ void Ekf::stopFlowFusion() if (_control_status.flags.opt_flow) { ECL_INFO("stopping optical flow fusion"); _control_status.flags.opt_flow = false; - _hagl_sensor_status.flags.flow = false; + _control_status.flags.opt_flow_terrain = false; _fault_status.flags.bad_optflow_X = false; _fault_status.flags.bad_optflow_Y = false; diff --git a/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp b/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp index 93e15766ea..2bc07e7b22 100644 --- a/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp @@ -112,11 +112,11 @@ void Ekf::controlRangeHaglFusion() && _range_sensor.isRegularlySendingData(); - const bool do_conditional_range_aid = (_hagl_sensor_status.flags.range_finder || _control_status.flags.rng_hgt) + const bool do_conditional_range_aid = (_control_status.flags.rng_terrain || _control_status.flags.rng_hgt) && (_params.rng_ctrl == static_cast(RngCtrl::CONDITIONAL)) && isConditionalRangeAidSuitable(); - const bool do_range_aid = (_hagl_sensor_status.flags.range_finder || _control_status.flags.rng_hgt) + const bool do_range_aid = (_control_status.flags.rng_terrain || _control_status.flags.rng_hgt) && (_params.rng_ctrl == static_cast(RngCtrl::ENABLED)); if (_control_status.flags.rng_hgt) { @@ -135,7 +135,7 @@ void Ekf::controlRangeHaglFusion() _control_status.flags.rng_hgt = true; stopRngTerrFusion(); - if (!_hagl_sensor_status.flags.flow && aid_src.innovation_rejected) { + if (!_control_status.flags.opt_flow_terrain && aid_src.innovation_rejected) { resetTerrainToRng(aid_src); } @@ -159,17 +159,17 @@ void Ekf::controlRangeHaglFusion() ECL_INFO("starting %s height fusion", HGT_SRC_NAME); _control_status.flags.rng_hgt = true; - if (!_hagl_sensor_status.flags.flow && aid_src.innovation_rejected) { + if (!_control_status.flags.opt_flow_terrain && aid_src.innovation_rejected) { resetTerrainToRng(aid_src); } } } } - if (_control_status.flags.rng_hgt || _hagl_sensor_status.flags.range_finder) { + if (_control_status.flags.rng_hgt || _control_status.flags.rng_terrain) { if (continuing_conditions_passing) { - fuseHaglRng(aid_src, _control_status.flags.rng_hgt, _hagl_sensor_status.flags.range_finder); + fuseHaglRng(aid_src, _control_status.flags.rng_hgt, _control_status.flags.rng_terrain); const bool is_fusion_failing = isTimedOut(aid_src.time_last_fuse, _params.hgt_fusion_timeout_max); @@ -187,7 +187,7 @@ void Ekf::controlRangeHaglFusion() } else if (is_fusion_failing) { // Some other height source is still working - if (_hagl_sensor_status.flags.flow && isTerrainEstimateValid()) { + if (_control_status.flags.opt_flow_terrain && isTerrainEstimateValid()) { ECL_WARN("stopping %s fusion, fusion failing", HGT_SRC_NAME); stopRngHgtFusion(); stopRngTerrFusion(); @@ -205,10 +205,10 @@ void Ekf::controlRangeHaglFusion() } else { if (starting_conditions_passing) { - if (_hagl_sensor_status.flags.flow) { + if (_control_status.flags.opt_flow_terrain) { if (!aid_src.innovation_rejected) { - _hagl_sensor_status.flags.range_finder = true; - fuseHaglRng(aid_src, _control_status.flags.rng_hgt, _hagl_sensor_status.flags.range_finder); + _control_status.flags.rng_terrain = true; + fuseHaglRng(aid_src, _control_status.flags.rng_hgt, _control_status.flags.rng_terrain); } } else { @@ -216,12 +216,12 @@ void Ekf::controlRangeHaglFusion() resetTerrainToRng(aid_src); } - _hagl_sensor_status.flags.range_finder = true; + _control_status.flags.rng_terrain = true; } } } - } else if ((_control_status.flags.rng_hgt || _hagl_sensor_status.flags.range_finder) + } else if ((_control_status.flags.rng_hgt || _control_status.flags.rng_terrain) && !isNewestSampleRecent(_time_last_range_buffer_push, 2 * estimator::sensor::RNG_MAX_INTERVAL)) { // No data anymore. Stop until it comes back. ECL_WARN("stopping %s fusion, no data", HGT_SRC_NAME); @@ -317,5 +317,5 @@ void Ekf::stopRngHgtFusion() void Ekf::stopRngTerrFusion() { - _hagl_sensor_status.flags.range_finder = false; + _control_status.flags.rng_terrain = false; } diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index ae1d185860..3a1648b3d8 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -581,7 +581,9 @@ 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 aux_gpos : 1; + uint64_t aux_gpos : 1; ///< 38 - true if auxiliary global position measurement fusion is intended + uint64_t rng_terrain : 1; ///< 39 - true if we are fusing range finder data for terrain + uint64_t opt_flow_terrain : 1; ///< 40 - true if we are fusing flow data for terrain } flags; uint64_t value; @@ -606,16 +608,6 @@ union ekf_solution_status_u { uint16_t value; }; -#if defined(CONFIG_EKF2_TERRAIN) -union terrain_fusion_status_u { - struct { - bool range_finder : 1; ///< 0 - true if we are fusing range finder data - bool flow : 1; ///< 1 - true if we are fusing flow data - } flags; - uint8_t value; -}; -#endif // CONFIG_EKF2_TERRAIN - // define structure used to communicate information events union information_event_status_u { struct { diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index b3f52e18d3..42ea84aa1f 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -100,8 +100,6 @@ public: // terrain estimate bool isTerrainEstimateValid() const; - uint8_t getTerrainEstimateSensorBitfield() const { return _hagl_sensor_status.value; } - // get the estimated terrain vertical position relative to the NED origin float getTerrainVertPos() const { return _state.terrain; }; float getHagl() const { return _state.terrain - _state.pos(2); } @@ -585,7 +583,6 @@ private: // Terrain height state estimation uint8_t _terrain_vpos_reset_counter{0}; ///< number of times _terrain_vpos has been reset - terrain_fusion_status_u _hagl_sensor_status{}; ///< Struct indicating type of sensor used to estimate height above ground float _last_on_ground_posD{0.0f}; ///< last vertical position when the in_air status was false (m) #endif // CONFIG_EKF2_TERRAIN diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index 5ac5f5467d..621ac87c65 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -266,7 +266,7 @@ void Ekf::get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, fl float flow_hagl_max = _flow_max_distance; // only limit optical flow height is dependent on range finder or terrain estimate invalid (precaution) - if ((!_hagl_sensor_status.flags.flow && _hagl_sensor_status.flags.range_finder) + if ((!_control_status.flags.opt_flow_terrain && _control_status.flags.rng_terrain) || !isTerrainEstimateValid() ) { flow_hagl_min = math::max(flow_hagl_min, rangefinder_hagl_min); @@ -472,7 +472,7 @@ float Ekf::getHeightAboveGroundInnovationTestRatio() const #if defined(CONFIG_EKF2_TERRAIN) # if defined(CONFIG_EKF2_RANGE_FINDER) - if (_hagl_sensor_status.flags.range_finder) { + if (_control_status.flags.rng_terrain) { // return the terrain height innovation test ratio test_ratio = math::max(test_ratio, fabsf(_aid_src_rng_hgt.test_ratio_filtered)); } diff --git a/src/modules/ekf2/EKF/terrain_control.cpp b/src/modules/ekf2/EKF/terrain_control.cpp index c21b7b35d8..8f277abac6 100644 --- a/src/modules/ekf2/EKF/terrain_control.cpp +++ b/src/modules/ekf2/EKF/terrain_control.cpp @@ -64,8 +64,8 @@ void Ekf::controlTerrainFakeFusion() } if (!_control_status.flags.in_air - && !_hagl_sensor_status.flags.range_finder - && !_hagl_sensor_status.flags.flow) { + && !_control_status.flags.rng_terrain + && !_control_status.flags.opt_flow_terrain) { bool recent_terrain_aiding = false; @@ -93,7 +93,7 @@ bool Ekf::isTerrainEstimateValid() const #if defined(CONFIG_EKF2_RANGE_FINDER) // Assume that the terrain estimate is always valid when direct observations are fused - if (_hagl_sensor_status.flags.range_finder && isRecent(_aid_src_rng_hgt.time_last_fuse, (uint64_t)5e6)) { + if (_control_status.flags.rng_terrain && isRecent(_aid_src_rng_hgt.time_last_fuse, (uint64_t)5e6)) { valid = true; } diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 9c330f5f40..62a0af99ad 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -1644,7 +1644,17 @@ void EKF2::PublishLocalPosition(const hrt_abstime ×tamp) // Distance to bottom surface (ground) in meters, must be positive lpos.dist_bottom = math::max(_ekf.getHagl(), 0.f); lpos.dist_bottom_valid = _ekf.isTerrainEstimateValid(); - lpos.dist_bottom_sensor_bitfield = _ekf.getTerrainEstimateSensorBitfield(); + + lpos.dist_bottom_sensor_bitfield = vehicle_local_position_s::DIST_BOTTOM_SENSOR_NONE; + + if (_ekf.control_status_flags().rng_terrain) { + lpos.dist_bottom_sensor_bitfield |= vehicle_local_position_s::DIST_BOTTOM_SENSOR_RANGE; + } + + if (_ekf.control_status_flags().opt_flow_terrain) { + lpos.dist_bottom_sensor_bitfield |= vehicle_local_position_s::DIST_BOTTOM_SENSOR_FLOW; + } + #endif // CONFIG_EKF2_TERRAIN _ekf.get_ekf_lpos_accuracy(&lpos.eph, &lpos.epv); @@ -1937,6 +1947,8 @@ void EKF2::PublishStatusFlags(const hrt_abstime ×tamp) 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_aux_gpos = _ekf.control_status_flags().aux_gpos; + status_flags.cs_rng_terrain = _ekf.control_status_flags().rng_terrain; + status_flags.cs_opt_flow_terrain = _ekf.control_status_flags().opt_flow_terrain; 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/test/sensor_simulator/ekf_wrapper.cpp b/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp index 148f30cfe4..26a1dd73c6 100644 --- a/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp +++ b/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp @@ -242,16 +242,12 @@ bool EkfWrapper::isWindVelocityEstimated() const bool EkfWrapper::isIntendingTerrainRngFusion() const { - terrain_fusion_status_u terrain_status; - terrain_status.value = _ekf->getTerrainEstimateSensorBitfield(); - return terrain_status.flags.range_finder; + return _ekf->control_status_flags().rng_terrain; } bool EkfWrapper::isIntendingTerrainFlowFusion() const { - terrain_fusion_status_u terrain_status; - terrain_status.value = _ekf->getTerrainEstimateSensorBitfield(); - return terrain_status.flags.flow; + return _ekf->control_status_flags().opt_flow_terrain; } Eulerf EkfWrapper::getEulerAngles() const