dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

ekf2-agp: follow measurement reset

This commit is contained in:
bresch 2024-01-19 14:48:05 +01:00 committed by Daniel Agar
parent 5d9081b0dd
commit 9c02e384e6
2 changed files with 12 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
src/modules/ekf2/EKF/aux_global_position.cpp
View File

@ -56,6 +56,7 @@ void AuxGlobalPosition::update(Ekf &ekf, const estimator::imuSample &imu_delayed
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);
@ -103,12 +104,14 @@ void AuxGlobalPosition::update(Ekf &ekf, const estimator::imuSample &imu_delayed
if (ekf.global_origin_valid()) {
ekf.enableControlStatusAuxGpos();
_reset_counters.lat_lon = sample.lat_lon_reset_counter;
_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();
_reset_counters.lat_lon = sample.lat_lon_reset_counter;
_state = State::active;
}
}
@ -119,9 +122,11 @@ void AuxGlobalPosition::update(Ekf &ekf, const estimator::imuSample &imu_delayed
if (continuing_conditions) {
ekf.fuseHorizontalPosition(aid_src);
if (isTimedOut(aid_src.time_last_fuse, imu_delayed.time_us, ekf._params.no_aid_timeout_max)) {
if (isTimedOut(aid_src.time_last_fuse, imu_delayed.time_us, ekf._params.no_aid_timeout_max)
|| (_reset_counters.lat_lon != sample.lat_lon_reset_counter)) {
ekf.resetHorizontalPositionTo(Vector2f(aid_src.observation), Vector2f(aid_src.observation_variance));
aid_src.time_last_fuse = imu_delayed.time_us;
_reset_counters.lat_lon = sample.lat_lon_reset_counter;
}
} else {

6
src/modules/ekf2/EKF/aux_global_position.hpp
View File

@ -87,6 +87,7 @@ private:
float altitude_amsl{};
float eph{};
float epv{};
uint8_t lat_lon_reset_counter{};
};
RingBuffer<AuxGlobalPositionSample> _aux_global_position_buffer{20}; // TODO: size with _obs_buffer_length and actual publication rate
@ -106,6 +107,11 @@ private:
State _state{State::stopped};
#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)};