diff --git a/msg/estimator_aid_source_1d.msg b/msg/estimator_aid_source_1d.msg index 2a4f8a07a1..b1c67c30f5 100644 --- a/msg/estimator_aid_source_1d.msg +++ b/msg/estimator_aid_source_1d.msg @@ -20,4 +20,5 @@ bool fused # true if the sample was successfully fused # TOPICS estimator_aid_source_1d # TOPICS estimator_aid_src_baro_hgt estimator_aid_src_rng_hgt estimator_aid_src_airspeed +# TOPICS estimator_aid_src_fake_hgt # TOPICS estimator_aid_src_mag_heading estimator_aid_src_gnss_yaw estimator_aid_src_ev_yaw diff --git a/msg/estimator_status_flags.msg b/msg/estimator_status_flags.msg index d2b52cfcb2..2905011582 100644 --- a/msg/estimator_status_flags.msg +++ b/msg/estimator_status_flags.msg @@ -37,6 +37,7 @@ bool cs_inertial_dead_reckoning # 29 - true if we are no longer fusing measureme bool cs_wind_dead_reckoning # 30 - true if we are navigationg reliant on wind relative measurements bool cs_rng_kin_consistent # 31 - true when the range finder kinematic consistency check is passing bool cs_fake_pos # 32 - true when fake position measurements are being fused +bool cs_fake_hgt # 33 - true when fake height measurements are being fused # fault status uint32 fault_status_changes # number of filter fault status (fs) changes diff --git a/src/modules/ekf2/CMakeLists.txt b/src/modules/ekf2/CMakeLists.txt index e9bb67b8a2..06a78d6809 100644 --- a/src/modules/ekf2/CMakeLists.txt +++ b/src/modules/ekf2/CMakeLists.txt @@ -59,6 +59,7 @@ px4_add_module( EKF/EKFGSF_yaw.cpp EKF/estimator_interface.cpp EKF/ev_height_control.cpp + EKF/fake_height_control.cpp EKF/fake_pos_control.cpp EKF/gnss_height_control.cpp EKF/gps_checks.cpp diff --git a/src/modules/ekf2/EKF/CMakeLists.txt b/src/modules/ekf2/EKF/CMakeLists.txt index bfd7111257..2a71b85056 100644 --- a/src/modules/ekf2/EKF/CMakeLists.txt +++ b/src/modules/ekf2/EKF/CMakeLists.txt @@ -43,6 +43,7 @@ add_library(ecl_EKF EKFGSF_yaw.cpp estimator_interface.cpp ev_height_control.cpp + fake_height_control.cpp fake_pos_control.cpp gnss_height_control.cpp gps_checks.cpp diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index 478594e595..f0502415af 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -533,6 +533,7 @@ union filter_control_status_u { uint64_t wind_dead_reckoning : 1; ///< 30 - true if we are navigationg reliant on wind relative measurements uint64_t rng_kin_consistent : 1; ///< 31 - true when the range finder kinematic consistency check is passing uint64_t fake_pos : 1; ///< 32 - true when fake position measurements are being fused + uint64_t fake_hgt : 1; ///< 33 - true when fake height measurements are being fused } flags; uint64_t value; }; diff --git a/src/modules/ekf2/EKF/control.cpp b/src/modules/ekf2/EKF/control.cpp index d11f40a7d4..f2bde288c8 100644 --- a/src/modules/ekf2/EKF/control.cpp +++ b/src/modules/ekf2/EKF/control.cpp @@ -190,6 +190,7 @@ void Ekf::controlFusionModes() // Fake position measurement for constraining drift when no other velocity or position measurements controlFakePosFusion(); + controlFakeHgtFusion(); // check if we are no longer fusing measurements that directly constrain velocity drift update_deadreckoning_status(); @@ -521,7 +522,7 @@ void Ekf::controlOpticalFlowFusion() // but use a relaxed time criteria to enable it to coast through bad range finder data if (isRecent(_time_last_hagl_fuse, (uint64_t)10e6)) { fuseOptFlow(); - _last_known_posNE = _state.pos.xy(); + _last_known_pos.xy() = _state.pos.xy(); } _flow_data_ready = false; diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 87e9065785..71883f1a09 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -402,6 +402,7 @@ public: const auto &aid_src_baro_hgt() const { return _aid_src_baro_hgt; } const auto &aid_src_rng_hgt() const { return _aid_src_rng_hgt; } + const auto &aid_src_fake_hgt() const { return _aid_src_fake_hgt; } const auto &aid_src_fake_pos() const { return _aid_src_fake_pos; } const auto &aid_src_ev_yaw() const { return _aid_src_ev_yaw; } @@ -479,7 +480,7 @@ private: uint64_t _time_last_healthy_rng_data{0}; uint8_t _nb_gps_yaw_reset_available{0}; ///< remaining number of resets allowed before switching to another aiding source - Vector2f _last_known_posNE{}; ///< last known local NE position vector (m) + Vector3f _last_known_pos{}; ///< last known local position vector (m) uint64_t _time_acc_bias_check{0}; ///< last time the accel bias check passed (uSec) uint64_t _delta_time_baro_us{0}; ///< delta time between two consecutive delayed baro samples from the buffer (uSec) @@ -549,6 +550,7 @@ private: estimator_aid_source_1d_s _aid_src_airspeed{}; estimator_aid_source_2d_s _aid_src_fake_pos{}; + estimator_aid_source_1d_s _aid_src_fake_hgt{}; estimator_aid_source_1d_s _aid_src_ev_yaw{}; @@ -932,6 +934,13 @@ private: // control fusion of fake position observations to constrain drift void controlFakePosFusion(); + void controlFakeHgtFusion(); + void startFakeHgtFusion(); + void resetFakeHgtFusion(); + void resetHeightToLastKnown(); + void stopFakeHgtFusion(); + void fuseFakeHgt(); + void controlZeroVelocityUpdate(); void controlZeroInnovationHeadingUpdate(); diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index 5a8ab7a7ac..acf70a010c 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -179,7 +179,7 @@ void Ekf::resetHorizontalPositionToOpticalFlow() resetHorizontalPositionTo(Vector2f(0.f, 0.f)); } else { - resetHorizontalPositionTo(_last_known_posNE); + resetHorizontalPositionTo(_last_known_pos.xy()); } // estimate is relative to initial position in this mode, so we start with zero error. @@ -191,7 +191,7 @@ void Ekf::resetHorizontalPositionToLastKnown() _information_events.flags.reset_pos_to_last_known = true; ECL_INFO("reset position to last known position"); // Used when falling back to non-aiding mode of operation - resetHorizontalPositionTo(_last_known_posNE); + resetHorizontalPositionTo(_last_known_pos.xy()); P.uncorrelateCovarianceSetVariance<2>(7, sq(_params.pos_noaid_noise)); } diff --git a/src/modules/ekf2/EKF/estimator_interface.cpp b/src/modules/ekf2/EKF/estimator_interface.cpp index 070ede1834..fcb890c5c7 100644 --- a/src/modules/ekf2/EKF/estimator_interface.cpp +++ b/src/modules/ekf2/EKF/estimator_interface.cpp @@ -584,6 +584,11 @@ int EstimatorInterface::getNumberOfActiveVerticalPositionAidingSources() const + int(_control_status.flags.ev_hgt); } +bool EstimatorInterface::isVerticalAidingActive() const +{ + return isVerticalPositionAidingActive() || isVerticalVelocityAidingActive(); +} + bool EstimatorInterface::isVerticalVelocityAidingActive() const { return getNumberOfActiveVerticalVelocityAidingSources() > 0; diff --git a/src/modules/ekf2/EKF/estimator_interface.h b/src/modules/ekf2/EKF/estimator_interface.h index 567ef7e522..519bcf6a1d 100644 --- a/src/modules/ekf2/EKF/estimator_interface.h +++ b/src/modules/ekf2/EKF/estimator_interface.h @@ -178,6 +178,7 @@ public: // Return true if at least one source of horizontal aiding is active // the flags considered are opt_flow, gps, ev_vel and ev_pos bool isHorizontalAidingActive() const; + bool isVerticalAidingActive() const; int getNumberOfActiveHorizontalAidingSources() const; diff --git a/src/modules/ekf2/EKF/fake_height_control.cpp b/src/modules/ekf2/EKF/fake_height_control.cpp new file mode 100644 index 0000000000..10f5d01a40 --- /dev/null +++ b/src/modules/ekf2/EKF/fake_height_control.cpp @@ -0,0 +1,143 @@ +/**************************************************************************** + * + * 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 fake_height_control.cpp + * Control functions for ekf fake height fusion + */ + +#include "ekf.h" + +void Ekf::controlFakeHgtFusion() +{ + auto &fake_hgt = _aid_src_fake_hgt; + + // clear + resetEstimatorAidStatusFlags(fake_hgt); + + // If we aren't doing any aiding, fake position measurements at the last known vertical position to constrain drift + const bool fake_hgt_data_ready = isTimedOut(fake_hgt.time_last_fuse, (uint64_t)2e5); // Fuse fake height at a limited rate + + if (fake_hgt_data_ready) { + const bool continuing_conditions_passing = !isVerticalAidingActive(); + const bool starting_conditions_passing = continuing_conditions_passing; + + if (_control_status.flags.fake_hgt) { + if (continuing_conditions_passing) { + fuseFakeHgt(); + + const bool is_fusion_failing = isTimedOut(fake_hgt.time_last_fuse, (uint64_t)4e5); + + if (is_fusion_failing) { + resetFakeHgtFusion(); + } + + } else { + stopFakeHgtFusion(); + } + + } else { + if (starting_conditions_passing) { + startFakeHgtFusion(); + } + } + } +} + +void Ekf::startFakeHgtFusion() +{ + if (!_control_status.flags.fake_hgt) { + ECL_INFO("start fake height fusion"); + _control_status.flags.fake_hgt = true; + resetFakeHgtFusion(); + } +} + +void Ekf::resetFakeHgtFusion() +{ + ECL_INFO("reset fake height fusion"); + _last_known_pos(2) = _state.pos(2); + + resetVerticalVelocityToZero(); + resetHeightToLastKnown(); + + _aid_src_fake_hgt.time_last_fuse = _imu_sample_delayed.time_us; +} + +void Ekf::resetHeightToLastKnown() +{ + _information_events.flags.reset_pos_to_last_known = true; + ECL_INFO("reset height to last known"); + resetVerticalPositionTo(_last_known_pos(2)); + P.uncorrelateCovarianceSetVariance<1>(9, sq(_params.pos_noaid_noise)); +} + +void Ekf::stopFakeHgtFusion() +{ + if (_control_status.flags.fake_hgt) { + ECL_INFO("stop fake height fusion"); + _control_status.flags.fake_hgt = false; + + resetEstimatorAidStatus(_aid_src_fake_hgt); + } +} + +void Ekf::fuseFakeHgt() +{ + const float obs_var = sq(_params.pos_noaid_noise); + + const float innov_gate = 3.f; + + auto &fake_hgt = _aid_src_fake_hgt; + + fake_hgt.observation = _last_known_pos(2); + fake_hgt.observation_variance = obs_var; + + fake_hgt.innovation = _state.pos(2) - _last_known_pos(2); + fake_hgt.innovation_variance = P(9, 9) + obs_var; + + setEstimatorAidStatusTestRatio(fake_hgt, innov_gate); + + // always protect against extreme values that could result in a NaN + fake_hgt.fusion_enabled = fake_hgt.test_ratio < sq(100.0f / innov_gate); + + // fuse + if (fake_hgt.fusion_enabled && !fake_hgt.innovation_rejected) { + if (fuseVelPosHeight(fake_hgt.innovation, fake_hgt.innovation_variance, 5)) { + fake_hgt.fused = true; + fake_hgt.time_last_fuse = _imu_sample_delayed.time_us; + } + } + + fake_hgt.timestamp_sample = _imu_sample_delayed.time_us; +} diff --git a/src/modules/ekf2/EKF/fake_pos_control.cpp b/src/modules/ekf2/EKF/fake_pos_control.cpp index ca70c2fd99..74bcdb9878 100644 --- a/src/modules/ekf2/EKF/fake_pos_control.cpp +++ b/src/modules/ekf2/EKF/fake_pos_control.cpp @@ -94,7 +94,7 @@ void Ekf::startFakePosFusion() void Ekf::resetFakePosFusion() { ECL_INFO("reset fake position fusion"); - _last_known_posNE = _state.pos.xy(); + _last_known_pos.xy() = _state.pos.xy(); resetHorizontalPositionToLastKnown(); resetHorizontalVelocityToZero(); @@ -134,10 +134,10 @@ void Ekf::fuseFakePosition() auto &fake_pos = _aid_src_fake_pos; for (int i = 0; i < 2; i++) { - fake_pos.observation[i] = _last_known_posNE(i); + fake_pos.observation[i] = _last_known_pos(i); fake_pos.observation_variance[i] = obs_var(i); - fake_pos.innovation[i] = _state.pos(i) - _last_known_posNE(i); + fake_pos.innovation[i] = _state.pos(i) - _last_known_pos(i); fake_pos.innovation_variance[i] = P(7 + i, 7 + i) + obs_var(i); } diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 08e67fa24c..5f8fb5219c 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -722,6 +722,7 @@ void EKF2::PublishAidSourceStatus(const hrt_abstime ×tamp) // fake position PublishAidSourceStatus(_ekf.aid_src_fake_pos(), _status_fake_pos_pub_last, _estimator_aid_src_fake_pos_pub); + PublishAidSourceStatus(_ekf.aid_src_fake_hgt(), _status_fake_hgt_pub_last, _estimator_aid_src_fake_hgt_pub); // EV yaw PublishAidSourceStatus(_ekf.aid_src_ev_yaw(), _status_ev_yaw_pub_last, _estimator_aid_src_ev_yaw_pub); @@ -1429,6 +1430,7 @@ void EKF2::PublishStatusFlags(const hrt_abstime ×tamp) status_flags.cs_wind_dead_reckoning = _ekf.control_status_flags().wind_dead_reckoning; status_flags.cs_rng_kin_consistent = _ekf.control_status_flags().rng_kin_consistent; status_flags.cs_fake_pos = _ekf.control_status_flags().fake_pos; + status_flags.cs_fake_hgt = _ekf.control_status_flags().fake_hgt; status_flags.fault_status_changes = _filter_fault_status_changes; status_flags.fs_bad_mag_x = _ekf.fault_status_flags().bad_mag_x; diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index 239d033adb..79f37f1a68 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -263,6 +263,7 @@ private: hrt_abstime _status_baro_hgt_pub_last{0}; hrt_abstime _status_rng_hgt_pub_last{0}; + hrt_abstime _status_fake_hgt_pub_last{0}; hrt_abstime _status_fake_pos_pub_last{0}; hrt_abstime _status_ev_yaw_pub_last{0}; @@ -346,6 +347,7 @@ private: uORB::PublicationMulti _estimator_aid_src_baro_hgt_pub{ORB_ID(estimator_aid_src_baro_hgt)}; uORB::PublicationMulti _estimator_aid_src_rng_hgt_pub{ORB_ID(estimator_aid_src_rng_hgt)}; + uORB::PublicationMulti _estimator_aid_src_fake_hgt_pub{ORB_ID(estimator_aid_src_fake_hgt)}; uORB::PublicationMulti _estimator_aid_src_fake_pos_pub{ORB_ID(estimator_aid_src_fake_pos)}; uORB::PublicationMulti _estimator_aid_src_gnss_yaw_pub{ORB_ID(estimator_aid_src_gnss_yaw)}; diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 07cff827a5..a835ddfeba 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -181,6 +181,7 @@ void LoggedTopics::add_default_topics() // add_optional_topic_multi("estimator_aid_src_airspeed", 100, MAX_ESTIMATOR_INSTANCES); // add_optional_topic_multi("estimator_aid_src_baro_hgt", 100, MAX_ESTIMATOR_INSTANCES); // add_optional_topic_multi("estimator_aid_src_rng_hgt", 100, MAX_ESTIMATOR_INSTANCES); + // add_optional_topic_multi("estimator_aid_src_fake_hgt", 100, MAX_ESTIMATOR_INSTANCES); // add_optional_topic_multi("estimator_aid_src_fake_pos", 100, MAX_ESTIMATOR_INSTANCES); // add_optional_topic_multi("estimator_aid_src_gnss_yaw", 100, MAX_ESTIMATOR_INSTANCES); // add_optional_topic_multi("estimator_aid_src_gnss_vel", 100, MAX_ESTIMATOR_INSTANCES); @@ -264,6 +265,7 @@ void LoggedTopics::add_default_topics() add_optional_topic_multi("estimator_aid_src_airspeed", 0, MAX_ESTIMATOR_INSTANCES); add_optional_topic_multi("estimator_aid_src_baro_hgt", 0, MAX_ESTIMATOR_INSTANCES); add_optional_topic_multi("estimator_aid_src_rng_hgt", 0, MAX_ESTIMATOR_INSTANCES); + add_optional_topic_multi("estimator_aid_src_fake_hgt", 0, MAX_ESTIMATOR_INSTANCES); add_optional_topic_multi("estimator_aid_src_fake_pos", 0, MAX_ESTIMATOR_INSTANCES); add_optional_topic_multi("estimator_aid_src_gnss_yaw", 0, MAX_ESTIMATOR_INSTANCES); add_optional_topic_multi("estimator_aid_src_gnss_vel", 0, MAX_ESTIMATOR_INSTANCES);