fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-13 06:40:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: add kconfig option to enable/disable range finder fusion (and terrain estimator)

Browse Source
This commit is contained in:
Daniel Agar
2023-03-20 12:41:42 -04:00
parent 0784901a66
commit 32a5bd32ad
17 changed files with 343 additions and 173 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/estimator_interface.cpp
+9 -1
View File
@@ -49,13 +49,15 @@ EstimatorInterface::~EstimatorInterface()
delete _gps_buffer;
delete _mag_buffer;
delete _baro_buffer;
#if defined(CONFIG_EKF2_RANGE_FINDER)
delete _range_buffer;
#endif // CONFIG_EKF2_RANGE_FINDER
#if defined(CONFIG_EKF2_AIRSPEED)
delete _airspeed_buffer;
#endif // CONFIG_EKF2_AIRSPEED
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
delete _flow_buffer;
#endif // _flow_buffer
#endif // CONFIG_EKF2_OPTICAL_FLOW
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
delete _ext_vision_buffer;
#endif // CONFIG_EKF2_EXTERNAL_VISION
@@ -288,6 +290,7 @@ void EstimatorInterface::setAirspeedData(const airspeedSample &airspeed_sample)
}
#endif // CONFIG_EKF2_AIRSPEED
#if defined(CONFIG_EKF2_RANGE_FINDER)
void EstimatorInterface::setRangeData(const rangeSample &range_sample)
{
if (!_initialised) {
@@ -323,6 +326,7 @@ void EstimatorInterface::setRangeData(const rangeSample &range_sample)
ECL_WARN("range data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _range_buffer->get_newest().time_us, _min_obs_interval_us);
}
}
#endif // CONFIG_EKF2_RANGE_FINDER
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
void EstimatorInterface::setOpticalFlowData(const flowSample &flow)
@@ -555,10 +559,12 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp)
max_time_delay_ms = math::max(_params.mag_delay_ms, max_time_delay_ms);
}
#if defined(CONFIG_EKF2_RANGE_FINDER)
// using range finder
if ((_params.rng_ctrl != RngCtrl::DISABLED)) {
max_time_delay_ms = math::max(_params.range_delay_ms, max_time_delay_ms);
}
#endif // CONFIG_EKF2_RANGE_FINDER
if (_params.gnss_ctrl > 0) {
max_time_delay_ms = math::max(_params.gps_delay_ms, max_time_delay_ms);
@@ -700,9 +706,11 @@ void EstimatorInterface::print_status()
printf("baro buffer: %d/%d (%d Bytes)\n", _baro_buffer->entries(), _baro_buffer->get_length(), _baro_buffer->get_total_size());
}
#if defined(CONFIG_EKF2_RANGE_FINDER)
if (_range_buffer) {
printf("range buffer: %d/%d (%d Bytes)\n", _range_buffer->entries(), _range_buffer->get_length(), _range_buffer->get_total_size());
}
#endif // CONFIG_EKF2_RANGE_FINDER
#if defined(CONFIG_EKF2_AIRSPEED)
if (_airspeed_buffer) {