fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-21 05:47:35 +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-20 15:16:19 +02: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 // 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 // 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 // rotate EV measurements into the EKF Navigation frame
calcExtVisRotMat(); 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 // external vision position aiding selection logic
if ((_params.fusion_mode & MASK_USE_EVPOS) && !_control_status.flags.ev_pos && _control_status.flags.tilt_align 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 // 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)) { if ((_time_last_imu - _time_last_ext_vision) < (2 * EV_MAX_INTERVAL)) {
EKF/ekf_helper.cpp
+1 -1
View File
@@ -731,7 +731,7 @@ bool Ekf::resetMagHeading(Vector3f &mag_init, bool increase_yaw_var, bool update
_R_to_earth = quat_to_invrotmat(_state.quat_nominal); _R_to_earth = quat_to_invrotmat(_state.quat_nominal);
// reset the rotation from the EV to EKF frame of reference if it is being used // reset the rotation from the EV to EKF frame of reference if it is being used
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) {
resetExtVisRotMat(); resetExtVisRotMat();
} }