diff --git a/src/modules/ekf2/CMakeLists.txt b/src/modules/ekf2/CMakeLists.txt index 48b9ab8b7e..be24c05d9d 100644 --- a/src/modules/ekf2/CMakeLists.txt +++ b/src/modules/ekf2/CMakeLists.txt @@ -54,6 +54,7 @@ px4_add_module( EKF/ekf_helper.cpp EKF/EKFGSF_yaw.cpp EKF/estimator_interface.cpp + EKF/fake_pos_control.cpp EKF/gps_checks.cpp EKF/gps_control.cpp EKF/gps_fusion.cpp diff --git a/src/modules/ekf2/EKF/CMakeLists.txt b/src/modules/ekf2/EKF/CMakeLists.txt index 4515bb9b32..8a92a22d6f 100644 --- a/src/modules/ekf2/EKF/CMakeLists.txt +++ b/src/modules/ekf2/EKF/CMakeLists.txt @@ -41,6 +41,7 @@ add_library(ecl_EKF ekf_helper.cpp EKFGSF_yaw.cpp estimator_interface.cpp + fake_pos_control.cpp gps_checks.cpp gps_control.cpp gps_fusion.cpp diff --git a/src/modules/ekf2/EKF/control.cpp b/src/modules/ekf2/EKF/control.cpp index e1bc47a721..225335046b 100644 --- a/src/modules/ekf2/EKF/control.cpp +++ b/src/modules/ekf2/EKF/control.cpp @@ -1066,64 +1066,6 @@ void Ekf::controlDragFusion() } } -void Ekf::controlFakePosFusion() -{ - // if we aren't doing any aiding, fake position measurements at the last known position to constrain drift - // Coincide fake measurements with baro data for efficiency with a minimum fusion rate of 5Hz - - if (!isHorizontalAidingActive() - && !(_control_status.flags.fuse_aspd && _control_status.flags.fuse_beta)) { - - // We now need to use a synthetic position observation to prevent unconstrained drift of the INS states. - _using_synthetic_position = true; - - // Fuse synthetic position observations every 200msec - if (isTimedOut(_time_last_fake_pos, (uint64_t)2e5)) { - - // Reset position and velocity states if we re-commence this aiding method - if (isTimedOut(_time_last_fake_pos, (uint64_t)4e5)) { - _last_known_posNE = _state.pos.xy(); - resetHorizontalPosition(); - resetVelocity(); - _fuse_hpos_as_odom = false; - - if (_time_last_fake_pos != 0) { - _warning_events.flags.stopping_navigation = true; - ECL_WARN("stopping navigation"); - } - - } - - _time_last_fake_pos = _time_last_imu; - - Vector3f fake_pos_obs_var; - - if (_control_status.flags.in_air && _control_status.flags.tilt_align) { - fake_pos_obs_var(0) = fake_pos_obs_var(1) = sq(fmaxf(_params.pos_noaid_noise, _params.gps_pos_noise)); - - } else if (_control_status.flags.vehicle_at_rest) { - // Accelerate tilt fine alignment by fusing more - // aggressively when the vehicle is at rest - fake_pos_obs_var(0) = fake_pos_obs_var(1) = sq(0.1f); - - } else { - fake_pos_obs_var(0) = fake_pos_obs_var(1) = sq(0.5f); - } - - _gps_pos_innov.xy() = Vector2f(_state.pos) - _last_known_posNE; - - const Vector2f fake_pos_innov_gate(3.0f, 3.0f); - - fuseHorizontalPosition(_gps_pos_innov, fake_pos_innov_gate, fake_pos_obs_var, - _gps_pos_innov_var, _gps_pos_test_ratio, true); - } - - } else { - _using_synthetic_position = false; - } - -} - void Ekf::controlAuxVelFusion() { bool data_ready = false; diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 60ffdffd69..7309dcc910 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -382,8 +382,7 @@ private: uint64_t _time_last_flow_terrain_fuse{0}; ///< time the last fusion of optical flow measurements for terrain estimation were performed (uSec) uint64_t _time_last_arsp_fuse{0}; ///< time the last fusion of airspeed measurements were performed (uSec) uint64_t _time_last_beta_fuse{0}; ///< time the last fusion of synthetic sideslip measurements were performed (uSec) - uint64_t _time_last_fake_pos{0}; ///< last time we faked position measurements to constrain tilt errors during operation without external aiding (uSec) - + uint64_t _time_last_fake_pos_fuse{0}; ///< last time we faked position measurements to constrain tilt errors during operation without external aiding (uSec) uint64_t _time_last_gps_yaw_fuse{0}; ///< time the last fusion of GPS yaw measurements were performed (uSec) uint64_t _time_last_gps_yaw_data{0}; ///< time the last GPS yaw measurement was available (uSec) uint8_t _nb_gps_yaw_reset_available{0}; ///< remaining number of resets allowed before switching to another aiding source @@ -1009,6 +1008,11 @@ private: void stopFlowFusion(); + void startFakePosFusion(); + void resetFakePosFusion(); + void stopFakePosFusion(); + void fuseFakePosition(); + void setVelPosFaultStatus(const int index, const bool status); // reset the quaternion states and covariances to the new yaw value, preserving the roll and pitch diff --git a/src/modules/ekf2/EKF/fake_pos_control.cpp b/src/modules/ekf2/EKF/fake_pos_control.cpp new file mode 100644 index 0000000000..341877a9b4 --- /dev/null +++ b/src/modules/ekf2/EKF/fake_pos_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 fake_pos_control.cpp + * Control functions for ekf fake position fusion + */ + +#include "ekf.h" + +void Ekf::controlFakePosFusion() +{ + // If we aren't doing any aiding, fake position measurements at the last known position to constrain drift + // During intial tilt aligment, fake position is used to perform a "quasi-stationary" leveling of the EKF + const bool fake_pos_data_ready = isTimedOut(_time_last_fake_pos_fuse, (uint64_t)2e5); // Fuse fake position at a limited rate + + if (fake_pos_data_ready) { + const bool continuing_conditions_passing = !isHorizontalAidingActive(); + const bool starting_conditions_passing = continuing_conditions_passing; + + if (_using_synthetic_position) { + if (continuing_conditions_passing) { + fuseFakePosition(); + + const bool is_fusion_failing = isTimedOut(_time_last_fake_pos_fuse, (uint64_t)4e5); + + if (is_fusion_failing) { + resetFakePosFusion(); + } + + } else { + stopFakePosFusion(); + } + + } else { + if (starting_conditions_passing) { + startFakePosFusion(); + + if (_control_status.flags.tilt_align) { + // The fake position fusion is not started for initial alignement + _warning_events.flags.stopping_navigation = true; + ECL_WARN("stopping navigation"); + } + } + } + } +} + +void Ekf::startFakePosFusion() +{ + if (!_using_synthetic_position) { + _using_synthetic_position = true; + _fuse_hpos_as_odom = false; // TODO: needed? + resetFakePosFusion(); + } +} + +void Ekf::resetFakePosFusion() +{ + _last_known_posNE = _state.pos.xy(); + resetHorizontalPosition(); + resetVelocity(); + _time_last_fake_pos_fuse = _time_last_imu; +} + +void Ekf::stopFakePosFusion() +{ + _using_synthetic_position = false; +} + +void Ekf::fuseFakePosition() +{ + Vector3f fake_pos_obs_var; + + if (_control_status.flags.in_air && _control_status.flags.tilt_align) { + fake_pos_obs_var(0) = fake_pos_obs_var(1) = sq(fmaxf(_params.pos_noaid_noise, _params.gps_pos_noise)); + + } else if (_control_status.flags.vehicle_at_rest) { + // Accelerate tilt fine alignment by fusing more + // aggressively when the vehicle is at rest + fake_pos_obs_var(0) = fake_pos_obs_var(1) = sq(0.1f); + + } else { + fake_pos_obs_var(0) = fake_pos_obs_var(1) = sq(0.5f); + } + + _gps_pos_innov.xy() = Vector2f(_state.pos) - _last_known_posNE; + + const Vector2f fake_pos_innov_gate(3.0f, 3.0f); + + if (fuseHorizontalPosition(_gps_pos_innov, fake_pos_innov_gate, fake_pos_obs_var, + _gps_pos_innov_var, _gps_pos_test_ratio, true)) { + _time_last_fake_pos_fuse = _time_last_imu; + } +} diff --git a/src/modules/ekf2/test/test_EKF_basics.cpp b/src/modules/ekf2/test/test_EKF_basics.cpp index 9eeebfd2f1..f42f439f62 100644 --- a/src/modules/ekf2/test/test_EKF_basics.cpp +++ b/src/modules/ekf2/test/test_EKF_basics.cpp @@ -67,7 +67,7 @@ public: SensorSimulator _sensor_simulator; // Duration of initalization with only providing baro,mag and IMU - const uint32_t _init_duration_s{4}; + const uint32_t _init_duration_s{5}; protected: double _latitude {0.0};