dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Adds reset counter logic to EV height fusion

This commit is contained in:
Charles Cross 2022-09-23 09:52:45 -07:00 committed by Mathieu Bresciani
parent 779e738143
commit cfdaba35cc
3 changed files with 20 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
src/modules/ekf2/EKF/ekf.h
View File

@ -712,6 +712,7 @@ private:
void resetHeightToEv();
void resetVerticalVelocityToGps(const gpsSample &gps_sample);
void resetVerticalVelocityToEv(const extVisionSample &ev_sample);
void resetVerticalVelocityToZero();
// fuse optical flow line of sight rate measurements

8
src/modules/ekf2/EKF/ekf_helper.cpp
View File

@ -259,6 +259,14 @@ void Ekf::resetVerticalVelocityToGps(const gpsSample &gps_sample)
P.uncorrelateCovarianceSetVariance<1>(6, sq(1.5f * gps_sample.sacc));
}
void Ekf::resetVerticalVelocityToEv(const extVisionSample &ev_sample)
{
resetVerticalVelocityTo(ev_sample.vel(2));
// the state variance is the same as the observation
P.uncorrelateCovarianceSetVariance<1>(6, ev_sample.velVar(2));
}
void Ekf::resetVerticalVelocityToZero()
{
// we don't know what the vertical velocity is, so set it to zero

15
src/modules/ekf2/EKF/ev_height_control.cpp
View File

@ -50,17 +50,24 @@ void Ekf::controlEvHeightFusion()
const bool ev_intermittent = !isNewestSampleRecent(_time_last_ext_vision_buffer_push, 2 * EV_MAX_INTERVAL);
if (_ev_data_ready) {
const bool continuing_conditions_passing = !ev_intermittent;
const bool position_valid = PX4_ISFINITE(_ev_sample_delayed.pos(2));
const bool continuing_conditions_passing = !ev_intermittent && position_valid;
const bool starting_conditions_passing = continuing_conditions_passing;
if (_control_status.flags.ev_hgt) {
if (continuing_conditions_passing) {
fuseEvHgt();
if (isHeightResetRequired()) {
// All height sources are failing
const bool reset = (_ev_sample_delayed.reset_counter != _ev_sample_delayed_prev.reset_counter);
if (isHeightResetRequired() || reset ) {
resetHeightToEv();
resetVerticalVelocityToZero();
// If the sample has a valid vertical velocity estimate, use it
if (PX4_ISFINITE(_ev_sample_delayed.vel(2))) {
resetVerticalVelocityToEv(_ev_sample_delayed);
} else {
resetVerticalVelocityToZero();
}
}
} else {