fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-23 02:33:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: pos/vel reset helpers pass new variance

Browse Source 
- optical flow velocity reset use already computed _flow_vel_ne
This commit is contained in:
Daniel Agar
2022-10-24 10:59:58 -04:00
parent f9f466854b
commit 6f861ba889
6 changed files with 402 additions and 429 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/optical_flow_control.cpp
+30 -8
View File
@@ -77,11 +77,12 @@ void Ekf::controlOpticalFlowFusion()
_delta_time_of = _imu_sample_delayed.delta_ang_dt;
is_delta_time_good = true;
}
}
if (is_quality_good && !is_delta_time_good) {
ECL_DEBUG("Optical flow: bad delta time: OF dt %.6f s (min: %.3f, max: %.3f), IMU dt %.6f s",
(double)_flow_sample_delayed.dt, (double)delta_time_min, (double)delta_time_max, (double)_imu_sample_delayed.delta_ang_dt);
(double)_flow_sample_delayed.dt, (double)delta_time_min, (double)delta_time_max,
(double)_imu_sample_delayed.delta_ang_dt);
}
}
@@ -166,12 +167,25 @@ void Ekf::controlOpticalFlowFusion()
// if we are not using GPS or external vision aiding, then the velocity and position states and covariances need to be set
if (!isHorizontalAidingActive()) {
resetHorizontalVelocityToOpticalFlow(_flow_sample_delayed);
resetHorizontalPositionToOpticalFlow();
ECL_INFO("reset velocity to flow");
_information_events.flags.reset_vel_to_flow = true;
resetHorizontalVelocityTo(_flow_vel_ne, calcOptFlowMeasVar(_flow_sample_delayed));
// reset position, estimate is relative to initial position in this mode, so we start with zero error
if (!_control_status.flags.in_air) {
ECL_INFO("reset position to zero");
resetHorizontalPositionTo(Vector2f(0.f, 0.f), 0.f);
_last_known_pos.xy() = _state.pos.xy();
} else {
_information_events.flags.reset_pos_to_last_known = true;
ECL_INFO("reset position to last known position");
resetHorizontalPositionTo(_last_known_pos.xy(), 0.f);
}
}
_control_status.flags.opt_flow = true;
_aid_src_optical_flow.time_last_fuse = _imu_sample_delayed.time_us;
_control_status.flags.opt_flow = true;
return;
}
@@ -190,11 +204,19 @@ void Ekf::controlOpticalFlowFusion()
}
// handle the case when we have optical flow, are reliant on it, but have not been using it for an extended period
if (isTimedOut(_aid_src_optical_flow.time_last_fuse, _params.reset_timeout_max)
if (isTimedOut(_aid_src_optical_flow.time_last_fuse, _params.no_aid_timeout_max)
&& !isOtherSourceOfHorizontalAidingThan(_control_status.flags.opt_flow)) {
resetHorizontalVelocityToOpticalFlow(_flow_sample_delayed);
resetHorizontalPositionToOpticalFlow();
ECL_INFO("reset velocity to flow");
_information_events.flags.reset_vel_to_flow = true;
resetHorizontalVelocityTo(_flow_vel_ne, calcOptFlowMeasVar(_flow_sample_delayed));
// reset position, estimate is relative to initial position in this mode, so we start with zero error
ECL_INFO("reset position to last known");
_information_events.flags.reset_pos_to_last_known = true;
resetHorizontalPositionTo(_last_known_pos.xy(), 0.f);
_aid_src_optical_flow.time_last_fuse = _imu_sample_delayed.time_us;
}
}