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

Improve readibility

Browse Source
This commit is contained in:
kamilritz
2020-01-08 09:00:31 +01:00
committed by Mathieu Bresciani
parent 3daf25763e
commit 0b18143745
2 changed files with 13 additions and 19 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/ekf_helper.cpp
+3 -7
View File
@@ -566,7 +566,6 @@ bool Ekf::resetMagHeading(const Vector3f &mag_init, bool increase_yaw_var, bool
// Set the yaw angle to zero and calculate the rotation matrix from body to earth frame
euler321(2) = 0.0f;
Dcmf R_to_earth(euler321);
// calculate the observed yaw angle
if (_control_status.flags.ev_yaw) {
@@ -576,6 +575,7 @@ bool Ekf::resetMagHeading(const Vector3f &mag_init, bool increase_yaw_var, bool
euler321(2) = atan2f(R_to_earth_ev(1, 0), R_to_earth_ev(0, 0));
} else if (_params.mag_fusion_type <= MAG_FUSE_TYPE_3D) {
const Dcmf R_to_earth(euler321);
// rotate the magnetometer measurements into earth frame using a zero yaw angle
const Vector3f mag_earth_pred = R_to_earth * mag_init;
// the angle of the projection onto the horizontal gives the yaw angle
@@ -690,14 +690,12 @@ bool Ekf::resetMagHeading(const Vector3f &mag_init, bool increase_yaw_var, bool
// update quaternion states
_state.quat_nominal = quat_after_reset;
_R_to_earth = Dcmf(_state.quat_nominal);
uncorrelateQuatStates();
// record the state change
_state_reset_status.quat_change = q_error;
// update transformation matrix from body to world frame using the current estimate
_R_to_earth = Dcmf(_state.quat_nominal);
// reset the rotation from the EV to EKF frame of reference if it is being used
if ((_params.fusion_mode & MASK_ROTATE_EV) && !_control_status.flags.ev_yaw) {
resetExtVisRotMat();
@@ -1641,9 +1639,7 @@ void Ekf::calcExtVisRotMat()
// convert to a delta angle and apply a spike and low pass filter
AxisAnglef rot_vec(q_error);
float rot_vec_norm = rot_vec.norm();
if (rot_vec_norm > 1e-6f) {
if (rot_vec.norm() > 1e-6f) {
// apply an input limiter to protect from spikes
const Vector3f _input_delta_vec = rot_vec - _ev_rot_vec_filt;
EKF/estimator_interface.cpp
+10 -12
View File
@@ -367,24 +367,22 @@ void EstimatorInterface::setOpticalFlowData(uint64_t time_usec, flow_message *fl
// check if enough integration time and fail if integration time is less than 50%
// of min arrival interval because too much data is being lost
float delta_time = 1e-6f * (float)flow->dt; // in seconds
float delta_time_min = 5e-7f * (float)_min_obs_interval_us;
float delta_time_min = 0.5f * 1e-6f * (float)_min_obs_interval_us;
bool delta_time_good = delta_time >= delta_time_min;
if (!delta_time_good) {
bool flow_magnitude_good = true;
if (delta_time_good) {
// check magnitude is within sensor limits
// use this to prevent use of a saturated flow sensor
// when there are other aiding sources available
const float flow_rate_magnitude = flow->flowdata.norm() / delta_time;
flow_magnitude_good = (flow_rate_magnitude <= _flow_max_rate);
} else {
// protect against overflow caused by division with very small delta_time
delta_time = delta_time_min;
}
// check magnitude is within sensor limits
// use this to prevent use of a saturated flow sensor when there are other aiding sources available
float flow_rate_magnitude;
bool flow_magnitude_good = true;
if (delta_time_good) {
flow_rate_magnitude = flow->flowdata.norm() / delta_time;
flow_magnitude_good = (flow_rate_magnitude <= _flow_max_rate);
}
const bool relying_on_flow = !_control_status.flags.gps && !_control_status.flags.ev_pos && !_control_status.flags.ev_vel;
const bool flow_quality_good = (flow->quality >= _params.flow_qual_min);