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: move AGP subs to manager class to always route messages to correct source

Browse Source
This commit is contained in:
Marco Hauswirth 2026-02-12 15:38:55 +01:00
parent 1dae88c661
commit 77a4e3f4d8
4 changed files with 84 additions and 64 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -39,11 +39,19 @@
AuxGlobalPosition::AuxGlobalPosition() : ModuleParams(nullptr)
{
for (int i = 0; i < MAX_AGP_IDS; i++) {
_id_param_values[i] = getAgpParamInt32("ID", i);
for (int slot = 0; slot < MAX_AGP_IDS; slot++) {
_id_param_values[slot] = getAgpParamInt32("ID", slot);
if (_id_param_values[i] != 0) {
_sources[i] = new AgpSource(i, this);
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);
}
}
}
@ -57,10 +65,36 @@ AuxGlobalPosition::~AuxGlobalPosition()
void AuxGlobalPosition::update(Ekf &ekf, const estimator::imuSample &imu_delayed)
{
for (int i = 0; i < MAX_AGP_IDS; i++) {
if (_sources[i]) {
_sources[i]->checkAndBufferData(imu_delayed);
_sources[i]->update(ekf, imu_delayed);
// 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;
}
for (int instance = 0; instance < MAX_AGP_IDS; instance++) {
if (_agp_sub[instance].updated()) {
aux_global_position_s msg{};
_agp_sub[instance].copy(&msg);
int slot = _instance_slot_map[instance];
if (slot < 0) {
slot = mapSensorIdToSlot(msg.id);
_instance_slot_map[instance] = static_cast<int8_t>(slot);
}
if (slot >= 0 && _sources[slot]) {
_sources[slot]->bufferData(msg, imu_delayed);
}
}
}
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;
}
}
}
}
@ -146,7 +180,7 @@ int AuxGlobalPosition::mapSensorIdToSlot(int32_t sensor_id)
}
}
return MAX_AGP_IDS;
return -1;
}
#endif // CONFIG_EKF2_AUX_GLOBAL_POSITION && MODULE_NAME

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

@ -41,6 +41,8 @@
#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;
@ -72,6 +74,9 @@ public:
private:
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};
int32_t getAgpParamInt32(const char *param_suffix, int instance) const;
bool setAgpParamInt32(const char *param_suffix, int instance, int32_t value);

75
src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position_control.cpp
View File

@ -37,10 +37,9 @@
#if defined(CONFIG_EKF2_AUX_GLOBAL_POSITION) && defined(MODULE_NAME)
AgpSource::AgpSource(int instance_id, AuxGlobalPosition *manager)
: _agp_sub(ORB_ID(aux_global_position), instance_id)
, _manager(manager)
, _instance_id(instance_id)
AgpSource::AgpSource(int slot, AuxGlobalPosition *manager)
: _manager(manager)
, _slot(slot)
{
initParams();
advertise();
@ -50,19 +49,19 @@ void AgpSource::initParams()
{
char param_name[20] {};
snprintf(param_name, sizeof(param_name), "EKF2_AGP%d_CTRL", _instance_id);
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", _instance_id);
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", _instance_id);
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", _instance_id);
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", _instance_id);
snprintf(param_name, sizeof(param_name), "EKF2_AGP%d_GATE", _slot);
_param_handles.gate = param_find(param_name);
updateParams();
@ -74,7 +73,6 @@ void AgpSource::updateParams()
return;
}
param_get(_param_handles.id, &_params.id);
param_get(_param_handles.ctrl, &_params.ctrl);
param_get(_param_handles.mode, &_params.mode);
param_get(_param_handles.delay, &_params.delay);
@ -82,52 +80,33 @@ void AgpSource::updateParams()
param_get(_param_handles.gate, &_params.gate);
}
void AgpSource::checkAndBufferData(const estimator::imuSample &imu_delayed)
void AgpSource::bufferData(const aux_global_position_s &msg, const estimator::imuSample &imu_delayed)
{
if (_agp_sub.updated()) {
const int64_t time_us = msg.timestamp_sample
- static_cast<int64_t>(_params.delay * 1000);
aux_global_position_s aux_global_position{};
_agp_sub.copy(&aux_global_position);
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;
const uint8_t sensor_id = aux_global_position.id;
const int slot = _manager->mapSensorIdToSlot(sensor_id);
if (slot >= AuxGlobalPosition::MAX_AGP_IDS) {
// All parameter slots are full, cannot handle this sensor
return;
}
if (slot != _instance_id) {
// This sensor is mapped to a different instance
return;
}
const int64_t time_us = aux_global_position.timestamp_sample
- static_cast<int64_t>(_params.delay * 1000);
AuxGlobalPositionSample sample{};
sample.time_us = time_us;
sample.id = sensor_id;
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;
_buffer.push(sample);
_time_last_buffer_push = imu_delayed.time_us;
}
_buffer.push(sample);
_time_last_buffer_push = imu_delayed.time_us;
}
void AgpSource::update(Ekf &ekf, const estimator::imuSample &imu_delayed)
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;
return true;
}
const LatLonAlt position(sample.latitude, sample.longitude, sample.altitude_amsl);
@ -230,13 +209,19 @@ void AgpSource::update(Ekf &ekf, const estimator::imuSample &imu_delayed)
_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

16
src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position_control.hpp
View File

@ -40,7 +40,6 @@
#include <lib/parameters/param.h>
#include <uORB/PublicationMulti.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/topics/estimator_aid_source2d.h>
#include <uORB/topics/aux_global_position.h>
@ -50,11 +49,11 @@ class AuxGlobalPosition;
class AgpSource
{
public:
AgpSource(int instance_id, AuxGlobalPosition *manager);
AgpSource(int slot, AuxGlobalPosition *manager);
~AgpSource() = default;
void checkAndBufferData(const estimator::imuSample &imu_delayed);
void update(Ekf &ekf, const estimator::imuSample &imu_delayed);
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();
@ -64,8 +63,8 @@ public:
private:
struct AuxGlobalPositionSample {
uint64_t time_us{}; ///< timestamp of the measurement (uSec)
uint8_t id{}; ///< source identifier
uint64_t time_us{};
uint8_t id{};
double latitude{};
double longitude{};
float altitude_amsl{};
@ -94,7 +93,6 @@ private:
uint8_t lat_lon{};
};
uORB::Subscription _agp_sub;
uORB::PublicationMulti<estimator_aid_source2d_s> _aid_src_pub{ORB_ID(estimator_aid_src_aux_global_position)};
TimestampedRingBuffer<AuxGlobalPositionSample> _buffer{20};
@ -105,10 +103,9 @@ private:
reset_counters_s _reset_counters{};
AuxGlobalPosition *_manager;
const int _instance_id;
int _slot;
struct ParamHandles {
param_t id{PARAM_INVALID};
param_t ctrl{PARAM_INVALID};
param_t mode{PARAM_INVALID};
param_t delay{PARAM_INVALID};
@ -117,7 +114,6 @@ private:
} _param_handles;
struct Params {
int32_t id{0};
int32_t ctrl{0};
int32_t mode{0};
float delay{0.f};