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

Clean up white space

Browse Source
This commit is contained in:
kamilritz 2019-12-25 17:23:25 +01:00 committed by Mathieu Bresciani
parent 48787c0160
commit 36da8d82c8
3 changed files with 21 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
EKF/estimator_interface.cpp
View File

@ -451,7 +451,6 @@ void EstimatorInterface::setExtVisionData(uint64_t time_usec, ext_vision_message
_time_last_ext_vision = time_usec;
_ext_vision_buffer.push(ev_sample_new);
}
}

2
EKF/optflow_fusion.cpp
View File

@ -541,7 +541,7 @@ bool Ekf::calcOptFlowBodyRateComp()
} else {
// Use the EKF gyro data if optical flow sensor gyro data is not available
// for clarification of the sign see definition of flowSample and imuSample in common.h
_flow_sample_delayed.gyroXYZ = - _imu_del_ang_of;
_flow_sample_delayed.gyroXYZ = -_imu_del_ang_of;
_flow_gyro_bias.zero();
}

40
EKF/vel_pos_fusion.cpp
View File

@ -93,29 +93,29 @@ bool Ekf::fuseVerticalVelocity(const Vector3f &innov, const Vector2f &innov_gate
bool Ekf::fuseHorizontalPosition(const Vector3f &innov, const Vector2f &innov_gate,
const Vector3f &obs_var, Vector3f &innov_var, Vector2f &test_ratio)
{
innov_var(0) = P(7,7) + obs_var(0);
innov_var(1) = P(8,8) + obs_var(1);
test_ratio(0) = fmaxf( sq(innov(0)) / (sq(innov_gate(0)) * innov_var(0)),
sq(innov(1)) / (sq(innov_gate(0)) * innov_var(1)));
innov_var(0) = P(7,7) + obs_var(0);
innov_var(1) = P(8,8) + obs_var(1);
test_ratio(0) = fmaxf( sq(innov(0)) / (sq(innov_gate(0)) * innov_var(0)),
sq(innov(1)) / (sq(innov_gate(0)) * innov_var(1)));
bool innov_check_pass = (test_ratio(0) <= 1.0f) || !_control_status.flags.tilt_align;
if (innov_check_pass) {
if (!_fuse_hpos_as_odom) {
_time_last_hor_pos_fuse = _time_last_imu;
const bool innov_check_pass = (test_ratio(0) <= 1.0f) || !_control_status.flags.tilt_align;
if (innov_check_pass) {
if (!_fuse_hpos_as_odom) {
_time_last_hor_pos_fuse = _time_last_imu;
} else {
_time_last_delpos_fuse = _time_last_imu;
}
_innov_check_fail_status.flags.reject_hor_pos = false;
fuseVelPosHeight(innov(0),innov_var(0),3);
fuseVelPosHeight(innov(1),innov_var(1),4);
return true;
}else{
_innov_check_fail_status.flags.reject_hor_pos = true;
return false;
} else {
_time_last_delpos_fuse = _time_last_imu;
}
_innov_check_fail_status.flags.reject_hor_pos = false;
fuseVelPosHeight(innov(0),innov_var(0),3);
fuseVelPosHeight(innov(1),innov_var(1),4);
return true;
}else{
_innov_check_fail_status.flags.reject_hor_pos = true;
return false;
}
}
bool Ekf::fuseVerticalPosition(const Vector3f &innov, const Vector2f &innov_gate,