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

ekf2: put agp params into struct, nullptr init for AgpSource list

Browse Source
This commit is contained in:
Marco Hauswirth
2026-02-12 16:10:31 +01:00
parent 7dd51038e5
commit 62ee63ed7e
4 changed files with 40 additions and 35 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.cpp
+5 -7
View File
@@ -51,16 +51,14 @@ AuxGlobalPosition::AuxGlobalPosition() : ModuleParams(nullptr)
AuxGlobalPosition::~AuxGlobalPosition()
{
for (int i = 0; i < MAX_AGP_IDS; i++) {
if (_id_param_values[i] != 0) {
delete _sources[i];
}
delete _sources[i];
}
}
void AuxGlobalPosition::update(Ekf &ekf, const estimator::imuSample &imu_delayed)
{
for (int i = 0; i < MAX_AGP_IDS; i++) {
if (_id_param_values[i] != 0) {
if (_sources[i]) {
_sources[i]->checkAndBufferData(imu_delayed);
_sources[i]->update(ekf, imu_delayed);
}
@@ -70,7 +68,7 @@ void AuxGlobalPosition::update(Ekf &ekf, const estimator::imuSample &imu_delayed
void AuxGlobalPosition::paramsUpdated()
{
for (int i = 0; i < MAX_AGP_IDS; i++) {
if (_id_param_values[i] != 0) {
if (_sources[i]) {
_sources[i]->updateParams();
}
}
@@ -81,7 +79,7 @@ float AuxGlobalPosition::test_ratio_filtered() const
float max_ratio = 0.f;
for (int i = 0; i < MAX_AGP_IDS; i++) {
if (_id_param_values[i] != 0 && _sources[i]->isFusing()) {
if (_sources[i] && _sources[i]->isFusing()) {
max_ratio = math::max(max_ratio, _sources[i]->getTestRatioFiltered());
}
}
@@ -92,7 +90,7 @@ float AuxGlobalPosition::test_ratio_filtered() const
bool AuxGlobalPosition::anySourceFusing() const
{
for (int i = 0; i < MAX_AGP_IDS; i++) {
if (_id_param_values[i] != 0 && _sources[i]->isFusing()) {
if (_sources[i] && _sources[i]->isFusing()) {
return true;
}
}
src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.hpp
+1 -1
View File
@@ -68,7 +68,7 @@ public:
void paramsUpdated();
private:
AgpSource *_sources[MAX_AGP_IDS];
AgpSource *_sources[MAX_AGP_IDS] {};
int32_t getAgpParamInt32(const char *param_suffix, int instance) const;
bool setAgpParamInt32(const char *param_suffix, int instance, int32_t value);
src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position_control.cpp
+18 -17
View File
@@ -51,34 +51,35 @@ void AgpSource::initParams()
char param_name[20] {};
snprintf(param_name, sizeof(param_name), "EKF2_AGP%d_CTRL", _instance_id);
_param_ctrl = param_find(param_name);
_param_handles.ctrl = param_find(param_name);
snprintf(param_name, sizeof(param_name), "EKF2_AGP%d_MODE", _instance_id);
_param_mode = param_find(param_name);
_param_handles.mode = param_find(param_name);
snprintf(param_name, sizeof(param_name), "EKF2_AGP%d_DELAY", _instance_id);
_param_delay = param_find(param_name);
_param_handles.delay = param_find(param_name);
snprintf(param_name, sizeof(param_name), "EKF2_AGP%d_NOISE", _instance_id);
_param_noise = param_find(param_name);
_param_handles.noise = param_find(param_name);
snprintf(param_name, sizeof(param_name), "EKF2_AGP%d_GATE", _instance_id);
_param_gate = param_find(param_name);
_param_handles.gate = param_find(param_name);
updateParams();
}
void AgpSource::updateParams()
{
if (_param_ctrl == PARAM_INVALID) {
if (_param_handles.ctrl == PARAM_INVALID) {
return;
}
param_get(_param_ctrl, &_ctrl);
param_get(_param_mode, &_mode);
param_get(_param_delay, &_delay);
param_get(_param_noise, &_noise);
param_get(_param_gate, &_gate);
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);
param_get(_param_handles.noise, &_params.noise);
param_get(_param_handles.gate, &_params.gate);
}
void AgpSource::checkAndBufferData(const estimator::imuSample &imu_delayed)
@@ -102,7 +103,7 @@ void AgpSource::checkAndBufferData(const estimator::imuSample &imu_delayed)
}
const int64_t time_us = aux_global_position.timestamp_sample
- static_cast<int64_t>(_delay * 1000);
- static_cast<int64_t>(_params.delay * 1000);
AuxGlobalPositionSample sample{};
sample.time_us = time_us;
@@ -125,14 +126,14 @@ void AgpSource::update(Ekf &ekf, const estimator::imuSample &imu_delayed)
if (_buffer.pop_first_older_than(imu_delayed.time_us, &sample)) {
if (!(_ctrl & static_cast<int32_t>(Ctrl::kHPos))) {
if (!(_params.ctrl & static_cast<int32_t>(Ctrl::kHPos))) {
return;
}
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, _noise, 0.01f);
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);
@@ -142,7 +143,7 @@ void AgpSource::update(Ekf &ekf, const estimator::imuSample &imu_delayed)
pos_obs_var, // observation variance
innovation, // innovation
Vector2f(ekf.getPositionVariance()) + pos_obs_var, // innovation variance
math::max(_gate, 1.f)); // innovation gate
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;
@@ -242,9 +243,9 @@ void AgpSource::update(Ekf &ekf, const estimator::imuSample &imu_delayed)
bool AgpSource::isResetAllowed(const Ekf &ekf) const
{
return ((static_cast<Mode>(_mode) == Mode::kAuto)
return ((static_cast<Mode>(_params.mode) == Mode::kAuto)
&& !ekf.isOtherSourceOfHorizontalPositionAidingThan(ekf.control_status_flags().aux_gpos))
|| ((static_cast<Mode>(_mode) == Mode::kDeadReckoning)
|| ((static_cast<Mode>(_params.mode) == Mode::kDeadReckoning)
&& !ekf.isOtherSourceOfHorizontalAidingThan(ekf.control_status_flags().aux_gpos));
}
src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position_control.hpp
+16 -10
View File
@@ -107,17 +107,23 @@ private:
AuxGlobalPosition *_manager;
const int _instance_id;
param_t _param_ctrl{PARAM_INVALID};
param_t _param_mode{PARAM_INVALID};
param_t _param_delay{PARAM_INVALID};
param_t _param_noise{PARAM_INVALID};
param_t _param_gate{PARAM_INVALID};
struct ParamHandles {
param_t id{PARAM_INVALID};
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;
int32_t _ctrl{0};
int32_t _mode{0};
float _delay{0.f};
float _noise{10.f};
float _gate{3.f};
struct Params {
int32_t id{0};
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;