diff --git a/msg/EstimatorStatusFlags.msg b/msg/EstimatorStatusFlags.msg index 23e6e15fd4..79b900f6a8 100644 --- a/msg/EstimatorStatusFlags.msg +++ b/msg/EstimatorStatusFlags.msg @@ -45,6 +45,8 @@ bool cs_mag_heading_consistent # 37 - true when the heading obtained from mag d bool cs_aux_gpos # 38 - true if auxiliary global position measurement fusion is intended bool cs_rng_terrain # 39 - true if we are fusing range finder data for terrain bool cs_opt_flow_terrain # 40 - true if we are fusing flow data for terrain +bool cs_valid_fake_pos # 41 - true if a valid constant position is being fused +bool cs_constant_pos # 42 - true if the vehicle is at a constant position # fault status uint32 fault_status_changes # number of filter fault status (fs) changes diff --git a/src/modules/ekf2/EKF/aid_sources/fake_pos_control.cpp b/src/modules/ekf2/EKF/aid_sources/fake_pos_control.cpp index df7257d9b0..b87e14654f 100644 --- a/src/modules/ekf2/EKF/aid_sources/fake_pos_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/fake_pos_control.cpp @@ -75,55 +75,20 @@ void Ekf::controlFakePosFusion() Vector2f(getStateVariance()) + obs_var, // innovation variance innov_gate); // innovation gate - const bool enable_conditions_passing = !isHorizontalAidingActive() - && ((getTiltVariance() > sq(math::radians(3.f))) || _control_status.flags.vehicle_at_rest) - && (!(_params.imu_ctrl & static_cast(ImuCtrl::GravityVector)) || _control_status.flags.vehicle_at_rest) - && _horizontal_deadreckon_time_exceeded; + const bool enable_valid_fake_pos = _control_status.flags.constant_pos || _control_status.flags.vehicle_at_rest; + const bool enable_fake_pos = !enable_valid_fake_pos + && (getTiltVariance() > sq(math::radians(3.f))) + && !(_params.imu_ctrl & static_cast(ImuCtrl::GravityVector)) + && _horizontal_deadreckon_time_exceeded; - if (_control_status.flags.fake_pos) { - if (enable_conditions_passing) { + _control_status.flags.fake_pos = runFakePosStateMachine(enable_fake_pos, _control_status.flags.fake_pos, aid_src); + _control_status.flags.valid_fake_pos = runFakePosStateMachine(enable_valid_fake_pos, + _control_status.flags.valid_fake_pos, aid_src); - // always protect against extreme values that could result in a NaN - if ((aid_src.test_ratio[0] < sq(100.0f / innov_gate)) - && (aid_src.test_ratio[1] < sq(100.0f / innov_gate)) - ) { - if (!aid_src.innovation_rejected - && fuseDirectStateMeasurement(aid_src.innovation[0], aid_src.innovation_variance[0], aid_src.observation_variance[0], - State::pos.idx + 0) - && fuseDirectStateMeasurement(aid_src.innovation[1], aid_src.innovation_variance[1], aid_src.observation_variance[1], - State::pos.idx + 1) - ) { - aid_src.fused = true; - aid_src.time_last_fuse = _time_delayed_us; - } - } - - const bool is_fusion_failing = isTimedOut(aid_src.time_last_fuse, (uint64_t)4e5); - - if (is_fusion_failing) { - ECL_WARN("fake position fusion failing, resetting"); - resetFakePosFusion(); - } - - } else { - stopFakePosFusion(); - } - - } else { - if (enable_conditions_passing) { - ECL_INFO("start fake position fusion"); - _control_status.flags.fake_pos = true; - resetFakePosFusion(); - - if (_control_status.flags.tilt_align) { - // The fake position fusion is not started for initial alignement - ECL_WARN("stopping navigation"); - } - } - } - - } else if (_control_status.flags.fake_pos && isHorizontalAidingActive()) { - stopFakePosFusion(); + } else if ((_control_status.flags.fake_pos || _control_status.flags.valid_fake_pos) && isHorizontalAidingActive()) { + ECL_INFO("stop fake position fusion"); + _control_status.flags.fake_pos = false; + _control_status.flags.valid_fake_pos = false; } } @@ -138,10 +103,41 @@ void Ekf::resetFakePosFusion() _aid_src_fake_pos.time_last_fuse = _time_delayed_us; } -void Ekf::stopFakePosFusion() +bool Ekf::runFakePosStateMachine(const bool enable_conditions_passing, bool status_flag, + estimator_aid_source2d_s &aid_src) { - if (_control_status.flags.fake_pos) { - ECL_INFO("stop fake position fusion"); - _control_status.flags.fake_pos = false; + if (status_flag) { + if (enable_conditions_passing) { + if (!aid_src.innovation_rejected + && fuseDirectStateMeasurement(aid_src.innovation[0], aid_src.innovation_variance[0], aid_src.observation_variance[0], + State::pos.idx + 0) + && fuseDirectStateMeasurement(aid_src.innovation[1], aid_src.innovation_variance[1], aid_src.observation_variance[1], + State::pos.idx + 1) + ) { + aid_src.fused = true; + aid_src.time_last_fuse = _time_delayed_us; + } + + const bool is_fusion_failing = isTimedOut(aid_src.time_last_fuse, (uint64_t)4e5); + + if (is_fusion_failing) { + ECL_WARN("fake position fusion failing, resetting"); + resetFakePosFusion(); + } + + } else { + ECL_INFO("stop fake position fusion"); + status_flag = false; + } + + } else { + if (enable_conditions_passing) { + ECL_INFO("start fake position fusion"); + status_flag = true; + + resetFakePosFusion(); + } } + + return status_flag; } diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index af03d2a8db..13c2767f75 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -261,6 +261,7 @@ struct systemFlagUpdate { bool in_air{true}; bool is_fixed_wing{false}; bool gnd_effect{false}; + bool constant_pos{false}; }; struct parameters { @@ -614,6 +615,8 @@ uint64_t mag_heading_consistent : uint64_t aux_gpos : 1; ///< 38 - true if auxiliary global position measurement fusion is intended uint64_t rng_terrain : 1; ///< 39 - true if we are fusing range finder data for terrain uint64_t opt_flow_terrain : 1; ///< 40 - true if we are fusing flow data for terrain + uint64_t valid_fake_pos : 1; ///< 41 - true if a valid constant position is being fused + uint64_t constant_pos : 1; ///< 42 - true if the vehicle is at a constant position } flags; uint64_t value; diff --git a/src/modules/ekf2/EKF/control.cpp b/src/modules/ekf2/EKF/control.cpp index b8aca027e7..ceefc01f6f 100644 --- a/src/modules/ekf2/EKF/control.cpp +++ b/src/modules/ekf2/EKF/control.cpp @@ -62,6 +62,8 @@ void Ekf::controlFusionModes(const imuSample &imu_delayed) if (system_flags_delayed.gnd_effect) { set_gnd_effect(); } + + set_constant_pos(system_flags_delayed.constant_pos); } } diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index d9ff978e20..e6c182171a 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -1132,7 +1132,7 @@ private: } void resetFakePosFusion(); - void stopFakePosFusion(); + bool runFakePosStateMachine(bool enable_condition_passing, bool status_flag, estimator_aid_source2d_s &aid_src); // reset the quaternion states and covariances to the new yaw value, preserving the roll and pitch // yaw : Euler yaw angle (rad) diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index ae3e9e355e..e88d3b483a 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -642,7 +642,8 @@ uint16_t Ekf::get_ekf_soln_status() const #endif // CONFIG_EKF2_TERRAIN // 128 ESTIMATOR_CONST_POS_MODE True if the EKF is in a constant position mode and is not using external measurements (eg GPS or optical flow) - soln_status.flags.const_pos_mode = _control_status.flags.fake_pos || _control_status.flags.vehicle_at_rest; + soln_status.flags.const_pos_mode = _control_status.flags.fake_pos || _control_status.flags.valid_fake_pos + || _control_status.flags.vehicle_at_rest; // 256 ESTIMATOR_PRED_POS_HORIZ_REL True if the EKF has sufficient data to enter a mode that will provide a (relative) position estimate soln_status.flags.pred_pos_horiz_rel = isHorizontalAidingActive(); @@ -793,6 +794,13 @@ void Ekf::updateHorizontalDeadReckoningstatus() } } + if (_control_status.flags.valid_fake_pos && isRecent(_aid_src_fake_pos.time_last_fuse, _params.no_aid_timeout_max)) { + // only respect as a valid aiding source now if we expect to have another valid source once in air + if (aiding_expected_in_air) { + inertial_dead_reckoning = false; + } + } + if (inertial_dead_reckoning) { if (isTimedOut(_time_last_horizontal_aiding, (uint64_t)_params.valid_timeout_max)) { // deadreckon time exceeded diff --git a/src/modules/ekf2/EKF/estimator_interface.h b/src/modules/ekf2/EKF/estimator_interface.h index 75e6526830..57e6fc06b0 100644 --- a/src/modules/ekf2/EKF/estimator_interface.h +++ b/src/modules/ekf2/EKF/estimator_interface.h @@ -179,6 +179,8 @@ public: _control_status.flags.vehicle_at_rest = at_rest; } + void set_constant_pos(bool constant_pos) { _control_status.flags.constant_pos = constant_pos; } + // return true if the attitude is usable bool attitude_valid() const { return _control_status.flags.tilt_align; } diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index af2eb6615f..bb986208de 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -1925,6 +1925,8 @@ void EKF2::PublishStatusFlags(const hrt_abstime ×tamp) status_flags.cs_aux_gpos = _ekf.control_status_flags().aux_gpos; status_flags.cs_rng_terrain = _ekf.control_status_flags().rng_terrain; status_flags.cs_opt_flow_terrain = _ekf.control_status_flags().opt_flow_terrain; + status_flags.cs_valid_fake_pos = _ekf.control_status_flags().valid_fake_pos; + status_flags.cs_constant_pos = _ekf.control_status_flags().constant_pos; status_flags.fault_status_changes = _filter_fault_status_changes; status_flags.fs_bad_mag_x = _ekf.fault_status_flags().bad_mag_x; @@ -2592,6 +2594,15 @@ void EKF2::UpdateSystemFlagsSample(ekf2_timestamps_s &ekf2_timestamps) flags.gnd_effect = vehicle_land_detected.in_ground_effect; } + launch_detection_status_s launch_detection_status; + + if (_launch_detection_status_sub.copy(&launch_detection_status) + && (ekf2_timestamps.timestamp < launch_detection_status.timestamp + 3_s)) { + + flags.constant_pos = (launch_detection_status.launch_detection_state == + launch_detection_status_s::STATE_WAITING_FOR_LAUNCH); + } + _ekf.setSystemFlagData(flags); } } diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index ef7280e88c..867cf9ae57 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -72,6 +72,7 @@ #include #include #include +#include #include #include #include @@ -388,6 +389,7 @@ private: uORB::Subscription _sensor_selection_sub{ORB_ID(sensor_selection)}; uORB::Subscription _status_sub{ORB_ID(vehicle_status)}; uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; + uORB::Subscription _launch_detection_status_sub{ORB_ID(launch_detection_status)}; uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)}; uORB::Publication _vehicle_command_ack_pub{ORB_ID(vehicle_command_ack)}; diff --git a/src/modules/ekf2/test/CMakeLists.txt b/src/modules/ekf2/test/CMakeLists.txt index 800b568d96..c1ecb992b9 100644 --- a/src/modules/ekf2/test/CMakeLists.txt +++ b/src/modules/ekf2/test/CMakeLists.txt @@ -40,6 +40,7 @@ px4_add_unit_gtest(SRC test_EKF_accelerometer.cpp LINKLIBS ecl_EKF ecl_sensor_si px4_add_unit_gtest(SRC test_EKF_airspeed.cpp LINKLIBS ecl_EKF ecl_sensor_sim) px4_add_unit_gtest(SRC test_EKF_basics.cpp LINKLIBS ecl_EKF ecl_sensor_sim) px4_add_unit_gtest(SRC test_EKF_externalVision.cpp LINKLIBS ecl_EKF ecl_sensor_sim ecl_test_helper) +px4_add_unit_gtest(SRC test_EKF_fake_pos.cpp LINKLIBS ecl_EKF ecl_sensor_sim ecl_test_helper) px4_add_unit_gtest(SRC test_EKF_flow.cpp LINKLIBS ecl_EKF ecl_sensor_sim ecl_test_helper) px4_add_unit_gtest(SRC test_EKF_flow_generated.cpp LINKLIBS ecl_EKF ecl_sensor_sim ecl_test_helper) px4_add_unit_gtest(SRC test_EKF_gyroscope.cpp LINKLIBS ecl_EKF ecl_sensor_sim) diff --git a/src/modules/ekf2/test/sensor_simulator/airspeed.cpp b/src/modules/ekf2/test/sensor_simulator/airspeed.cpp index 520aca024e..67d89224d2 100644 --- a/src/modules/ekf2/test/sensor_simulator/airspeed.cpp +++ b/src/modules/ekf2/test/sensor_simulator/airspeed.cpp @@ -15,13 +15,18 @@ Airspeed::~Airspeed() void Airspeed::send(uint64_t time) { - if (_true_airspeed_data > FLT_EPSILON && _indicated_airspeed_data > FLT_EPSILON) { - airspeedSample airspeed_sample; - airspeed_sample.time_us = time; - airspeed_sample.eas2tas = _true_airspeed_data / _indicated_airspeed_data; - airspeed_sample.true_airspeed = _true_airspeed_data; - _ekf->setAirspeedData(airspeed_sample); + float ias2tas = 1.f; + + if (PX4_ISFINITE(_indicated_airspeed_data) + && (_indicated_airspeed_data > FLT_EPSILON)) { + ias2tas = _true_airspeed_data / _indicated_airspeed_data; } + + airspeedSample airspeed_sample; + airspeed_sample.time_us = time; + airspeed_sample.eas2tas = ias2tas; + airspeed_sample.true_airspeed = _true_airspeed_data; + _ekf->setAirspeedData(airspeed_sample); } void Airspeed::setData(float true_airspeed, float indicated_airspeed) diff --git a/src/modules/ekf2/test/test_EKF_airspeed.cpp b/src/modules/ekf2/test/test_EKF_airspeed.cpp index d284b959e3..61865dfc7a 100644 --- a/src/modules/ekf2/test/test_EKF_airspeed.cpp +++ b/src/modules/ekf2/test/test_EKF_airspeed.cpp @@ -78,7 +78,6 @@ public: TEST_F(EkfAirspeedTest, testWindVelocityEstimation) { - const Vector3f simulated_velocity_earth(0.0f, 1.5f, 0.0f); const Vector2f airspeed_body(2.4f, 0.0f); _ekf_wrapper.enableExternalVisionVelocityFusion(); @@ -86,6 +85,9 @@ TEST_F(EkfAirspeedTest, testWindVelocityEstimation) _sensor_simulator._vio.setVelocityFrameToLocalNED(); _sensor_simulator.startExternalVision(); + // Let the EV fusion start first to reset the velocity estimate + _sensor_simulator.runSeconds(0.5); + _ekf->set_in_air_status(true); _ekf->set_vehicle_at_rest(false); _ekf->set_is_fixed_wing(true); diff --git a/src/modules/ekf2/test/test_EKF_fake_pos.cpp b/src/modules/ekf2/test/test_EKF_fake_pos.cpp new file mode 100644 index 0000000000..3cd755a3a5 --- /dev/null +++ b/src/modules/ekf2/test/test_EKF_fake_pos.cpp @@ -0,0 +1,121 @@ +/**************************************************************************** + * + * Copyright (c) 2024 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. + * + ****************************************************************************/ + +#include +#include "EKF/ekf.h" +#include "sensor_simulator/sensor_simulator.h" +#include "sensor_simulator/ekf_wrapper.h" + + +class EkfFakePosTest : public ::testing::Test +{ +public: + + EkfFakePosTest(): ::testing::Test(), + _ekf{std::make_shared()}, + _sensor_simulator(_ekf), + _ekf_wrapper(_ekf), + _quat_sim(Eulerf(0.0f, 0.0f, math::radians(45.0f))) {}; + + std::shared_ptr _ekf; + SensorSimulator _sensor_simulator; + EkfWrapper _ekf_wrapper; + const Quatf _quat_sim; + + // Setup the Ekf with synthetic measurements + void SetUp() override + { + // run briefly to init, then manually set in air and at rest (default for a real vehicle) + _ekf->init(0); + _sensor_simulator.runSeconds(0.1); + _ekf->set_in_air_status(false); + _ekf->set_vehicle_at_rest(true); + + _sensor_simulator.simulateOrientation(_quat_sim); + _sensor_simulator.runSeconds(7); + } + + // Use this method to clean up any memory, network etc. after each test + void TearDown() override + { + } +}; + +TEST_F(EkfFakePosTest, testValidFakePos) +{ + _ekf->set_vehicle_at_rest(false); + _ekf->set_constant_pos(true); + _sensor_simulator.runSeconds(1); + + EXPECT_EQ(1, (int) _ekf->control_status_flags().constant_pos); + EXPECT_EQ(0, (int) _ekf->control_status_flags().fake_pos); + EXPECT_EQ(1, (int) _ekf->control_status_flags().valid_fake_pos); +} + +TEST_F(EkfFakePosTest, testFakePosStopGnss) +{ + _ekf->set_vehicle_at_rest(false); + _ekf->set_constant_pos(true); + _sensor_simulator.startGps(); + _sensor_simulator.runSeconds(12); + + EXPECT_TRUE(_ekf_wrapper.isIntendingGpsFusion()); + EXPECT_EQ(1, (int) _ekf->control_status_flags().constant_pos); + EXPECT_EQ(0, (int) _ekf->control_status_flags().fake_pos); + EXPECT_EQ(0, (int) _ekf->control_status_flags().valid_fake_pos); +} + +TEST_F(EkfFakePosTest, testValidFakePosValidLocalPos) +{ + _ekf->set_is_fixed_wing(true); + _sensor_simulator.startAirspeedSensor(); + _sensor_simulator._airspeed.setData(-0.01f, 0.f); // airspeed close to 0 + _ekf_wrapper.enableBetaFusion(); + + // WHEN: the vehicle is not as rest but is known to be at a constant position + _ekf->set_vehicle_at_rest(false); + _ekf->set_constant_pos(true); + _sensor_simulator.runSeconds(1); + + // THEN: the valid fake position is fused + EXPECT_EQ(1, (int) _ekf->control_status_flags().constant_pos); + EXPECT_EQ(0, (int) _ekf->control_status_flags().fake_pos); + EXPECT_EQ(1, (int) _ekf->control_status_flags().valid_fake_pos); + + // AND: since airspeed is expected to provide wind-relative dead-reckoning after takeoff + // the local position is considered valid + _sensor_simulator.runSeconds(60); + + EXPECT_EQ(1, (int) _ekf->control_status_flags().valid_fake_pos); + EXPECT_TRUE(_ekf->local_position_is_valid()); +}