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

Enable local frame alignment also without using it

Browse Source
This commit is contained in:
kamilritz
2019-08-29 08:34:10 +10:00
committed by Paul Riseborough
parent ea352a6631
commit 933c32c921
2 changed files with 10 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/control.cpp
+9 -2
View File
@@ -167,14 +167,21 @@ void Ekf::controlExternalVisionFusion()
// if the ev data is not in a NED reference frame, then the transformation between EV and EKF navigation frames
// needs to be calculated and the observations rotated into the EKF frame of reference
if ((_params.fusion_mode & MASK_ROTATE_EV) && (_params.fusion_mode & MASK_USE_EVPOS) && !_control_status.flags.ev_yaw) {
if ((_params.fusion_mode & MASK_ROTATE_EV) && !_control_status.flags.ev_yaw) {
// rotate EV measurements into the EKF Navigation frame
calcExtVisRotMat();
}
// reset external vision rotation matrix independent if is actually used for debugging purposes
if ((_time_last_imu - _time_last_ext_vision) < (2 * EV_MAX_INTERVAL)
&& (_params.fusion_mode & MASK_ROTATE_EV) && !(_params.fusion_mode & MASK_USE_EVYAW)){
// Reset transformation between EV and EKF navigation frames
resetExtVisRotMat();
}
// 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) {
&& _control_status.flags.yaw_align) {
// check for a external vision measurement that has fallen behind the fusion time horizon
if ((_time_last_imu - _time_last_ext_vision) < (2 * EV_MAX_INTERVAL)) {