fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: implement multi-instance AGP fusion

Browse Source 
Refactor auxiliary global position fusion to support multiple AGP
sources. Add AgpSourceControl manager class that routes AGP messages
to the correct AgpSource instance based on configured source IDs.
Only instantiate AgpSource slots that have a configured ID on boot.
Move AGP subscriptions to manager class for correct message routing.
This commit is contained in:
Marco Hauswirth 2026-02-12 16:20:13 +01:00 committed by Marco Hauswirth
parent 1d852907a2
commit b346fcfa00
9 changed files with 536 additions and 221 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
src/modules/ekf2/CMakeLists.txt
View File

@ -141,7 +141,10 @@ if(CONFIG_EKF2_AIRSPEED)
endif()
if(CONFIG_EKF2_AUX_GLOBAL_POSITION)
list(APPEND EKF_SRCS EKF/aid_sources/aux_global_position/aux_global_position.cpp)
list(APPEND EKF_SRCS
EKF/aid_sources/aux_global_position/aux_global_position.cpp
EKF/aid_sources/aux_global_position/aux_global_position_control.cpp
)
list(APPEND EKF_MODULE_PARAMS params_aux_global_position.yaml)
endif()

4
src/modules/ekf2/EKF/CMakeLists.txt
View File

@ -60,10 +60,6 @@ if(CONFIG_EKF2_AIRSPEED)
list(APPEND EKF_SRCS aid_sources/airspeed/airspeed_fusion.cpp)
endif()
if(CONFIG_EKF2_AUX_GLOBAL_POSITION)
list(APPEND EKF_SRCS aid_sources/aux_global_position/aux_global_position.cpp)
endif()
if(CONFIG_EKF2_AUXVEL)
list(APPEND EKF_SRCS aid_sources/auxvel/auxvel_fusion.cpp)
endif()

267
src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.cpp
View File

@ -32,158 +32,155 @@
****************************************************************************/
#include "ekf.h"
#include "aid_sources/aux_global_position/aux_global_position.hpp"
#include <aid_sources/aux_global_position/aux_global_position.hpp>
#include <aid_sources/aux_global_position/aux_global_position_control.hpp>
#if defined(CONFIG_EKF2_AUX_GLOBAL_POSITION) && defined(MODULE_NAME)
AuxGlobalPosition::AuxGlobalPosition() : ModuleParams(nullptr)
{
for (int slot = 0; slot < MAX_AGP_IDS; slot++) {
_id_param_values[slot] = getAgpParamInt32("ID", slot);
if (_id_param_values[slot] != 0) {
_sources[slot] = new AgpSource(slot, this);
_n_sources++;
}
}
// Only subscribe to uORB instances if there are configured sources
if (_n_sources > 0) {
for (int i = 0; i < MAX_AGP_IDS; i++) {
_agp_sub[i] = uORB::Subscription(ORB_ID(aux_global_position), i);
}
}
}
AuxGlobalPosition::~AuxGlobalPosition()
{
for (int i = 0; i < MAX_AGP_IDS; i++) {
delete _sources[i];
}
}
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<int64_t>(_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;
sample.lat_lon_reset_counter = aux_global_position.lat_lon_reset_counter;
_aux_global_position_buffer.push(sample);
_time_last_buffer_push = imu_delayed.time_us;
// If there are no sources configured, we also don't subscribe to any uORB topic
// and skip the update completely.
if (_n_sources == 0) {
return;
}
#endif // MODULE_NAME
for (int instance = 0; instance < MAX_AGP_IDS; instance++) {
if (_agp_sub[instance].updated()) {
aux_global_position_s msg{};
_agp_sub[instance].copy(&msg);
AuxGlobalPositionSample sample;
int slot = _instance_slot_map[instance];
if (_aux_global_position_buffer.pop_first_older_than(imu_delayed.time_us, &sample)) {
if (!(_param_ekf2_agp_ctrl.get() & static_cast<int32_t>(Ctrl::kHPos))) {
return;
}
estimator_aid_source2d_s &aid_src = _aid_src_aux_global_position;
const LatLonAlt position(sample.latitude, sample.longitude, sample.altitude_amsl);
const Vector2f innovation = (ekf.getLatLonAlt() - position).xy(); // altitude measurements are not used
// 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(), 0.01f);
const float pos_var = sq(pos_noise);
const Vector2f pos_obs_var(pos_var, pos_var);
ekf.updateAidSourceStatus(aid_src,
sample.time_us, // sample timestamp
matrix::Vector2d(sample.latitude, sample.longitude), // observation
pos_obs_var, // observation variance
innovation, // innovation
Vector2f(ekf.getPositionVariance()) + pos_obs_var, // innovation variance
math::max(_param_ekf2_agp_gate.get(), 1.f)); // innovation gate
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::kStopped:
/* FALLTHROUGH */
case State::kStarting:
if (starting_conditions) {
_state = State::kStarting;
if (ekf.global_origin_valid()) {
const bool fused = ekf.fuseHorizontalPosition(aid_src);
bool reset = false;
if (!fused && isResetAllowed(ekf)) {
ekf.resetHorizontalPositionTo(sample.latitude, sample.longitude, Vector2f(aid_src.observation_variance));
ekf.resetAidSourceStatusZeroInnovation(aid_src);
reset = true;
}
if (fused || reset) {
ekf.enableControlStatusAuxGpos();
_reset_counters.lat_lon = sample.lat_lon_reset_counter;
_state = State::kActive;
}
} else {
// Try to initialize using measurement
if (ekf.resetGlobalPositionTo(sample.latitude, sample.longitude, sample.altitude_amsl, pos_var,
sq(sample.epv))) {
ekf.resetAidSourceStatusZeroInnovation(aid_src);
ekf.enableControlStatusAuxGpos();
_reset_counters.lat_lon = sample.lat_lon_reset_counter;
_state = State::kActive;
}
}
if (slot < 0) {
slot = mapSensorIdToSlot(msg.id);
_instance_slot_map[instance] = static_cast<int8_t>(slot);
}
break;
case State::kActive:
if (continuing_conditions) {
ekf.fuseHorizontalPosition(aid_src);
if (isTimedOut(aid_src.time_last_fuse, imu_delayed.time_us, ekf._params.reset_timeout_max)
|| (_reset_counters.lat_lon != sample.lat_lon_reset_counter)) {
if (isResetAllowed(ekf)) {
ekf.resetHorizontalPositionTo(sample.latitude, sample.longitude, Vector2f(aid_src.observation_variance));
ekf.resetAidSourceStatusZeroInnovation(aid_src);
_reset_counters.lat_lon = sample.lat_lon_reset_counter;
} else {
ekf.disableControlStatusAuxGpos();
_state = State::kStopped;
}
}
} else {
ekf.disableControlStatusAuxGpos();
_state = State::kStopped;
if (slot >= 0 && _sources[slot]) {
_sources[slot]->bufferData(msg, imu_delayed);
}
break;
default:
break;
}
}
#if defined(MODULE_NAME)
aid_src.timestamp = hrt_absolute_time();
_estimator_aid_src_aux_global_position_pub.publish(aid_src);
_test_ratio_filtered = math::max(fabsf(aid_src.test_ratio_filtered[0]), fabsf(aid_src.test_ratio_filtered[1]));
#endif // MODULE_NAME
} else if ((_state != State::kStopped) && isTimedOut(_time_last_buffer_push, imu_delayed.time_us, (uint64_t)5e6)) {
ekf.disableControlStatusAuxGpos();
_state = State::kStopped;
ECL_WARN("Aux global position data stopped");
for (int slot = 0; slot < MAX_AGP_IDS; slot++) {
if (_sources[slot]) {
if (_sources[slot]->update(ekf, imu_delayed)) {
// Only update one source per update cycle
break;
}
}
}
}
bool AuxGlobalPosition::isResetAllowed(const Ekf &ekf) const
void AuxGlobalPosition::paramsUpdated()
{
return ((static_cast<Mode>(_param_ekf2_agp_mode.get()) == Mode::kAuto)
&& !ekf.isOtherSourceOfHorizontalPositionAidingThan(ekf.control_status_flags().aux_gpos))
|| ((static_cast<Mode>(_param_ekf2_agp_mode.get()) == Mode::kDeadReckoning)
&& !ekf.isOtherSourceOfHorizontalAidingThan(ekf.control_status_flags().aux_gpos));
for (int i = 0; i < MAX_AGP_IDS; i++) {
if (_sources[i]) {
_sources[i]->updateParams();
}
}
}
#endif // CONFIG_EKF2_AUX_GLOBAL_POSITION
float AuxGlobalPosition::testRatioFiltered() const
{
float max_ratio = 0.f;
for (int i = 0; i < MAX_AGP_IDS; i++) {
if (_sources[i] && _sources[i]->isFusing()) {
max_ratio = math::max(max_ratio, _sources[i]->getTestRatioFiltered());
}
}
return max_ratio;
}
bool AuxGlobalPosition::anySourceFusing() const
{
for (int i = 0; i < MAX_AGP_IDS; i++) {
if (_sources[i] && _sources[i]->isFusing()) {
return true;
}
}
return false;
}
int32_t AuxGlobalPosition::getAgpParamInt32(const char *param_suffix, int instance) const
{
char param_name[20] {};
snprintf(param_name, sizeof(param_name), "EKF2_AGP%d_%s", instance, param_suffix);
int32_t value = 0;
if (param_get(param_find(param_name), &value) != 0) {
PX4_ERR("failed to get %s", param_name);
}
return value;
}
bool AuxGlobalPosition::setAgpParamInt32(const char *param_suffix, int instance, int32_t value)
{
char param_name[20] {};
snprintf(param_name, sizeof(param_name), "EKF2_AGP%d_%s", instance, param_suffix);
return param_set_no_notification(param_find(param_name), &value) == PX4_OK;
}
int32_t AuxGlobalPosition::getIdParam(int instance)
{
return _id_param_values[instance];
}
void AuxGlobalPosition::setIdParam(int instance, int32_t sensor_id)
{
setAgpParamInt32("ID", instance, sensor_id);
_id_param_values[instance] = sensor_id;
}
int AuxGlobalPosition::mapSensorIdToSlot(int32_t sensor_id)
{
for (int slot = 0; slot < MAX_AGP_IDS; slot++) {
if (getIdParam(slot) == sensor_id) {
return slot;
}
}
for (int slot = 0; slot < MAX_AGP_IDS; slot++) {
if (getIdParam(slot) == 0) {
setIdParam(slot, sensor_id);
return slot;
}
}
return -1;
}
#endif // CONFIG_EKF2_AUX_GLOBAL_POSITION && MODULE_NAME

105
src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.hpp
View File

@ -34,38 +34,26 @@
#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 <lib/ringbuffer/TimestampedRingBuffer.hpp>
#if defined(CONFIG_EKF2_AUX_GLOBAL_POSITION) && defined(MODULE_NAME)
#if defined(MODULE_NAME)
# include <px4_platform_common/module_params.h>
# include <uORB/PublicationMulti.hpp>
# include <uORB/Subscription.hpp>
# include <uORB/topics/estimator_aid_source2d.h>
# include <uORB/topics/vehicle_global_position.h>
#endif // MODULE_NAME
#include <px4_platform_common/module_params.h>
#include <px4_platform_common/log.h>
#include <lib/parameters/param.h>
#include <uORB/Subscription.hpp>
#include <uORB/topics/aux_global_position.h>
#include <aid_sources/aux_global_position/aux_global_position_control.hpp>
class Ekf;
class AuxGlobalPosition : public ModuleParams
{
public:
AuxGlobalPosition() : ModuleParams(nullptr)
{
_estimator_aid_src_aux_global_position_pub.advertise();
}
static constexpr uint8_t MAX_AGP_IDS = 4;
~AuxGlobalPosition() = default;
AuxGlobalPosition();
~AuxGlobalPosition();
void update(Ekf &ekf, const estimator::imuSample &imu_delayed);
@ -74,70 +62,29 @@ public:
updateParams();
}
float test_ratio_filtered() const { return _test_ratio_filtered; }
/**
* Returns the maximum filtered test ratio across all active AGP sources.
*/
float testRatioFiltered() const;
bool anySourceFusing() const;
int32_t getIdParam(int instance);
void setIdParam(int instance, int32_t sensor_id);
int mapSensorIdToSlot(int32_t sensor_id);
void paramsUpdated();
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);
}
AgpSource *_sources[MAX_AGP_IDS] {};
uORB::Subscription _agp_sub[MAX_AGP_IDS];
int8_t _instance_slot_map[MAX_AGP_IDS] {-1, -1, -1, -1};
uint8_t _n_sources{0};
bool isResetAllowed(const Ekf &ekf) const;
int32_t getAgpParamInt32(const char *param_suffix, int instance) const;
bool setAgpParamInt32(const char *param_suffix, int instance, int32_t value);
struct AuxGlobalPositionSample {
uint64_t time_us{}; ///< timestamp of the measurement (uSec)
double latitude{};
double longitude{};
float altitude_amsl{};
float eph{};
float epv{};
uint8_t lat_lon_reset_counter{};
};
int32_t _id_param_values[MAX_AGP_IDS] {};
estimator_aid_source2d_s _aid_src_aux_global_position{};
TimestampedRingBuffer<AuxGlobalPositionSample> _aux_global_position_buffer{20}; // TODO: size with _obs_buffer_length and actual publication rate
uint64_t _time_last_buffer_push{0};
enum class Ctrl : uint8_t {
kHPos = (1 << 0),
kVPos = (1 << 1)
};
enum class Mode : uint8_t {
kAuto = 0, ///< Reset on fusion timeout if no other source of position is available
kDeadReckoning = 1 ///< Reset on fusion timeout if no source of velocity is availabl
};
enum class State {
kStopped,
kStarting,
kActive,
};
State _state{State::kStopped};
float _test_ratio_filtered{INFINITY};
#if defined(MODULE_NAME)
struct reset_counters_s {
uint8_t lat_lon{};
};
reset_counters_s _reset_counters{};
uORB::PublicationMulti<estimator_aid_source2d_s> _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<px4::params::EKF2_AGP_CTRL>) _param_ekf2_agp_ctrl,
(ParamInt<px4::params::EKF2_AGP_MODE>) _param_ekf2_agp_mode,
(ParamFloat<px4::params::EKF2_AGP_DELAY>) _param_ekf2_agp_delay,
(ParamFloat<px4::params::EKF2_AGP_NOISE>) _param_ekf2_agp_noise,
(ParamFloat<px4::params::EKF2_AGP_GATE>) _param_ekf2_agp_gate
)
#endif // MODULE_NAME
};
#endif // CONFIG_EKF2_AUX_GLOBAL_POSITION
#endif // CONFIG_EKF2_AUX_GLOBAL_POSITION && MODULE_NAME
#endif // !EKF_AUX_GLOBAL_POSITION_HPP

240
src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position_control.cpp Normal file
View File

@ -0,0 +1,240 @@
/****************************************************************************
*
* 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 <aid_sources/aux_global_position/aux_global_position_control.hpp>
#include <aid_sources/aux_global_position/aux_global_position.hpp>
#if defined(CONFIG_EKF2_AUX_GLOBAL_POSITION) && defined(MODULE_NAME)
AgpSource::AgpSource(int slot, AuxGlobalPosition *manager)
: _manager(manager)
, _slot(slot)
{
initParams();
advertise();
}
void AgpSource::initParams()
{
char param_name[20] {};
snprintf(param_name, sizeof(param_name), "EKF2_AGP%d_CTRL", _slot);
_param_handles.ctrl = param_find(param_name);
snprintf(param_name, sizeof(param_name), "EKF2_AGP%d_MODE", _slot);
_param_handles.mode = param_find(param_name);
snprintf(param_name, sizeof(param_name), "EKF2_AGP%d_DELAY", _slot);
_param_handles.delay = param_find(param_name);
snprintf(param_name, sizeof(param_name), "EKF2_AGP%d_NOISE", _slot);
_param_handles.noise = param_find(param_name);
snprintf(param_name, sizeof(param_name), "EKF2_AGP%d_GATE", _slot);
_param_handles.gate = param_find(param_name);
updateParams();
}
void AgpSource::updateParams()
{
if (_param_handles.ctrl == PARAM_INVALID) {
return;
}
param_get(_param_handles.ctrl, &_params.ctrl);
param_get(_param_handles.mode, &_params.mode);
param_get(_param_handles.delay, &_params.delay);
param_get(_param_handles.noise, &_params.noise);
param_get(_param_handles.gate, &_params.gate);
}
void AgpSource::bufferData(const aux_global_position_s &msg, const estimator::imuSample &imu_delayed)
{
const int64_t time_us = msg.timestamp_sample
- static_cast<int64_t>(_params.delay * 1000);
AuxGlobalPositionSample sample{};
sample.time_us = time_us;
sample.id = msg.id;
sample.latitude = msg.lat;
sample.longitude = msg.lon;
sample.altitude_amsl = msg.alt;
sample.eph = msg.eph;
sample.epv = msg.epv;
sample.lat_lon_reset_counter = msg.lat_lon_reset_counter;
_buffer.push(sample);
_time_last_buffer_push = imu_delayed.time_us;
}
bool AgpSource::update(Ekf &ekf, const estimator::imuSample &imu_delayed)
{
AuxGlobalPositionSample sample;
if (_buffer.pop_first_older_than(imu_delayed.time_us, &sample)) {
if (!(_params.ctrl & static_cast<int32_t>(Ctrl::kHPos))) {
return true;
}
const LatLonAlt position(sample.latitude, sample.longitude, sample.altitude_amsl);
const Vector2f innovation = (ekf.getLatLonAlt() - position).xy(); // altitude measurements are not used
float pos_noise = math::max(sample.eph, _params.noise, 0.01f);
const float pos_var = sq(pos_noise);
const Vector2f pos_obs_var(pos_var, pos_var);
ekf.updateAidSourceStatus(_aid_src,
sample.time_us, // sample timestamp
matrix::Vector2d(sample.latitude, sample.longitude), // observation
pos_obs_var, // observation variance
innovation, // innovation
Vector2f(ekf.getPositionVariance()) + pos_obs_var, // innovation variance
math::max(_params.gate, 1.f)); // innovation gate
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::kStopped:
/* FALLTHROUGH */
case State::kStarting:
if (starting_conditions) {
_state = State::kStarting;
if (ekf.global_origin_valid()) {
const bool fused = ekf.fuseHorizontalPosition(_aid_src);
bool reset = false;
if (!fused && isResetAllowed(ekf)) {
ekf.resetHorizontalPositionTo(sample.latitude, sample.longitude, Vector2f(_aid_src.observation_variance));
ekf.resetAidSourceStatusZeroInnovation(_aid_src);
reset = true;
}
if (fused || reset) {
ekf.enableControlStatusAuxGpos();
_reset_counters.lat_lon = sample.lat_lon_reset_counter;
_state = State::kActive;
}
} else {
// Try to initialize using measurement
if (ekf.resetGlobalPositionTo(sample.latitude, sample.longitude, sample.altitude_amsl, pos_var,
sq(sample.epv))) {
ekf.resetAidSourceStatusZeroInnovation(_aid_src);
ekf.enableControlStatusAuxGpos();
_reset_counters.lat_lon = sample.lat_lon_reset_counter;
_state = State::kActive;
}
}
}
break;
case State::kActive:
if (continuing_conditions) {
ekf.fuseHorizontalPosition(_aid_src);
if (isTimedOut(_aid_src.time_last_fuse, imu_delayed.time_us, ekf._params.reset_timeout_max)
|| (_reset_counters.lat_lon != sample.lat_lon_reset_counter)) {
if (isResetAllowed(ekf)) {
ekf.resetHorizontalPositionTo(sample.latitude, sample.longitude, Vector2f(_aid_src.observation_variance));
ekf.resetAidSourceStatusZeroInnovation(_aid_src);
_reset_counters.lat_lon = sample.lat_lon_reset_counter;
} else {
_state = State::kStopped;
if (!_manager->anySourceFusing()) {
ekf.disableControlStatusAuxGpos();
}
}
}
} else {
_state = State::kStopped;
if (!_manager->anySourceFusing()) {
ekf.disableControlStatusAuxGpos();
}
}
break;
default:
break;
}
_aid_src.timestamp = hrt_absolute_time();
_aid_src_pub.publish(_aid_src);
_test_ratio_filtered = math::max(fabsf(_aid_src.test_ratio_filtered[0]), fabsf(_aid_src.test_ratio_filtered[1]));
return true;
} else if ((_state != State::kStopped) && isTimedOut(_time_last_buffer_push, imu_delayed.time_us, (uint64_t)5e6)) {
_state = State::kStopped;
if (!_manager->anySourceFusing()) {
ekf.disableControlStatusAuxGpos();
}
ECL_INFO("Aux global position data stopped for slot %d", _slot);
}
return false;
}
bool AgpSource::isResetAllowed(const Ekf &ekf) const
{
return ((static_cast<Mode>(_params.mode) == Mode::kAuto)
&& !ekf.isOtherSourceOfHorizontalPositionAidingThan(ekf.control_status_flags().aux_gpos))
|| ((static_cast<Mode>(_params.mode) == Mode::kDeadReckoning)
&& !ekf.isOtherSourceOfHorizontalAidingThan(ekf.control_status_flags().aux_gpos));
}
bool AgpSource::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);
}
#endif // CONFIG_EKF2_AUX_GLOBAL_POSITION && MODULE_NAME

130
src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position_control.hpp Normal file
View File

@ -0,0 +1,130 @@
/****************************************************************************
*
* Copyright (c) 2026 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_CONTROL_HPP
#define EKF_AUX_GLOBAL_POSITION_CONTROL_HPP
#include "../../common.h"
#if defined(CONFIG_EKF2_AUX_GLOBAL_POSITION) && defined(MODULE_NAME)
#include <lib/parameters/param.h>
#include <uORB/PublicationMulti.hpp>
#include <uORB/topics/estimator_aid_source2d.h>
#include <uORB/topics/aux_global_position.h>
class Ekf;
class AuxGlobalPosition;
class AgpSource
{
public:
AgpSource(int slot, AuxGlobalPosition *manager);
~AgpSource() = default;
void bufferData(const aux_global_position_s &msg, const estimator::imuSample &imu_delayed);
bool update(Ekf &ekf, const estimator::imuSample &imu_delayed);
void advertise() { _aid_src_pub.advertise(); }
void initParams();
void updateParams();
float getTestRatioFiltered() const { return _test_ratio_filtered; }
bool isFusing() const { return _state == State::kActive; }
private:
struct AuxGlobalPositionSample {
uint64_t time_us{};
uint8_t id{};
double latitude{};
double longitude{};
float altitude_amsl{};
float eph{};
float epv{};
uint8_t lat_lon_reset_counter{};
};
enum class Ctrl : uint8_t {
kHPos = (1 << 0),
kVPos = (1 << 1)
};
enum class Mode : uint8_t {
kAuto = 0, ///< Reset on fusion timeout if no other source of position is available
kDeadReckoning = 1 ///< Reset on fusion timeout if no source of velocity is available
};
enum class State {
kStopped,
kStarting,
kActive,
};
struct reset_counters_s {
uint8_t lat_lon{};
};
uORB::PublicationMulti<estimator_aid_source2d_s> _aid_src_pub{ORB_ID(estimator_aid_src_aux_global_position)};
TimestampedRingBuffer<AuxGlobalPositionSample> _buffer{20};
State _state{State::kStopped};
estimator_aid_source2d_s _aid_src{};
float _test_ratio_filtered{0.f};
uint64_t _time_last_buffer_push{0};
reset_counters_s _reset_counters{};
AuxGlobalPosition *_manager;
int _slot;
struct ParamHandles {
param_t ctrl{PARAM_INVALID};
param_t mode{PARAM_INVALID};
param_t delay{PARAM_INVALID};
param_t noise{PARAM_INVALID};
param_t gate{PARAM_INVALID};
} _param_handles;
struct Params {
int32_t ctrl{0};
int32_t mode{0};
float delay{0.f};
float noise{10.f};
float gate{3.f};
} _params;
bool isResetAllowed(const Ekf &ekf) const;
bool isTimedOut(uint64_t last_sensor_timestamp, uint64_t time_delayed_us, uint64_t timeout_period) const;
};
#endif // CONFIG_EKF2_AUX_GLOBAL_POSITION && MODULE_NAME
#endif // !EKF_AUX_GLOBAL_POSITION_CONTROL_HPP

3
src/modules/ekf2/EKF/ekf.cpp
View File

@ -419,7 +419,8 @@ void Ekf::updateParameters()
#endif // CONFIG_EKF2_WIND
#if defined(CONFIG_EKF2_AUX_GLOBAL_POSITION) && defined(MODULE_NAME)
_aux_global_position.updateParameters();
_aux_global_position.paramsUpdated();
#endif // CONFIG_EKF2_AUX_GLOBAL_POSITION
}

1
src/modules/ekf2/EKF/ekf.h
View File

@ -435,6 +435,7 @@ public:
void updateParameters();
friend class AuxGlobalPosition;
friend class AgpSource;
private:

2
src/modules/ekf2/EKF/ekf_helper.cpp
View File

@ -541,7 +541,7 @@ float Ekf::getHorizontalPositionInnovationTestRatio() const
#if defined(CONFIG_EKF2_AUX_GLOBAL_POSITION) && defined(MODULE_NAME)
if (_control_status.flags.aux_gpos) {
test_ratio = math::max(test_ratio, fabsf(_aux_global_position.test_ratio_filtered()));
test_ratio = math::max(test_ratio, fabsf(_aux_global_position.testRatioFiltered()));
}
#endif // CONFIG_EKF2_AUX_GLOBAL_POSITION