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

EKF: Add fusion of external yaw data

Browse Source
This commit is contained in:
Paul Riseborough
2016-05-14 21:17:29 +10:00
parent 37a09c61bc
commit e917d6c7f2
4 changed files with 60 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/control.cpp
+24 -3
View File
@@ -70,9 +70,30 @@ void Ekf::controlFusionModes()
}
// 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
if ((_params.mag_fusion_type == USE_EV_YAW) && !_control_status.flags.ev_yaw && _control_status.flags.tilt_align) {
// check for a exernal vision measurement that has fallen behind the fusion time horizon
if (_time_last_imu - _time_last_ext_vision < 2 * EV_MAX_INTERVAL) {
// reset the yaw angle to the value from the observaton quaternion
// get the roll, pitch, yaw estimates from the quaternion states
matrix::Quaternion<float> q_init(_state.quat_nominal(0), _state.quat_nominal(1), _state.quat_nominal(2),
_state.quat_nominal(3));
matrix::Euler<float> euler_init(q_init);
// get initial yaw from the observation quaternion
extVisionSample ev_newest = _ext_vision_buffer.get_newest();
matrix::Quaternion<float> q_obs(ev_newest.quat(0), ev_newest.quat(1), ev_newest.quat(2), ev_newest.quat(3));
matrix::Euler<float> euler_obs(q_obs);
euler_init(2) = euler_obs(2);
// calculate initial quaternion states for the ekf
_state.quat_nominal = Quaternion(euler_init);
// flag the yaw as aligned
_control_status.flags.yaw_align = true;
// turn on fusion of external vision yaw measurements
_control_status.flags.ev_yaw = true;
}
}
// optical flow fusion mode selection logic