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

Various small non-functional improvements

Browse Source
This commit is contained in:
kamilritz
2019-12-26 16:20:11 +01:00
committed by Mathieu Bresciani
parent deeac03d6a
commit 1fcfd394dd
3 changed files with 16 additions and 42 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/control.cpp
+11 -37
View File
@@ -789,10 +789,9 @@ void Ekf::controlHeightSensorTimeouts()
// check if height has been inertial deadreckoning for too long
bool hgt_fusion_timeout = ((_time_last_imu - _time_last_hgt_fuse) > (uint64_t)5e6);
// reset the vertical position and velocity states
if (hgt_fusion_timeout || continuous_bad_accel_hgt) {
// boolean that indicates we will do a height reset
bool reset_height = false;
bool request_height_reset = false;
// handle the case where we are using baro for height
if (_control_status.flags.baro_hgt) {
@@ -816,8 +815,7 @@ void Ekf::controlHeightSensorTimeouts()
setControlGPSHeight();
// request a reset
reset_height = true;
request_height_reset = true;
ECL_WARN_TIMESTAMPED("baro hgt timeout - reset to GPS");
} else if (baro_data_available) {
@@ -826,15 +824,9 @@ void Ekf::controlHeightSensorTimeouts()
setControlBaroHeight();
// request a reset
reset_height = true;
request_height_reset = true;
ECL_WARN_TIMESTAMPED("baro hgt timeout - reset to baro");
} else {
// we have nothing we can reset to
// deny a reset
reset_height = false;
}
}
@@ -861,21 +853,15 @@ void Ekf::controlHeightSensorTimeouts()
setControlBaroHeight();
// request a reset
reset_height = true;
request_height_reset = true;
ECL_WARN_TIMESTAMPED("gps hgt timeout - reset to baro");
} else if (!_gps_hgt_intermittent) {
setControlGPSHeight();
// request a reset
reset_height = true;
request_height_reset = true;
ECL_WARN_TIMESTAMPED("gps hgt timeout - reset to GPS");
} else {
// we have nothing to reset to
reset_height = false;
}
}
@@ -890,8 +876,7 @@ void Ekf::controlHeightSensorTimeouts()
setControlRangeHeight();
// request a reset
reset_height = true;
request_height_reset = true;
ECL_WARN_TIMESTAMPED("rng hgt timeout - reset to rng hgt");
} else if (baro_data_available) {
@@ -900,14 +885,9 @@ void Ekf::controlHeightSensorTimeouts()
setControlBaroHeight();
// request a reset
reset_height = true;
request_height_reset = true;
ECL_WARN_TIMESTAMPED("rng hgt timeout - reset to baro");
} else {
// we have nothing to reset to
reset_height = false;
}
}
@@ -924,8 +904,7 @@ void Ekf::controlHeightSensorTimeouts()
if (ev_data_available) {
setControlEVHeight();
// request a reset
reset_height = true;
request_height_reset = true;
ECL_WARN_TIMESTAMPED("ev hgt timeout - reset to ev hgt");
} else if (baro_data_available) {
@@ -934,19 +913,14 @@ void Ekf::controlHeightSensorTimeouts()
setControlBaroHeight();
// request a reset
reset_height = true;
request_height_reset = true;
ECL_WARN_TIMESTAMPED("ev hgt timeout - reset to baro");
} else {
// we have nothing to reset to
reset_height = false;
}
}
// Reset vertical position and velocity states to the last measurement
if (reset_height) {
if (request_height_reset) {
resetHeight();
// Reset the timout timer
_time_last_hgt_fuse = _time_last_imu;
EKF/estimator_interface.cpp
+1 -1
View File
@@ -367,7 +367,7 @@ 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 = 0.5f * 1e-6f * (float)_min_obs_interval_us;
const float delta_time_min = 0.5e-6f * (float)_min_obs_interval_us;
bool delta_time_good = delta_time >= delta_time_min;
bool flow_magnitude_good = true;
EKF/vel_pos_fusion.cpp
+4 -4
View File
@@ -51,8 +51,8 @@ bool Ekf::fuseHorizontalVelocity(const Vector3f &innov, const Vector2f &innov_ga
{
innov_var(0) = P(4,4) + obs_var(0);
innov_var(1) = P(5,5) + 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)));
test_ratio(0) = fmaxf(sq(innov(0)) / (sq(innov_gate(0)) * innov_var(0)),
sq(innov(1)) / (sq(innov_gate(0)) * innov_var(1)));
const bool innov_check_pass = (test_ratio(0) <= 1.0f);
if (innov_check_pass)
@@ -95,8 +95,8 @@ bool Ekf::fuseHorizontalPosition(const Vector3f &innov, const Vector2f &innov_ga
{
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)));
test_ratio(0) = fmaxf(sq(innov(0)) / (sq(innov_gate(0)) * innov_var(0)),
sq(innov(1)) / (sq(innov_gate(0)) * innov_var(1)));
const bool innov_check_pass = (test_ratio(0) <= 1.0f) || !_control_status.flags.tilt_align;
if (innov_check_pass) {