From fe7988672f02d5a2ff4eac5b95195c130997039f Mon Sep 17 00:00:00 2001 From: bresch Date: Wed, 29 Nov 2023 12:19:18 +0100 Subject: [PATCH] ekf2: auxiliary position fusion Co-authored-by: Daniel Agar --- boards/px4/sitl/default.px4board | 1 + msg/EstimatorAidSource2d.msg | 2 +- msg/EstimatorStatusFlags.msg | 1 + msg/VehicleGlobalPosition.msg | 1 + src/modules/ekf2/CMakeLists.txt | 4 + src/modules/ekf2/EKF/CMakeLists.txt | 4 + src/modules/ekf2/EKF/aux_global_position.cpp | 143 +++++++++++++++++++ src/modules/ekf2/EKF/aux_global_position.hpp | 120 ++++++++++++++++ src/modules/ekf2/EKF/common.h | 1 + src/modules/ekf2/EKF/control.cpp | 4 + src/modules/ekf2/EKF/ekf.cpp | 7 + src/modules/ekf2/EKF/ekf.h | 15 +- src/modules/ekf2/EKF/ekf_helper.cpp | 9 +- src/modules/ekf2/EKF/estimator_interface.cpp | 1 + src/modules/ekf2/EKF/estimator_interface.h | 3 + src/modules/ekf2/EKF2.cpp | 3 + src/modules/ekf2/Kconfig | 7 + src/modules/ekf2/ekf2_params.c | 48 +++++++ src/modules/logger/logged_topics.cpp | 3 + src/modules/replay/ReplayEkf2.cpp | 6 + src/modules/replay/ReplayEkf2.hpp | 1 + src/modules/uxrce_dds_client/dds_topics.yaml | 3 + 22 files changed, 379 insertions(+), 8 deletions(-) create mode 100644 src/modules/ekf2/EKF/aux_global_position.cpp create mode 100644 src/modules/ekf2/EKF/aux_global_position.hpp diff --git a/boards/px4/sitl/default.px4board b/boards/px4/sitl/default.px4board index 1c84ecfbe1..51f3e092f1 100644 --- a/boards/px4/sitl/default.px4board +++ b/boards/px4/sitl/default.px4board @@ -13,6 +13,7 @@ CONFIG_MODULES_COMMANDER=y CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_MODULES_DATAMAN=y CONFIG_MODULES_EKF2=y +CONFIG_EKF2_AUX_GLOBAL_POSITION=y CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y diff --git a/msg/EstimatorAidSource2d.msg b/msg/EstimatorAidSource2d.msg index 67074de143..98e645a3ec 100644 --- a/msg/EstimatorAidSource2d.msg +++ b/msg/EstimatorAidSource2d.msg @@ -17,6 +17,6 @@ float32[2] test_ratio bool innovation_rejected # true if the observation has been rejected bool fused # true if the sample was successfully fused -# TOPICS estimator_aid_src_ev_pos estimator_aid_src_fake_pos estimator_aid_src_gnss_pos +# TOPICS estimator_aid_src_ev_pos estimator_aid_src_fake_pos estimator_aid_src_gnss_pos estimator_aid_src_aux_global_position # TOPICS estimator_aid_src_aux_vel estimator_aid_src_optical_flow estimator_aid_src_terrain_optical_flow # TOPICS estimator_aid_src_drag diff --git a/msg/EstimatorStatusFlags.msg b/msg/EstimatorStatusFlags.msg index 4d747d8268..b8c5d3e073 100644 --- a/msg/EstimatorStatusFlags.msg +++ b/msg/EstimatorStatusFlags.msg @@ -42,6 +42,7 @@ bool cs_gravity_vector # 34 - true when gravity vector measurements are bool cs_mag # 35 - true if 3-axis magnetometer measurement fusion (mag states only) is intended bool cs_ev_yaw_fault # 36 - true when the EV heading has been declared faulty and is no longer being used bool cs_mag_heading_consistent # 37 - true when the heading obtained from mag data is declared consistent with the filter +bool cs_aux_gpos # 38 - true if auxiliary global position measurement fusion is intended # fault status uint32 fault_status_changes # number of filter fault status (fs) changes diff --git a/msg/VehicleGlobalPosition.msg b/msg/VehicleGlobalPosition.msg index 77244473e2..c7d9ee7812 100644 --- a/msg/VehicleGlobalPosition.msg +++ b/msg/VehicleGlobalPosition.msg @@ -27,3 +27,4 @@ bool dead_reckoning # True if this position is estimated through dead-reckoning # TOPICS vehicle_global_position vehicle_global_position_groundtruth external_ins_global_position # TOPICS estimator_global_position +# TOPICS aux_global_position diff --git a/src/modules/ekf2/CMakeLists.txt b/src/modules/ekf2/CMakeLists.txt index 2854fc3b0d..abd1bab9d6 100644 --- a/src/modules/ekf2/CMakeLists.txt +++ b/src/modules/ekf2/CMakeLists.txt @@ -139,6 +139,10 @@ if(CONFIG_EKF2_AIRSPEED) list(APPEND EKF_SRCS EKF/airspeed_fusion.cpp) endif() +if(CONFIG_EKF2_AUX_GLOBAL_POSITION) + list(APPEND EKF_SRCS EKF/aux_global_position.cpp) +endif() + if(CONFIG_EKF2_AUXVEL) list(APPEND EKF_SRCS EKF/auxvel_fusion.cpp) endif() diff --git a/src/modules/ekf2/EKF/CMakeLists.txt b/src/modules/ekf2/EKF/CMakeLists.txt index cdb0a56ba5..f3c018c5ed 100644 --- a/src/modules/ekf2/EKF/CMakeLists.txt +++ b/src/modules/ekf2/EKF/CMakeLists.txt @@ -56,6 +56,10 @@ if(CONFIG_EKF2_AIRSPEED) list(APPEND EKF_SRCS airspeed_fusion.cpp) endif() +if(CONFIG_EKF2_AUX_GLOBAL_POSITION) + list(APPEND EKF_SRCS aux_global_position.cpp) +endif() + if(CONFIG_EKF2_AUXVEL) list(APPEND EKF_SRCS auxvel_fusion.cpp) endif() diff --git a/src/modules/ekf2/EKF/aux_global_position.cpp b/src/modules/ekf2/EKF/aux_global_position.cpp new file mode 100644 index 0000000000..539643513c --- /dev/null +++ b/src/modules/ekf2/EKF/aux_global_position.cpp @@ -0,0 +1,143 @@ +/**************************************************************************** + * + * Copyright (c) 2023 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 "ekf.h" + +#include "aux_global_position.hpp" + +#if defined(CONFIG_EKF2_AUX_GLOBAL_POSITION) && defined(MODULE_NAME) + +void AuxGlobalPosition::update(Ekf &ekf, const estimator::imuSample &imu_delayed) +{ + +#if defined(MODULE_NAME) + + if (_aux_global_position_sub.updated()) { + + vehicle_global_position_s aux_global_position{}; + _aux_global_position_sub.copy(&aux_global_position); + + const int64_t time_us = aux_global_position.timestamp_sample - static_cast(_param_ekf2_agp_delay.get() * 1000); + + AuxGlobalPositionSample sample{}; + sample.time_us = time_us; + sample.latitude = aux_global_position.lat; + sample.longitude = aux_global_position.lon; + sample.altitude_amsl = aux_global_position.alt; + sample.eph = aux_global_position.eph; + sample.epv = aux_global_position.epv; + + _aux_global_position_buffer.push(sample); + + _time_last_buffer_push = imu_delayed.time_us; + } + +#endif // MODULE_NAME + + AuxGlobalPositionSample sample; + + if (_aux_global_position_buffer.pop_first_older_than(imu_delayed.time_us, &sample)) { + + if (!(_param_ekf2_agp_ctrl.get() & static_cast(Ctrl::HPOS))) { + return; + } + + estimator_aid_source2d_s aid_src{}; + Vector2f position; + + if (ekf.global_origin_valid()) { + position = ekf.global_origin().project(sample.latitude, sample.longitude); + //const float hgt = ekf.getEkfGlobalOriginAltitude() - (float)sample.altitude; + // relax the upper observation noise limit which prevents bad measurements perturbing the position estimate + float pos_noise = math::max(sample.eph, _param_ekf2_agp_noise.get()); + const float pos_var = sq(pos_noise); + const Vector2f pos_obs_var(pos_var, pos_var); + ekf.updateHorizontalPositionAidSrcStatus(sample.time_us, + position, // observation + pos_obs_var, // observation variance + math::max(_param_ekf2_agp_gate.get(), 1.f), // innovation gate + aid_src); + } + + const bool starting_conditions = PX4_ISFINITE(sample.latitude) && PX4_ISFINITE(sample.longitude) + && ekf.control_status_flags().yaw_align; + const bool continuing_conditions = starting_conditions + && ekf.global_origin_valid(); + + switch (_state) { + case State::stopped: + /* FALLTHROUGH */ + case State::starting: + if (starting_conditions) { + _state = State::starting; + + if (ekf.global_origin_valid()) { + ekf.enableControlStatusAuxGpos(); + _state = State::active; + + } else { + // Try to initialize using measurement + if (ekf.setEkfGlobalOrigin(sample.latitude, sample.longitude, sample.altitude_amsl, sample.eph, sample.epv)) { + ekf.enableControlStatusAuxGpos(); + _state = State::active; + } + } + } + break; + + case State::active: + if (continuing_conditions) { + ekf.fuseHorizontalPosition(aid_src); + + } else { + ekf.disableControlStatusAuxGpos(); + _state = State::stopped; + } + break; + + default: + break; + } + +#if defined(MODULE_NAME) + aid_src.timestamp = hrt_absolute_time(); + _estimator_aid_src_aux_global_position_pub.publish(aid_src); +#endif // MODULE_NAME + } else if ((_state != State::stopped) && isTimedOut(_time_last_buffer_push, imu_delayed.time_us, (uint64_t)5e6)) { + ekf.disableControlStatusAuxGpos(); + _state = State::stopped; + ECL_WARN("Aux global position data stopped"); + } +} + +#endif // CONFIG_EKF2_AUX_GLOBAL_POSITION diff --git a/src/modules/ekf2/EKF/aux_global_position.hpp b/src/modules/ekf2/EKF/aux_global_position.hpp new file mode 100644 index 0000000000..f07adf32e0 --- /dev/null +++ b/src/modules/ekf2/EKF/aux_global_position.hpp @@ -0,0 +1,120 @@ +/**************************************************************************** + * + * Copyright (c) 2023 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. + * + ****************************************************************************/ + +#ifndef EKF_AUX_GLOBAL_POSITION_HPP +#define EKF_AUX_GLOBAL_POSITION_HPP + +// interface? +// - ModuleParams +// - Base class EKF +// - bool update(imu) +// how to get delay? +// WelfordMean for init? +// WelfordMean for rate + +#include "common.h" +#include "RingBuffer.h" + +#if defined(CONFIG_EKF2_AUX_GLOBAL_POSITION) && defined(MODULE_NAME) + +#if defined(MODULE_NAME) +# include +# include +# include +# include +# include +#endif // MODULE_NAME + +class Ekf; + +class AuxGlobalPosition : public ModuleParams +{ +public: + AuxGlobalPosition() : ModuleParams(nullptr) {} + ~AuxGlobalPosition() = default; + + void update(Ekf &ekf, const estimator::imuSample &imu_delayed); + + void updateParameters() + { + updateParams(); + } + +private: + bool isTimedOut(uint64_t last_sensor_timestamp, uint64_t time_delayed_us, uint64_t timeout_period) const + { + return (last_sensor_timestamp == 0) || (last_sensor_timestamp + timeout_period < time_delayed_us); + } + + struct AuxGlobalPositionSample { + uint64_t time_us{}; ///< timestamp of the measurement (uSec) + double latitude{}; + double longitude{}; + float altitude_amsl{}; + float eph{}; + float epv{}; + }; + + RingBuffer _aux_global_position_buffer{20}; // TODO: size with _obs_buffer_length and actual publication rate + uint64_t _time_last_buffer_push{0}; + + enum Ctrl : uint8_t { + HPOS = (1<<0), + VPOS = (1<<1) + }; + + enum class State { + stopped, + starting, + active, + }; + + State _state{State::stopped}; + +#if defined(MODULE_NAME) + uORB::PublicationMulti _estimator_aid_src_aux_global_position_pub{ORB_ID(estimator_aid_src_aux_global_position)}; + uORB::Subscription _aux_global_position_sub{ORB_ID(aux_global_position)}; + + DEFINE_PARAMETERS( + (ParamInt) _param_ekf2_agp_ctrl, + (ParamFloat) _param_ekf2_agp_delay, + (ParamFloat) _param_ekf2_agp_noise, + (ParamFloat) _param_ekf2_agp_gate + ) + +#endif // MODULE_NAME +}; + +#endif // CONFIG_EKF2_AUX_GLOBAL_POSITION + +#endif // !EKF_AUX_GLOBAL_POSITION_HPP diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index aba810ceb4..ad052f33d4 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -614,6 +614,7 @@ union filter_control_status_u { uint64_t mag : 1; ///< 35 - true if 3-axis magnetometer measurement fusion (mag states only) is intended uint64_t ev_yaw_fault : 1; ///< 36 - true when the EV heading has been declared faulty and is no longer being used uint64_t mag_heading_consistent : 1; ///< 37 - true when the heading obtained from mag data is declared consistent with the filter + uint64_t aux_gpos : 1; } flags; uint64_t value; diff --git a/src/modules/ekf2/EKF/control.cpp b/src/modules/ekf2/EKF/control.cpp index 037ffe86b2..2ab26658c3 100644 --- a/src/modules/ekf2/EKF/control.cpp +++ b/src/modules/ekf2/EKF/control.cpp @@ -115,6 +115,10 @@ void Ekf::controlFusionModes(const imuSample &imu_delayed) controlGpsFusion(imu_delayed); #endif // CONFIG_EKF2_GNSS +#if defined(CONFIG_EKF2_AUX_GLOBAL_POSITION) && defined(MODULE_NAME) + _aux_global_position.update(*this, imu_delayed); +#endif // CONFIG_EKF2_AUX_GLOBAL_POSITION + #if defined(CONFIG_EKF2_AIRSPEED) controlAirDataFusion(imu_delayed); #endif // CONFIG_EKF2_AIRSPEED diff --git a/src/modules/ekf2/EKF/ekf.cpp b/src/modules/ekf2/EKF/ekf.cpp index 454b29eb70..f3330fed8c 100644 --- a/src/modules/ekf2/EKF/ekf.cpp +++ b/src/modules/ekf2/EKF/ekf.cpp @@ -340,3 +340,10 @@ void Ekf::predictState(const imuSample &imu_delayed) float alpha_height_rate_lpf = 0.1f * imu_delayed.delta_vel_dt; // 10 seconds time constant _height_rate_lpf = _height_rate_lpf * (1.0f - alpha_height_rate_lpf) + _state.vel(2) * alpha_height_rate_lpf; } + +void Ekf::updateParameters() +{ +#if defined(CONFIG_EKF2_AUX_GLOBAL_POSITION) && defined(MODULE_NAME) + _aux_global_position.updateParameters(); +#endif // CONFIG_EKF2_AUX_GLOBAL_POSITION +} diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 7da797f6c1..26244d942e 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -62,6 +62,10 @@ #include "aid_sources/ZeroGyroUpdate.hpp" #include "aid_sources/ZeroVelocityUpdate.hpp" +#if defined(CONFIG_EKF2_AUX_GLOBAL_POSITION) +# include "aux_global_position.hpp" +#endif // CONFIG_EKF2_AUX_GLOBAL_POSITION + enum class Likelihood { LOW, MEDIUM, HIGH }; class Ekf final : public EstimatorInterface @@ -255,7 +259,7 @@ public: // get the ekf WGS-84 origin position and height and the system time it was last set // return true if the origin is valid bool getEkfGlobalOrigin(uint64_t &origin_time, double &latitude, double &longitude, float &origin_alt) const; - bool setEkfGlobalOrigin(const double latitude, const double longitude, const float altitude); + bool setEkfGlobalOrigin(double latitude, double longitude, float altitude, float eph = 0.f, float epv = 0.f); // get the 1-sigma horizontal and vertical position uncertainty of the ekf WGS-84 position void get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv) const; @@ -467,7 +471,6 @@ public: const auto &aid_src_aux_vel() const { return _aid_src_aux_vel; } #endif // CONFIG_EKF2_AUXVEL - bool measurementUpdate(VectorState &K, float innovation_variance, float innovation) { clearInhibitedStateKalmanGains(K); @@ -498,6 +501,10 @@ public: return is_healthy; } + void updateParameters(); + + friend class AuxGlobalPosition; + private: // set the internal states and status to their default value @@ -1228,6 +1235,10 @@ private: ZeroGyroUpdate _zero_gyro_update{}; ZeroVelocityUpdate _zero_velocity_update{}; + +#if defined(CONFIG_EKF2_AUX_GLOBAL_POSITION) && defined(MODULE_NAME) + AuxGlobalPosition _aux_global_position{}; +#endif // CONFIG_EKF2_AUX_GLOBAL_POSITION }; #endif // !EKF_EKF_H diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index e6a56655a4..75544f4e3f 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -302,7 +302,7 @@ bool Ekf::getEkfGlobalOrigin(uint64_t &origin_time, double &latitude, double &lo return _NED_origin_initialised; } -bool Ekf::setEkfGlobalOrigin(const double latitude, const double longitude, const float altitude) +bool Ekf::setEkfGlobalOrigin(const double latitude, const double longitude, const float altitude, const float eph, const float epv) { // sanity check valid latitude/longitude and altitude anywhere between the Mariana Trench and edge of Space if (PX4_ISFINITE(latitude) && (abs(latitude) <= 90) @@ -339,9 +339,8 @@ bool Ekf::setEkfGlobalOrigin(const double latitude, const double longitude, cons } #endif // CONFIG_EKF2_MAGNETOMETER - // We don't know the uncertainty of the origin - _gpos_origin_eph = 0.f; - _gpos_origin_epv = 0.f; + _gpos_origin_eph = eph; + _gpos_origin_epv = epv; _NED_origin_initialised = true; @@ -789,7 +788,7 @@ void Ekf::updateDeadReckoningStatus() void Ekf::updateHorizontalDeadReckoningstatus() { - const bool velPosAiding = (_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.ev_vel) + const bool velPosAiding = (_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.ev_vel || _control_status.flags.aux_gpos) && (isRecent(_time_last_hor_pos_fuse, _params.no_aid_timeout_max) || isRecent(_time_last_hor_vel_fuse, _params.no_aid_timeout_max)); diff --git a/src/modules/ekf2/EKF/estimator_interface.cpp b/src/modules/ekf2/EKF/estimator_interface.cpp index 850e409bdb..19c555202a 100644 --- a/src/modules/ekf2/EKF/estimator_interface.cpp +++ b/src/modules/ekf2/EKF/estimator_interface.cpp @@ -656,6 +656,7 @@ int EstimatorInterface::getNumberOfActiveHorizontalAidingSources() const + int(_control_status.flags.opt_flow) + int(_control_status.flags.ev_pos) + int(_control_status.flags.ev_vel) + + int(_control_status.flags.aux_gpos) // Combined airspeed and sideslip fusion allows sustained wind relative dead reckoning // and so is treated as a single aiding source. + int(_control_status.flags.fuse_aspd && _control_status.flags.fuse_beta); diff --git a/src/modules/ekf2/EKF/estimator_interface.h b/src/modules/ekf2/EKF/estimator_interface.h index 067d8b123c..463702d873 100644 --- a/src/modules/ekf2/EKF/estimator_interface.h +++ b/src/modules/ekf2/EKF/estimator_interface.h @@ -284,6 +284,9 @@ public: const filter_control_status_u &control_status_prev() const { return _control_status_prev; } const decltype(filter_control_status_u::flags) &control_status_prev_flags() const { return _control_status_prev.flags; } + void enableControlStatusAuxGpos() { _control_status.flags.aux_gpos = true; } + void disableControlStatusAuxGpos() { _control_status.flags.aux_gpos = false; } + // get EKF internal fault status const fault_status_u &fault_status() const { return _fault_status; } const decltype(fault_status_u::flags) &fault_status_flags() const { return _fault_status.flags; } diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index bafb94bab5..d752659a69 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -511,6 +511,8 @@ void EKF2::Run() } #endif // CONFIG_EKF2_MAGNETOMETER + + _ekf.updateParameters(); } if (!_callback_registered) { @@ -1896,6 +1898,7 @@ void EKF2::PublishStatusFlags(const hrt_abstime ×tamp) status_flags.cs_mag = _ekf.control_status_flags().mag; status_flags.cs_ev_yaw_fault = _ekf.control_status_flags().ev_yaw_fault; status_flags.cs_mag_heading_consistent = _ekf.control_status_flags().mag_heading_consistent; + status_flags.cs_aux_gpos = _ekf.control_status_flags().aux_gpos; 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/Kconfig b/src/modules/ekf2/Kconfig index 442d30b6ee..e4e94c31d1 100644 --- a/src/modules/ekf2/Kconfig +++ b/src/modules/ekf2/Kconfig @@ -22,6 +22,13 @@ depends on MODULES_EKF2 ---help--- EKF2 airspeed fusion support. +menuconfig EKF2_AUX_GLOBAL_POSITION +depends on MODULES_EKF2 + bool "aux global position fusion support" + default n + ---help--- + EKF2 auxiliary global position fusion support. + menuconfig EKF2_AUXVEL depends on MODULES_EKF2 bool "aux velocity fusion support" diff --git a/src/modules/ekf2/ekf2_params.c b/src/modules/ekf2/ekf2_params.c index 2916b78c2f..cc8adcca56 100644 --- a/src/modules/ekf2/ekf2_params.c +++ b/src/modules/ekf2/ekf2_params.c @@ -1537,3 +1537,51 @@ PARAM_DEFINE_INT32(EKF2_SYNT_MAG_Z, 0); * @decimal 1 */ PARAM_DEFINE_FLOAT(EKF2_GSF_TAS, 15.0f); + +/** + * Aux global position (AGP) sensor aiding + * + * Set bits in the following positions to enable: + * 0 : Horizontal position fusion + * 1 : Vertical position fusion + * + * @group EKF2 + * @min 0 + * @max 3 + * @bit 0 Horizontal position + * @bit 1 Vertical position + */ +PARAM_DEFINE_INT32(EKF2_AGP_CTRL, 1); + +/** + * Aux global position estimator delay relative to IMU measurements + * + * @group EKF2 + * @min 0 + * @max 300 + * @unit ms + * @reboot_required true + * @decimal 1 + */ +PARAM_DEFINE_FLOAT(EKF2_AGP_DELAY, 0); + +/** + * Measurement noise for aux global position observations used to lower bound or replace the uncertainty included in the message + * + * @group EKF2 + * @min 0.01 + * @unit m + * @decimal 2 + */ +PARAM_DEFINE_FLOAT(EKF2_AGP_NOISE, 0.9f); + +/** + * Gate size for aux global position fusion + * + * Sets the number of standard deviations used by the innovation consistency test. + * @group EKF2 + * @min 1.0 + * @unit SD + * @decimal 1 +*/ +PARAM_DEFINE_FLOAT(EKF2_AGP_GATE, 3.0f); diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 679dec756f..9981f359d4 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -203,6 +203,7 @@ void LoggedTopics::add_default_topics() // add_optional_topic_multi("estimator_aid_src_optical_flow", 100, MAX_ESTIMATOR_INSTANCES); // add_optional_topic_multi("estimator_aid_src_terrain_optical_flow", 100, MAX_ESTIMATOR_INSTANCES); // add_optional_topic_multi("estimator_aid_src_ev_yaw", 100, MAX_ESTIMATOR_INSTANCES); + //add_optional_topic_multi("estimator_aid_src_aux_global_position", 100, MAX_ESTIMATOR_INSTANCES); // log all raw sensors at minimal rate (at least 1 Hz) add_topic_multi("battery_status", 200, 2); @@ -220,6 +221,7 @@ void LoggedTopics::add_default_topics() add_topic_multi("vehicle_imu_status", 1000, 4); add_optional_topic_multi("vehicle_magnetometer", 500, 4); add_topic("vehicle_optical_flow", 500); + add_topic("aux_global_position", 500); //add_optional_topic("vehicle_optical_flow_vel", 100); add_optional_topic("pps_capture"); @@ -342,6 +344,7 @@ void LoggedTopics::add_estimator_replay_topics() add_topic("vehicle_magnetometer"); add_topic("vehicle_status"); add_topic("vehicle_visual_odometry"); + add_topic("aux_global_position"); add_topic_multi("distance_sensor"); } diff --git a/src/modules/replay/ReplayEkf2.cpp b/src/modules/replay/ReplayEkf2.cpp index 6b246a4ea4..1add36489d 100644 --- a/src/modules/replay/ReplayEkf2.cpp +++ b/src/modules/replay/ReplayEkf2.cpp @@ -45,6 +45,7 @@ #include #include #include +#include #include #include #include @@ -104,6 +105,9 @@ ReplayEkf2::onSubscriptionAdded(Subscription &sub, uint16_t msg_id) } else if (sub.orb_meta == ORB_ID(vehicle_visual_odometry)) { _vehicle_visual_odometry_msg_id = msg_id; + + } else if (sub.orb_meta == ORB_ID(aux_global_position)) { + _aux_global_position_msg_id = msg_id; } // the main loop should only handle publication of the following topics, the sensor topics are @@ -130,6 +134,7 @@ ReplayEkf2::publishEkf2Topics(const ekf2_timestamps_s &ekf2_timestamps, std::ifs handle_sensor_publication(ekf2_timestamps.vehicle_air_data_timestamp_rel, _vehicle_air_data_msg_id); handle_sensor_publication(ekf2_timestamps.vehicle_magnetometer_timestamp_rel, _vehicle_magnetometer_msg_id); handle_sensor_publication(ekf2_timestamps.visual_odometry_timestamp_rel, _vehicle_visual_odometry_msg_id); + handle_sensor_publication(0, _aux_global_position_msg_id); // sensor_combined: publish last because ekf2 is polling on this if (!findTimestampAndPublish(ekf2_timestamps.timestamp / 100, _sensor_combined_msg_id, replay_file)) { @@ -214,6 +219,7 @@ ReplayEkf2::onExitMainLoop() print_sensor_statistics(_vehicle_air_data_msg_id, "vehicle_air_data"); print_sensor_statistics(_vehicle_magnetometer_msg_id, "vehicle_magnetometer"); print_sensor_statistics(_vehicle_visual_odometry_msg_id, "vehicle_visual_odometry"); + print_sensor_statistics(_aux_global_position_msg_id, "aux_global_position"); } } // namespace px4 diff --git a/src/modules/replay/ReplayEkf2.hpp b/src/modules/replay/ReplayEkf2.hpp index 43c7de1913..bb7f8d1359 100644 --- a/src/modules/replay/ReplayEkf2.hpp +++ b/src/modules/replay/ReplayEkf2.hpp @@ -88,6 +88,7 @@ private: uint16_t _vehicle_air_data_msg_id = msg_id_invalid; uint16_t _vehicle_magnetometer_msg_id = msg_id_invalid; uint16_t _vehicle_visual_odometry_msg_id = msg_id_invalid; + uint16_t _aux_global_position_msg_id = msg_id_invalid; }; } //namespace px4 diff --git a/src/modules/uxrce_dds_client/dds_topics.yaml b/src/modules/uxrce_dds_client/dds_topics.yaml index e630abc3ef..e467c61929 100644 --- a/src/modules/uxrce_dds_client/dds_topics.yaml +++ b/src/modules/uxrce_dds_client/dds_topics.yaml @@ -151,5 +151,8 @@ subscriptions: - topic: /fmu/in/actuator_servos type: px4_msgs::msg::ActuatorServos + - topic: /fmu/in/aux_global_position + type: px4_msgs::msg::VehicleGlobalPosition + # Create uORB::PublicationMulti subscriptions_multi: