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

EKF: Add position, height and velocity reset for EV aiding

Browse Source
This commit is contained in:
Paul Riseborough
2016-05-14 21:17:29 +10:00
parent 3a0fcd03d7
commit 81469d6621
5 changed files with 96 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/control.cpp
+19
View File
@@ -56,6 +56,25 @@ void Ekf::controlFusionModes()
_control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag);
}
// external vision position aiding selection logic
if ((_params.fusion_mode & MASK_USE_EVPOS) && !_control_status.flags.ev_pos && _control_status.flags.tilt_align && _control_status.flags.yaw_align) {
// check for a exernal vision measurement that has fallen behind the fusion time horizon
if (_ext_vision_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_ev_sample_delayed)) {
// turn on use of external vision measurements for position
_control_status.flags.ev_pos = true;
// reset the position, height and velocity
resetPosition();
resetVelocity();
resetHeight();
}
}
// external vision yaw aiding selection logic
if ((_params.fusion_mode & MASK_USE_EVYAW) && !_control_status.flags.ev_yaw && _control_status.flags.tilt_align
&& (_time_last_imu - _time_last_ext_vision) < 5e5) {
//TODO
}
// optical flow fusion mode selection logic
// to start using optical flow data we need angular alignment complete, and fresh optical flow and height above terrain data
if ((_params.fusion_mode & MASK_USE_OF) && !_control_status.flags.opt_flow && _control_status.flags.tilt_align