Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-02 11:59:17 +08:00
Code Issues Actions 21 Packages Projects Releases Wiki Activity

EKF2: fix builds without CONFIG_EKF2_RANGE_FINDER

Browse Source 
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer
2024-06-26 10:13:29 +02:00
parent a50ef2eb5e
commit 1ae96d6509
2 changed files with 19 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_control.cpp
+11 -3
View File
@@ -53,7 +53,13 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed)
const bool is_quality_good = (_flow_sample_delayed.quality >= min_quality); const bool is_quality_good = (_flow_sample_delayed.quality >= min_quality);
const bool is_magnitude_good = _flow_sample_delayed.flow_rate.isAllFinite() const bool is_magnitude_good = _flow_sample_delayed.flow_rate.isAllFinite()
&& !_flow_sample_delayed.flow_rate.longerThan(_flow_max_rate); && !_flow_sample_delayed.flow_rate.longerThan(_flow_max_rate);
const bool is_tilt_good = (_R_to_earth(2, 2) > _params.range_cos_max_tilt);
bool is_tilt_good = true;
#if defined(CONFIG_EKF2_RANGE_FINDER)
is_tilt_good = (_R_to_earth(2, 2) > _params.range_cos_max_tilt);
#endif // CONFIG_EKF2_RANGE_FINDER
if (is_quality_good if (is_quality_good
&& is_magnitude_good && is_magnitude_good
@@ -198,7 +204,8 @@ void Ekf::stopFlowFusion()
void Ekf::calcOptFlowBodyRateComp(const imuSample &imu_delayed) void Ekf::calcOptFlowBodyRateComp(const imuSample &imu_delayed)
{ {
if (imu_delayed.delta_ang_dt > FLT_EPSILON) { if (imu_delayed.delta_ang_dt > FLT_EPSILON) {
_ref_body_rate = -imu_delayed.delta_ang / imu_delayed.delta_ang_dt - getGyroBias(); // flow gyro has opposite sign convention _ref_body_rate = -imu_delayed.delta_ang / imu_delayed.delta_ang_dt -
getGyroBias(); // flow gyro has opposite sign convention
} else { } else {
_ref_body_rate.zero(); _ref_body_rate.zero();
@@ -213,5 +220,6 @@ void Ekf::calcOptFlowBodyRateComp(const imuSample &imu_delayed)
} }
// calculate the bias estimate using a combined LPF and spike filter // calculate the bias estimate using a combined LPF and spike filter
_flow_gyro_bias = _flow_gyro_bias * 0.99f + matrix::constrain(_flow_sample_delayed.gyro_rate - _ref_body_rate, -0.1f, 0.1f) * 0.01f; _flow_gyro_bias = _flow_gyro_bias * 0.99f + matrix::constrain(_flow_sample_delayed.gyro_rate - _ref_body_rate, -0.1f,
0.1f) * 0.01f;
} }
src/modules/ekf2/EKF2.cpp
+4
View File
@@ -2314,6 +2314,8 @@ bool EKF2::UpdateFlowSample(ekf2_timestamps_s &ekf2_timestamps)
new_optical_flow = true; new_optical_flow = true;
} }
#if defined(CONFIG_EKF2_RANGE_FINDER)
// use optical_flow distance as range sample if distance_sensor unavailable // use optical_flow distance as range sample if distance_sensor unavailable
if (PX4_ISFINITE(optical_flow.distance_m) && (ekf2_timestamps.timestamp > _last_range_sensor_update + 1_s)) { if (PX4_ISFINITE(optical_flow.distance_m) && (ekf2_timestamps.timestamp > _last_range_sensor_update + 1_s)) {
@@ -2330,6 +2332,8 @@ bool EKF2::UpdateFlowSample(ekf2_timestamps_s &ekf2_timestamps)
_ekf.set_rangefinder_limits(optical_flow.min_ground_distance, optical_flow.max_ground_distance); _ekf.set_rangefinder_limits(optical_flow.min_ground_distance, optical_flow.max_ground_distance);
} }
#endif // CONFIG_EKF2_RANGE_FINDER
ekf2_timestamps.optical_flow_timestamp_rel = (int16_t)((int64_t)optical_flow.timestamp / 100 - ekf2_timestamps.optical_flow_timestamp_rel = (int16_t)((int64_t)optical_flow.timestamp / 100 -
(int64_t)ekf2_timestamps.timestamp / 100); (int64_t)ekf2_timestamps.timestamp / 100);
} }