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

Apply code review changes from @dagar

Browse Source
This commit is contained in:
alexklimaj
2023-07-11 08:18:06 -07:00
committed by Ramon Roche
parent 9511248514
commit bca0be47bc
2 changed files with 4 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/optflow_fusion.cpp
+2 -5
View File
@@ -83,13 +83,10 @@ void Ekf::updateOptFlow(estimator_aid_source2d_s &aid_src)
Vector2f innov_var; Vector2f innov_var;
Vector24f H; Vector24f H;
sym::ComputeFlowXyInnovVarAndHx(state_vector, P, range, R_LOS, FLT_EPSILON, &innov_var, &H); sym::ComputeFlowXyInnovVarAndHx(state_vector, P, range, R_LOS, FLT_EPSILON, &innov_var, &H);
innov_var.copyTo(_aid_src_optical_flow.innovation_variance); innov_var.copyTo(aid_src.innovation_variance);
// run the innovation consistency check and record result // run the innovation consistency check and record result
setEstimatorAidStatusTestRatio(_aid_src_optical_flow, math::max(_params.flow_innov_gate, 1.f)); setEstimatorAidStatusTestRatio(aid_src, math::max(_params.flow_innov_gate, 1.f));
_innov_check_fail_status.flags.reject_optflow_X = (_aid_src_optical_flow.test_ratio[0] > 1.f);
_innov_check_fail_status.flags.reject_optflow_Y = (_aid_src_optical_flow.test_ratio[1] > 1.f);
} }
void Ekf::fuseOptFlow() void Ekf::fuseOptFlow()
src/modules/ekf2/EKF/optical_flow_control.cpp
+2 -2
View File
@@ -59,7 +59,7 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed)
// Accumulate autopilot gyro data across the same time interval as the flow sensor // Accumulate autopilot gyro data across the same time interval as the flow sensor
const Vector3f delta_angle(imu_delayed.delta_ang - (getGyroBias() * imu_delayed.delta_ang_dt)); const Vector3f delta_angle(imu_delayed.delta_ang - (getGyroBias() * imu_delayed.delta_ang_dt));
if (_delta_time_of < 0.5f) { if (_delta_time_of < 0.2f) {
_imu_del_ang_of += delta_angle; _imu_del_ang_of += delta_angle;
_delta_time_of += imu_delayed.delta_ang_dt; _delta_time_of += imu_delayed.delta_ang_dt;
@@ -75,7 +75,7 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed)
const bool is_tilt_good = (_R_to_earth(2, 2) > _params.range_cos_max_tilt); const bool is_tilt_good = (_R_to_earth(2, 2) > _params.range_cos_max_tilt);
const float delta_time_min = fmaxf(0.7f * _delta_time_of, 0.001f); const float delta_time_min = fmaxf(0.7f * _delta_time_of, 0.001f);
const float delta_time_max = fminf(1.3f * _delta_time_of, 0.5f); const float delta_time_max = fminf(1.3f * _delta_time_of, 0.2f);
bool is_delta_time_good = _flow_sample_delayed.dt >= delta_time_min && _flow_sample_delayed.dt <= delta_time_max; bool is_delta_time_good = _flow_sample_delayed.dt >= delta_time_min && _flow_sample_delayed.dt <= delta_time_max;
if (!is_delta_time_good && (_flow_sample_delayed.dt > FLT_EPSILON)) { if (!is_delta_time_good && (_flow_sample_delayed.dt > FLT_EPSILON)) {