Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-21 21:55:34 +08:00
Code Issues Actions 5 Packages Projects Releases Wiki Activity

ekf2: add terrain estimator_status_flags

Browse Source
This commit is contained in:
Daniel Agar
2024-06-28 01:55:08 -04:00
parent 64a6971bdb
commit 3e3b886b5d
9 changed files with 47 additions and 48 deletions
Download Patch File Download Diff File Expand all files Collapse all files
msg/EstimatorStatusFlags.msg
+2
View File
@@ -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
src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_control.cpp
+9 -9
View File
@@ -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;
src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp
+13 -13
View File
@@ -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<int32_t>(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<int32_t>(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;
}
src/modules/ekf2/EKF/common.h
+3 -11
View File
@@ -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 {
src/modules/ekf2/EKF/ekf.h
-3
View File
@@ -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
src/modules/ekf2/EKF/ekf_helper.cpp
+2 -2
View File
@@ -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));
}
src/modules/ekf2/EKF/terrain_control.cpp
+3 -3
View File
@@ -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;
}
src/modules/ekf2/EKF2.cpp
+13 -1
View File
@@ -1644,7 +1644,17 @@ void EKF2::PublishLocalPosition(const hrt_abstime &timestamp)
// 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 &timestamp)
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;
src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp
+2 -6
View File
@@ -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