Skip to content

Commit 8542261

Browse files
haumarcohaumarco
committed
ekf2: implement multi-instance AGP fusion
- Support up to 4 independent AGP sources simultaneously - Dynamic parameter lookup without boilerplate switch statements - Per-source state tracking and fusion logic - Update simulator to use new AuxGlobalPosition message
1 parent b9af959 commit 8542261

File tree

5 files changed

+266
-134
lines changed

5 files changed

+266
-134
lines changed

‎src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.cpp‎

Lines changed: 117 additions & 105 deletions
Original file line numberDiff line numberDiff line change
@@ -42,147 +42,159 @@ void AuxGlobalPosition::update(Ekf &ekf, const estimator::imuSample &imu_delayed
4242

4343
#if defined(MODULE_NAME)
4444

45-
if (_aux_global_position_sub.updated()) {
45+
for (int instance = 0; instance < MAX_AGP_IDS; instance++) {
46+
if (_aux_global_position_subs[instance].updated()) {
4647

47-
vehicle_global_position_s aux_global_position{};
48-
_aux_global_position_sub.copy(&aux_global_position);
48+
aux_global_position_s aux_global_position{};
49+
_aux_global_position_subs[instance].copy(&aux_global_position);
4950

50-
const int64_t time_us = aux_global_position.timestamp_sample - static_cast<int64_t>(_param_ekf2_agp_delay.get() * 1000);
51+
const uint8_t sensor_id = aux_global_position.id;
52+
const int slot = mapSensorIdToSlot(sensor_id);
5153

52-
AuxGlobalPositionSample sample{};
53-
sample.time_us = time_us;
54-
sample.latitude = aux_global_position.lat;
55-
sample.longitude = aux_global_position.lon;
56-
sample.altitude_amsl = aux_global_position.alt;
57-
sample.eph = aux_global_position.eph;
58-
sample.epv = aux_global_position.epv;
59-
sample.lat_lon_reset_counter = aux_global_position.lat_lon_reset_counter;
54+
if (slot >= MAX_AGP_IDS) {
55+
// All parameter slots are full, cannot handle this sensor. Set ID of unused slot to 0
56+
continue;
57+
}
58+
59+
const int64_t time_us = aux_global_position.timestamp_sample - static_cast<int64_t>(getDelayParam(slot) * 1000);
6060

61-
_aux_global_position_buffer.push(sample);
61+
AuxGlobalPositionSample sample{};
62+
sample.time_us = time_us;
63+
sample.id = sensor_id;
64+
sample.latitude = aux_global_position.lat;
65+
sample.longitude = aux_global_position.lon;
66+
sample.altitude_amsl = aux_global_position.alt;
67+
sample.eph = aux_global_position.eph;
68+
sample.epv = aux_global_position.epv;
69+
sample.lat_lon_reset_counter = aux_global_position.lat_lon_reset_counter;
6270

63-
_time_last_buffer_push = imu_delayed.time_us;
71+
_sources[slot].buffer.push(sample);
72+
_sources[slot].time_last_buffer_push = imu_delayed.time_us;
73+
}
6474
}
6575

6676
#endif // MODULE_NAME
6777

68-
AuxGlobalPositionSample sample;
78+
for (int slot = 0; slot < MAX_AGP_IDS; slot++) {
79+
SourceData &source = _sources[slot];
80+
AuxGlobalPositionSample sample;
6981

70-
if (_aux_global_position_buffer.pop_first_older_than(imu_delayed.time_us, &sample)) {
71-
72-
if (!(_param_ekf2_agp_ctrl.get() & static_cast<int32_t>(Ctrl::kHPos))) {
73-
return;
74-
}
82+
if (source.buffer.pop_first_older_than(imu_delayed.time_us, &sample)) {
7583

76-
estimator_aid_source2d_s &aid_src = _aid_src_aux_global_position;
77-
const LatLonAlt position(sample.latitude, sample.longitude, sample.altitude_amsl);
78-
const Vector2f innovation = (ekf.getLatLonAlt() - position).xy(); // altitude measurements are not used
79-
80-
// relax the upper observation noise limit which prevents bad measurements perturbing the position estimate
81-
float pos_noise = math::max(sample.eph, _param_ekf2_agp_noise.get(), 0.01f);
82-
const float pos_var = sq(pos_noise);
83-
const Vector2f pos_obs_var(pos_var, pos_var);
84-
85-
ekf.updateAidSourceStatus(aid_src,
86-
sample.time_us, // sample timestamp
87-
matrix::Vector2d(sample.latitude, sample.longitude), // observation
88-
pos_obs_var, // observation variance
89-
innovation, // innovation
90-
Vector2f(ekf.getPositionVariance()) + pos_obs_var, // innovation variance
91-
math::max(_param_ekf2_agp_gate.get(), 1.f)); // innovation gate
92-
93-
const bool starting_conditions = PX4_ISFINITE(sample.latitude) && PX4_ISFINITE(sample.longitude)
94-
&& ekf.control_status_flags().yaw_align;
95-
const bool continuing_conditions = starting_conditions
96-
&& ekf.global_origin_valid();
97-
98-
switch (_state) {
99-
case State::kStopped:
100-
101-
/* FALLTHROUGH */
102-
case State::kStarting:
103-
if (starting_conditions) {
104-
_state = State::kStarting;
105-
106-
if (ekf.global_origin_valid()) {
107-
const bool fused = ekf.fuseHorizontalPosition(aid_src);
108-
bool reset = false;
109-
110-
if (!fused && isResetAllowed(ekf)) {
111-
ekf.resetHorizontalPositionTo(sample.latitude, sample.longitude, Vector2f(aid_src.observation_variance));
112-
ekf.resetAidSourceStatusZeroInnovation(aid_src);
113-
reset = true;
114-
}
84+
if (!(getCtrlParam(slot) & static_cast<int32_t>(Ctrl::kHPos))) {
85+
continue;
86+
}
11587

116-
if (fused || reset) {
117-
ekf.enableControlStatusAuxGpos();
118-
_reset_counters.lat_lon = sample.lat_lon_reset_counter;
119-
_state = State::kActive;
120-
}
88+
estimator_aid_source2d_s &aid_src = source.aid_src;
89+
const LatLonAlt position(sample.latitude, sample.longitude, sample.altitude_amsl);
90+
const Vector2f innovation = (ekf.getLatLonAlt() - position).xy(); // altitude measurements are not used
91+
92+
float pos_noise = math::max(sample.eph, getNoiseParam(slot), 0.01f);
93+
const float pos_var = sq(pos_noise);
94+
const Vector2f pos_obs_var(pos_var, pos_var);
95+
96+
ekf.updateAidSourceStatus(aid_src,
97+
sample.time_us, // sample timestamp
98+
matrix::Vector2d(sample.latitude, sample.longitude), // observation
99+
pos_obs_var, // observation variance
100+
innovation, // innovation
101+
Vector2f(ekf.getPositionVariance()) + pos_obs_var, // innovation variance
102+
math::max(getGateParam(slot), 1.f)); // innovation gate
103+
104+
const bool starting_conditions = PX4_ISFINITE(sample.latitude) && PX4_ISFINITE(sample.longitude)
105+
&& ekf.control_status_flags().yaw_align;
106+
const bool continuing_conditions = starting_conditions
107+
&& ekf.global_origin_valid();
108+
109+
switch (source.state) {
110+
case State::kStopped:
111+
112+
/* FALLTHROUGH */
113+
case State::kStarting:
114+
if (starting_conditions) {
115+
source.state = State::kStarting;
116+
117+
if (ekf.global_origin_valid()) {
118+
const bool fused = ekf.fuseHorizontalPosition(aid_src);
119+
bool reset = false;
120+
121+
if (!fused && isResetAllowed(ekf, slot)) {
122+
ekf.resetHorizontalPositionTo(sample.latitude, sample.longitude, Vector2f(aid_src.observation_variance));
123+
ekf.resetAidSourceStatusZeroInnovation(aid_src);
124+
reset = true;
125+
}
126+
127+
if (fused || reset) {
128+
ekf.enableControlStatusAuxGpos();
129+
source.reset_counters.lat_lon = sample.lat_lon_reset_counter;
130+
source.state = State::kActive;
131+
}
121132

122-
} else {
123-
// Try to initialize using measurement
124-
if (ekf.resetGlobalPositionTo(sample.latitude, sample.longitude, sample.altitude_amsl, pos_var,
125-
sq(sample.epv))) {
126-
ekf.resetAidSourceStatusZeroInnovation(aid_src);
127-
ekf.enableControlStatusAuxGpos();
128-
_reset_counters.lat_lon = sample.lat_lon_reset_counter;
129-
_state = State::kActive;
133+
} else {
134+
// Try to initialize using measurement
135+
if (ekf.resetGlobalPositionTo(sample.latitude, sample.longitude, sample.altitude_amsl, pos_var,
136+
sq(sample.epv))) {
137+
ekf.resetAidSourceStatusZeroInnovation(aid_src);
138+
ekf.enableControlStatusAuxGpos();
139+
source.reset_counters.lat_lon = sample.lat_lon_reset_counter;
140+
source.state = State::kActive;
141+
}
130142
}
131143
}
132-
}
133144

134-
break;
145+
break;
135146

136-
case State::kActive:
137-
if (continuing_conditions) {
138-
ekf.fuseHorizontalPosition(aid_src);
147+
case State::kActive:
148+
if (continuing_conditions) {
149+
ekf.fuseHorizontalPosition(aid_src);
139150

140-
if (isTimedOut(aid_src.time_last_fuse, imu_delayed.time_us, ekf._params.reset_timeout_max)
141-
|| (_reset_counters.lat_lon != sample.lat_lon_reset_counter)) {
142-
if (isResetAllowed(ekf)) {
151+
if (isTimedOut(aid_src.time_last_fuse, imu_delayed.time_us, ekf._params.reset_timeout_max)
152+
|| (source.reset_counters.lat_lon != sample.lat_lon_reset_counter)) {
153+
if (isResetAllowed(ekf, slot)) {
143154

144-
ekf.resetHorizontalPositionTo(sample.latitude, sample.longitude, Vector2f(aid_src.observation_variance));
155+
ekf.resetHorizontalPositionTo(sample.latitude, sample.longitude, Vector2f(aid_src.observation_variance));
145156

146-
ekf.resetAidSourceStatusZeroInnovation(aid_src);
157+
ekf.resetAidSourceStatusZeroInnovation(aid_src);
147158

148-
_reset_counters.lat_lon = sample.lat_lon_reset_counter;
159+
source.reset_counters.lat_lon = sample.lat_lon_reset_counter;
149160

150-
} else {
151-
ekf.disableControlStatusAuxGpos();
152-
_state = State::kStopped;
161+
} else {
162+
ekf.disableControlStatusAuxGpos();
163+
source.state = State::kStopped;
164+
}
153165
}
154-
}
155166

156-
} else {
157-
ekf.disableControlStatusAuxGpos();
158-
_state = State::kStopped;
159-
}
167+
} else {
168+
ekf.disableControlStatusAuxGpos();
169+
source.state = State::kStopped;
170+
}
160171

161-
break;
172+
break;
162173

163-
default:
164-
break;
165-
}
174+
default:
175+
break;
176+
}
166177

167178
#if defined(MODULE_NAME)
168-
aid_src.timestamp = hrt_absolute_time();
169-
_estimator_aid_src_aux_global_position_pub.publish(aid_src);
179+
aid_src.timestamp = hrt_absolute_time();
180+
source.aid_src_pub.publish(aid_src);
170181

171-
_test_ratio_filtered = math::max(fabsf(aid_src.test_ratio_filtered[0]), fabsf(aid_src.test_ratio_filtered[1]));
182+
source.test_ratio_filtered = math::max(fabsf(aid_src.test_ratio_filtered[0]), fabsf(aid_src.test_ratio_filtered[1]));
172183
#endif // MODULE_NAME
173184

174-
} else if ((_state != State::kStopped) && isTimedOut(_time_last_buffer_push, imu_delayed.time_us, (uint64_t)5e6)) {
175-
ekf.disableControlStatusAuxGpos();
176-
_state = State::kStopped;
177-
ECL_WARN("Aux global position data stopped");
185+
} else if ((source.state != State::kStopped) && isTimedOut(source.time_last_buffer_push, imu_delayed.time_us, (uint64_t)5e6)) {
186+
ekf.disableControlStatusAuxGpos();
187+
source.state = State::kStopped;
188+
ECL_WARN("Aux global position data stopped for slot %d (sensor ID %d)", slot, getIdParam(slot));
189+
}
178190
}
179191
}
180192

181-
bool AuxGlobalPosition::isResetAllowed(const Ekf &ekf) const
193+
bool AuxGlobalPosition::isResetAllowed(const Ekf &ekf, int slot) const
182194
{
183-
return ((static_cast<Mode>(_param_ekf2_agp_mode.get()) == Mode::kAuto)
195+
return ((static_cast<Mode>(getModeParam(slot)) == Mode::kAuto)
184196
&& !ekf.isOtherSourceOfHorizontalPositionAidingThan(ekf.control_status_flags().aux_gpos))
185-
|| ((static_cast<Mode>(_param_ekf2_agp_mode.get()) == Mode::kDeadReckoning)
197+
|| ((static_cast<Mode>(getModeParam(slot)) == Mode::kDeadReckoning)
186198
&& !ekf.isOtherSourceOfHorizontalAidingThan(ekf.control_status_flags().aux_gpos));
187199
}
188200

0 commit comments

Comments
 (0)