diff --git a/src/modules/ekf2/CMakeLists.txt b/src/modules/ekf2/CMakeLists.txt index 920c3971c5..95dbd5b9dd 100644 --- a/src/modules/ekf2/CMakeLists.txt +++ b/src/modules/ekf2/CMakeLists.txt @@ -57,6 +57,7 @@ px4_add_module( EKF/ekf_helper.cpp EKF/EKFGSF_yaw.cpp EKF/estimator_interface.cpp + EKF/ev_height_control.cpp EKF/fake_pos_control.cpp EKF/gps_checks.cpp EKF/gps_control.cpp diff --git a/src/modules/ekf2/EKF/CMakeLists.txt b/src/modules/ekf2/EKF/CMakeLists.txt index 9d0ba294be..aeb87d2c46 100644 --- a/src/modules/ekf2/EKF/CMakeLists.txt +++ b/src/modules/ekf2/EKF/CMakeLists.txt @@ -42,6 +42,7 @@ add_library(ecl_EKF ekf_helper.cpp EKFGSF_yaw.cpp estimator_interface.cpp + ev_height_control.cpp fake_pos_control.cpp gps_checks.cpp gps_control.cpp diff --git a/src/modules/ekf2/EKF/control.cpp b/src/modules/ekf2/EKF/control.cpp index db5861d2ca..a9cb737415 100644 --- a/src/modules/ekf2/EKF/control.cpp +++ b/src/modules/ekf2/EKF/control.cpp @@ -793,46 +793,6 @@ void Ekf::controlRangeHeightFusion() } } -void Ekf::controlEvHeightFusion() -{ - if (!(_params.height_sensor_ref == HeightSensor::EV)) { // TODO: replace by EV control parameter - stopEvHgtFusion(); - return; - } - - _ev_hgt_b_est.predict(_dt_ekf_avg); - - const bool ev_intermittent = !isRecent(_time_last_ext_vision, 2 * EV_MAX_INTERVAL); - - if (_ev_data_ready) { - const bool continuing_conditions_passing = !ev_intermittent; - 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 - resetHeightToRng(); - resetVerticalVelocityToZero(); - } - - } else { - stopEvHgtFusion(); - } - - } else { - if (starting_conditions_passing) { - startEvHgtFusion(); - } - } - - } else if (_control_status.flags.ev_hgt && ev_intermittent) { - stopEvHgtFusion(); - } -} - void Ekf::controlAirDataFusion() { // control activation and initialisation/reset of wind states required for airspeed fusion diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index 5457d5e480..1e6399179e 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -294,21 +294,6 @@ void Ekf::resetHeightToRng() _ev_hgt_b_est.setBias(_ev_hgt_b_est.getBias() - _state_reset_status.posD_change); } -void Ekf::resetHeightToEv() -{ - ECL_INFO("reset height to EV"); - _information_events.flags.reset_hgt_to_ev = true; - - resetVerticalPositionTo(_ev_sample_delayed.pos(2) - _ev_hgt_b_est.getBias()); - - // the state variance is the same as the observation - P.uncorrelateCovarianceSetVariance<1>(9, fmaxf(_ev_sample_delayed.posVar(2), sq(0.01f))); - - _baro_b_est.setBias(_baro_b_est.getBias() + _state_reset_status.posD_change); - _gps_hgt_b_est.setBias(_gps_hgt_b_est.getBias() + _state_reset_status.posD_change); - _rng_hgt_b_est.setBias(_rng_hgt_b_est.getBias() + _state_reset_status.posD_change); -} - void Ekf::resetVerticalVelocityToGps(const gpsSample &gps_sample_delayed) { resetVerticalVelocityTo(gps_sample_delayed.vel(2)); @@ -1357,37 +1342,6 @@ void Ekf::stopRngHgtFusion() } } -void Ekf::startEvHgtFusion() -{ - if (!_control_status.flags.ev_hgt) { - if (_params.height_sensor_ref == HeightSensor::EV) { - _rng_hgt_b_est.reset(); - _height_sensor_ref = HeightSensor::EV; - resetHeightToEv(); - - } else { - _ev_hgt_b_est.setBias(-_state.pos(2) + _ev_sample_delayed.pos(2)); - } - - _control_status.flags.ev_hgt = true; - _ev_hgt_b_est.setFusionActive(); - ECL_INFO("starting EV height fusion"); - } -} - -void Ekf::stopEvHgtFusion() -{ - if (_control_status.flags.ev_hgt) { - if (_height_sensor_ref == HeightSensor::EV) { - _height_sensor_ref = HeightSensor::UNKNOWN; - } - - _control_status.flags.ev_hgt = false; - _ev_hgt_b_est.setFusionInactive(); - ECL_INFO("stopping EV height fusion"); - } -} - float Ekf::getGpsHeightVariance() { // observation variance - receiver defined and parameter limited diff --git a/src/modules/ekf2/EKF/ev_height_control.cpp b/src/modules/ekf2/EKF/ev_height_control.cpp new file mode 100644 index 0000000000..71d8378cb9 --- /dev/null +++ b/src/modules/ekf2/EKF/ev_height_control.cpp @@ -0,0 +1,125 @@ +/**************************************************************************** + * + * Copyright (c) 2022 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file ev_height_control.cpp + * Control functions for ekf external vision height fusion + */ + +#include "ekf.h" + +void Ekf::controlEvHeightFusion() +{ + if (!(_params.height_sensor_ref == HeightSensor::EV)) { // TODO: replace by EV control parameter + stopEvHgtFusion(); + return; + } + + _ev_hgt_b_est.predict(_dt_ekf_avg); + + const bool ev_intermittent = !isRecent(_time_last_ext_vision, 2 * EV_MAX_INTERVAL); + + if (_ev_data_ready) { + const bool continuing_conditions_passing = !ev_intermittent; + 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 + resetHeightToEv(); + resetVerticalVelocityToZero(); + } + + } else { + stopEvHgtFusion(); + } + + } else { + if (starting_conditions_passing) { + startEvHgtFusion(); + } + } + + } else if (_control_status.flags.ev_hgt && ev_intermittent) { + stopEvHgtFusion(); + } +} + +void Ekf::startEvHgtFusion() +{ + if (!_control_status.flags.ev_hgt) { + if (_params.height_sensor_ref == HeightSensor::EV) { + _rng_hgt_b_est.reset(); + _height_sensor_ref = HeightSensor::EV; + resetHeightToEv(); + + } else { + _ev_hgt_b_est.setBias(-_state.pos(2) + _ev_sample_delayed.pos(2)); + } + + _control_status.flags.ev_hgt = true; + _ev_hgt_b_est.setFusionActive(); + ECL_INFO("starting EV height fusion"); + } +} + +void Ekf::resetHeightToEv() +{ + ECL_INFO("reset height to EV"); + _information_events.flags.reset_hgt_to_ev = true; + + resetVerticalPositionTo(_ev_sample_delayed.pos(2) - _ev_hgt_b_est.getBias()); + + // the state variance is the same as the observation + P.uncorrelateCovarianceSetVariance<1>(9, fmaxf(_ev_sample_delayed.posVar(2), sq(0.01f))); + + _baro_b_est.setBias(_baro_b_est.getBias() + _state_reset_status.posD_change); + _gps_hgt_b_est.setBias(_gps_hgt_b_est.getBias() + _state_reset_status.posD_change); + _rng_hgt_b_est.setBias(_rng_hgt_b_est.getBias() + _state_reset_status.posD_change); +} + +void Ekf::stopEvHgtFusion() +{ + if (_control_status.flags.ev_hgt) { + if (_height_sensor_ref == HeightSensor::EV) { + _height_sensor_ref = HeightSensor::UNKNOWN; + } + + _control_status.flags.ev_hgt = false; + _ev_hgt_b_est.setFusionInactive(); + ECL_INFO("stopping EV height fusion"); + } +}