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

Ekf: centralize baro fusion startup

Browse Source
This commit is contained in:
bresch
2020-04-19 12:38:13 +02:00
committed by Mathieu Bresciani
parent 6f383248ca
commit 61763544b6
3 changed files with 26 additions and 39 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/control.cpp
+6 -38
View File
@@ -848,7 +848,7 @@ void Ekf::controlHeightSensorTimeouts()
_gps_hgt_intermittent);
if (reset_to_baro) {
setControlBaroHeight();
startBaroHgtFusion();
request_height_reset = true;
ECL_WARN_TIMESTAMPED("gps hgt timeout - reset to baro");
@@ -865,7 +865,7 @@ void Ekf::controlHeightSensorTimeouts()
ECL_WARN_TIMESTAMPED("rng hgt timeout - reset to rng hgt");
} else if (!_baro_hgt_faulty) {
setControlBaroHeight();
startBaroHgtFusion();
request_height_reset = true;
ECL_WARN_TIMESTAMPED("rng hgt timeout - reset to baro");
@@ -881,7 +881,7 @@ void Ekf::controlHeightSensorTimeouts()
ECL_WARN_TIMESTAMPED("ev hgt timeout - reset to ev hgt");
} else if (!_baro_hgt_faulty) {
setControlBaroHeight();
startBaroHgtFusion();
request_height_reset = true;
ECL_WARN_TIMESTAMPED("ev hgt timeout - reset to baro");
@@ -961,23 +961,9 @@ void Ekf::controlHeightFusion()
}
} else if (!do_range_aid && _baro_data_ready && !_baro_hgt_faulty) {
setControlBaroHeight();
startBaroHgtFusion();
fuse_height = true;
// we have just switched to using baro height, we don't need to set a height sensor offset
// since we track a separate _baro_hgt_offset
if (_control_status_prev.flags.baro_hgt != _control_status.flags.baro_hgt) {
_hgt_sensor_offset = 0.0f;
}
// Turn off ground effect compensation if it times out
if (_control_status.flags.gnd_effect) {
if (isTimedOut(_time_last_gnd_effect_on, GNDEFFECT_TIMEOUT)) {
_control_status.flags.gnd_effect = false;
}
}
} else if (_control_status.flags.gps_hgt && _gps_data_ready && !_gps_hgt_intermittent) {
// switch to gps if there was a reset to gps
fuse_height = true;
@@ -1012,14 +998,8 @@ void Ekf::controlHeightFusion()
}
} else if (_baro_data_ready && !_baro_hgt_faulty) {
setControlBaroHeight();
startBaroHgtFusion();
fuse_height = true;
// we have just switched to using baro height, we don't need to set a height sensor offset
// since we track a separate _baro_hgt_offset
if (_control_status_prev.flags.baro_hgt != _control_status.flags.baro_hgt) {
_hgt_sensor_offset = 0.0f;
}
}
break;
@@ -1050,12 +1030,6 @@ void Ekf::controlHeightFusion()
} else if (_control_status.flags.baro_hgt && _baro_data_ready && !_baro_hgt_faulty) {
// switch to baro if there was a reset to baro
fuse_height = true;
// we have just switched to using baro height, we don't need to set a height sensor offset
// since we track a separate _baro_hgt_offset
if (_control_status_prev.flags.baro_hgt != _control_status.flags.baro_hgt) {
_hgt_sensor_offset = 0.0f;
}
}
break;
@@ -1072,12 +1046,6 @@ void Ekf::controlHeightFusion()
if (_control_status.flags.baro_hgt && _baro_data_ready && !_baro_hgt_faulty) {
// switch to baro if there was a reset to baro
fuse_height = true;
// we have just switched to using baro height, we don't need to set a height sensor offset
// since we track a separate _baro_hgt_offset
if (_control_status_prev.flags.baro_hgt != _control_status.flags.baro_hgt) {
_hgt_sensor_offset = 0.0f;
}
}
// determine if we should use the vertical position observation
@@ -1119,7 +1087,7 @@ void Ekf::controlHeightFusion()
Vector3f baro_hgt_obs_var;
// vertical position innovation - baro measurement has opposite sign to earth z axis
_baro_hgt_innov(2) = _state.pos(2) + _baro_sample_delayed.hgt - _baro_hgt_offset - _hgt_sensor_offset;
_baro_hgt_innov(2) = _state.pos(2) + _baro_sample_delayed.hgt - _baro_hgt_offset;
// observation variance - user parameter defined
baro_hgt_obs_var(2) = sq(fmaxf(_params.baro_noise, 0.01f));
// innovation gate size
EKF/ekf.h
+2
View File
@@ -758,6 +758,8 @@ private:
void startMagHdgFusion();
void startMag3DFusion();
void startBaroHgtFusion();
// calculate the measurement variance for the optical flow sensor
float calcOptFlowMeasVar();
EKF/ekf_helper.cpp
+18 -1
View File
@@ -252,7 +252,7 @@ void Ekf::resetHeight()
const baroSample &baro_newest = _baro_buffer.get_newest();
if (!_baro_hgt_faulty) {
_state.pos(2) = _hgt_sensor_offset - baro_newest.hgt + _baro_hgt_offset;
_state.pos(2) = -baro_newest.hgt + _baro_hgt_offset;
// the state variance is the same as the observation
P.uncorrelateCovarianceSetVariance<1>(9, sq(_params.baro_noise));
@@ -1470,6 +1470,23 @@ void Ekf::startMag3DFusion()
}
}
void Ekf::startBaroHgtFusion()
{
setControlBaroHeight();
// We don't need to set a height sensor offset
// since we track a separate _baro_hgt_offset
_hgt_sensor_offset = 0.0f;
// Turn off ground effect compensation if it times out
if (_control_status.flags.gnd_effect) {
if (isTimedOut(_time_last_gnd_effect_on, GNDEFFECT_TIMEOUT)) {
_control_status.flags.gnd_effect = false;
}
}
}
// update the rotation matrix which rotates EV measurements into the EKF's navigation frame
void Ekf::calcExtVisRotMat()
{