Skip to content

Commit

Permalink
ekf2: migrate uorb events to events interface
Browse files Browse the repository at this point in the history
  • Loading branch information
@bresch
bresch committed Mar 18, 2024
1 parent da39d07 commit 80cefa3
Show file tree
Hide file tree
Showing 7 changed files with 227 additions and 98 deletions.
2 changes: 1 addition & 1 deletion Tools/px4events/srcparser.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -126,7 +126,7 @@ def finalize_current_tag(event, tag, value):
descr = descr[:i] + ' ' + descr[i+1:]
event.description = descr
elif tag == "group":
known_groups = ["calibration", "health", "arming_check", "default"]
known_groups = ["calibration", "health", "arming_check", "default", "ekf2"]
event.group = value.strip()
if not event.group in known_groups:
raise Exception("Unknown event group: '{}'\nKnown groups: {}\n" \
Expand Down
1 change: 0 additions & 1 deletion msg/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -81,7 +81,6 @@ set(msg_files
EstimatorAidSource3d.msg
EstimatorBias.msg
EstimatorBias3d.msg
EstimatorEventFlags.msg
EstimatorGpsStatus.msg
EstimatorInnovations.msg
EstimatorSelectorStatus.msg
Expand Down
37 changes: 0 additions & 37 deletions msg/EstimatorEventFlags.msg
View file

This file was deleted.

274 changes: 226 additions & 48 deletions src/modules/ekf2/EKF2.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -217,7 +217,6 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
_attitude_pub.advertise();
_local_position_pub.advertise();

_estimator_event_flags_pub.advertise();
_estimator_innovation_test_ratios_pub.advertise();
_estimator_innovation_variances_pub.advertise();
_estimator_innovations_pub.advertise();
Expand All @@ -238,7 +237,6 @@ bool EKF2::multi_init(int imu, int mag)
{
// advertise all topics to ensure consistent uORB instance numbering
_ekf2_timestamps_pub.advertise();
_estimator_event_flags_pub.advertise();
_estimator_innovation_test_ratios_pub.advertise();
_estimator_innovation_variances_pub.advertise();
_estimator_innovations_pub.advertise();
Expand Down Expand Up @@ -1050,55 +1048,235 @@ void EKF2::PublishEventFlags(const hrt_abstime &timestamp)
}

if (information_event_updated || warning_event_updated) {
estimator_event_flags_s event_flags{};
event_flags.timestamp_sample = _ekf.time_delayed_us();

event_flags.information_event_changes = _filter_information_event_changes;
event_flags.gps_checks_passed = _ekf.information_event_flags().gps_checks_passed;
event_flags.reset_vel_to_gps = _ekf.information_event_flags().reset_vel_to_gps;
event_flags.reset_vel_to_flow = _ekf.information_event_flags().reset_vel_to_flow;
event_flags.reset_vel_to_vision = _ekf.information_event_flags().reset_vel_to_vision;
event_flags.reset_vel_to_zero = _ekf.information_event_flags().reset_vel_to_zero;
event_flags.reset_pos_to_last_known = _ekf.information_event_flags().reset_pos_to_last_known;
event_flags.reset_pos_to_gps = _ekf.information_event_flags().reset_pos_to_gps;
event_flags.reset_pos_to_vision = _ekf.information_event_flags().reset_pos_to_vision;
event_flags.starting_gps_fusion = _ekf.information_event_flags().starting_gps_fusion;
event_flags.starting_vision_pos_fusion = _ekf.information_event_flags().starting_vision_pos_fusion;
event_flags.starting_vision_vel_fusion = _ekf.information_event_flags().starting_vision_vel_fusion;
event_flags.starting_vision_yaw_fusion = _ekf.information_event_flags().starting_vision_yaw_fusion;
event_flags.yaw_aligned_to_imu_gps = _ekf.information_event_flags().yaw_aligned_to_imu_gps;
event_flags.reset_hgt_to_baro = _ekf.information_event_flags().reset_hgt_to_baro;
event_flags.reset_hgt_to_gps = _ekf.information_event_flags().reset_hgt_to_gps;
event_flags.reset_hgt_to_rng = _ekf.information_event_flags().reset_hgt_to_rng;
event_flags.reset_hgt_to_ev = _ekf.information_event_flags().reset_hgt_to_ev;

event_flags.warning_event_changes = _filter_warning_event_changes;
event_flags.gps_quality_poor = _ekf.warning_event_flags().gps_quality_poor;
event_flags.gps_fusion_timout = _ekf.warning_event_flags().gps_fusion_timout;
event_flags.gps_data_stopped = _ekf.warning_event_flags().gps_data_stopped;
event_flags.gps_data_stopped_using_alternate = _ekf.warning_event_flags().gps_data_stopped_using_alternate;
event_flags.height_sensor_timeout = _ekf.warning_event_flags().height_sensor_timeout;
event_flags.stopping_navigation = _ekf.warning_event_flags().stopping_mag_use;
event_flags.invalid_accel_bias_cov_reset = _ekf.warning_event_flags().invalid_accel_bias_cov_reset;
event_flags.bad_yaw_using_gps_course = _ekf.warning_event_flags().bad_yaw_using_gps_course;
event_flags.stopping_mag_use = _ekf.warning_event_flags().stopping_mag_use;
event_flags.vision_data_stopped = _ekf.warning_event_flags().vision_data_stopped;
event_flags.emergency_yaw_reset_mag_stopped = _ekf.warning_event_flags().emergency_yaw_reset_mag_stopped;
event_flags.emergency_yaw_reset_gps_yaw_stopped = _ekf.warning_event_flags().emergency_yaw_reset_gps_yaw_stopped;

event_flags.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
_estimator_event_flags_pub.update(event_flags);

_last_event_flags_publish = event_flags.timestamp;
if (_ekf.information_event_flags().gps_checks_passed) {
/* EVENT
* @group ekf2
* @description
* The GNSS checks enabled by <param>EKF2_GPS_CHECK</param> have passed.
*/
events::send<int32_t>(events::ID("ekf2_gnss_checks_passed"), events::Log::Debug,
"EKF2({1}): GNSS checks passed", _instance);
}

if (_ekf.information_event_flags().reset_vel_to_gps) {
/* EVENT
* @group ekf2
*/
events::send<int32_t>(events::ID("ekf2_reset_vel_to_gnss"), events::Log::Debug,
"EKF2({1}): Reset velocity to GNSS", _instance);
}

if (_ekf.information_event_flags().reset_vel_to_flow) {
/* EVENT
* @group ekf2
*/
events::send<int32_t>(events::ID("ekf2_reset_vel_to_flow"), events::Log::Debug,
"EKF2({1}): Reset velocity to optical flow", _instance);
}

if (_ekf.information_event_flags().reset_vel_to_vision) {
/* EVENT
* @group ekf2
*/
events::send<int32_t>(events::ID("ekf2_reset_vel_to_vision"), events::Log::Debug,
"EKF2({1}): Reset velocity vision", _instance);
}

if (_ekf.information_event_flags().reset_vel_to_zero) {
/* EVENT
* @group ekf2
*/
events::send<int32_t>(events::ID("ekf2_reset_vel_to_zero"), events::Log::Debug,
"EKF2({1}): Reset velocity zero", _instance);
}

if (_ekf.information_event_flags().reset_pos_to_last_known) {
/* EVENT
* @group ekf2
*/
events::send<int32_t>(events::ID("ekf2_reset_pos_to_last_known"), events::Log::Debug,
"EKF2({1}): Reset position to last known", _instance);
}

if (_ekf.information_event_flags().reset_pos_to_gps) {
/* EVENT
* @group ekf2
*/
events::send<int32_t>(events::ID("ekf2_reset_pos_to_gnss"), events::Log::Debug,
"EKF2({1}): Reset position to GNSS", _instance);
}

if (_ekf.information_event_flags().reset_pos_to_vision) {
/* EVENT
* @group ekf2
*/
events::send<int32_t>(events::ID("ekf2_reset_pos_to_vision"), events::Log::Debug,
"EKF2({1}): Reset position to vision", _instance);
}

if (_ekf.information_event_flags().starting_gps_fusion) {
/* EVENT
* @group ekf2
*/
events::send<int32_t>(events::ID("ekf2_starting_gnss_fusion"), events::Log::Debug,
"EKF2({1}): Starting GNSS fusion", _instance);
}

if (_ekf.information_event_flags().starting_vision_pos_fusion) {
/* EVENT
* @group ekf2
*/
events::send<int32_t>(events::ID("ekf2_starting_vision_pos_fusion"), events::Log::Debug,
"EKF2({1}): Starting vision position fusion", _instance);
}

if (_ekf.information_event_flags().starting_vision_vel_fusion) {
/* EVENT
* @group ekf2
*/
events::send<int32_t>(events::ID("ekf2_starting_vision_vel_fusion"), events::Log::Debug,
"EKF2({1}): Starting vision velocity fusion", _instance);
}

if (_ekf.information_event_flags().starting_vision_yaw_fusion) {
/* EVENT
* @group ekf2
*/
events::send<int32_t>(events::ID("ekf2_starting_vision_yaw_fusion"), events::Log::Debug,
"EKF2({1}): Starting vision yaw fusion", _instance);
}

if (_ekf.information_event_flags().yaw_aligned_to_imu_gps) {
/* EVENT
* @group ekf2
*/
events::send<int32_t>(events::ID("ekf2_yaw_aligned_to_imu_gnss"), events::Log::Debug,
"EKF2({1}): Yaw aligned using IMU and GNSS", _instance);
}

if (_ekf.information_event_flags().reset_hgt_to_baro) {
/* EVENT
* @group ekf2
*/
events::send<int32_t>(events::ID("ekf2_reset_hgt_to_baro"), events::Log::Debug,
"EKF2({1}): Reset height to baro", _instance);
}

if (_ekf.information_event_flags().reset_hgt_to_gps) {
/* EVENT
* @group ekf2
*/
events::send<int32_t>(events::ID("ekf2_reset_hgt_to_gnss"), events::Log::Debug,
"EKF2({1}): Reset height to GNSS", _instance);
}

if (_ekf.information_event_flags().reset_hgt_to_rng) {
/* EVENT
* @group ekf2
*/
events::send<int32_t>(events::ID("ekf2_reset_hgt_to_rng"), events::Log::Debug,
"EKF2({1}): Reset height to range finder", _instance);
}

if (_ekf.information_event_flags().reset_hgt_to_ev) {
/* EVENT
* @group ekf2
*/
events::send<int32_t>(events::ID("ekf2_reset_hgt_to_ev"), events::Log::Debug,
"EKF2({1}): Reset height to vision", _instance);
}

// Warning events
if (_ekf.warning_event_flags().gps_quality_poor) {
/* EVENT
* @group ekf2
*/
events::send<int32_t>(events::ID("ekf2_gnss_quality_poor"), {events::Log::Debug, events::LogInternal::Warning},
"EKF2({1}): Poor GNSS quality", _instance);
}

if (_ekf.warning_event_flags().gps_fusion_timout) {
/* EVENT
* @group ekf2
*/
events::send<int32_t>(events::ID("ekf2_gnss_fusion_timout"), {events::Log::Debug, events::LogInternal::Warning},
"EKF2({1}): GNSS fusion timeout", _instance);
}

if (_ekf.warning_event_flags().gps_data_stopped) {
/* EVENT
* @group ekf2
*/
events::send<int32_t>(events::ID("ekf2_gnss_data_stopped"), {events::Log::Debug, events::LogInternal::Warning},
"EKF2({1}): GNSS data stopped", _instance);
}

if (_ekf.warning_event_flags().gps_data_stopped_using_alternate) {
/* EVENT
* @group ekf2
*/
events::send<int32_t>(events::ID("ekf2_gnss_data_stopped_using_alternate"), {events::Log::Debug, events::LogInternal::Warning},
"EKF2({1}): GNSS data stopped, using alternate aiding source", _instance);
}

if (_ekf.warning_event_flags().height_sensor_timeout) {
/* EVENT
* @group ekf2
*/
events::send<int32_t>(events::ID("ekf2_height_sensor_timeout"), {events::Log::Debug, events::LogInternal::Warning},
"EKF2({1}): Height sensor timeout", _instance);
}

if (_ekf.warning_event_flags().stopping_mag_use) {
/* EVENT
* @group ekf2
*/
events::send<int32_t>(events::ID("ekf2_stopping_mag_use"), {events::Log::Debug, events::LogInternal::Warning},
"EKF2({1}): Stopping magnetometer use", _instance);
}

if (_ekf.warning_event_flags().invalid_accel_bias_cov_reset) {
/* EVENT
* @group ekf2
*/
events::send<int32_t>(events::ID("ekf2_invalid_accel_bias_cov_reset"), {events::Log::Debug, events::LogInternal::Warning},
"EKF2({1}): Invalid accelerometer bias, resetting covariance", _instance);
}

if (_ekf.warning_event_flags().bad_yaw_using_gps_course) {
/* EVENT
* @group ekf2
*/
events::send<int32_t>(events::ID("ekf2_bad_yaw_using_gnss_course"), {events::Log::Debug, events::LogInternal::Warning},
"EKF2({1}): Bad yaw, reset to GNSS COG", _instance);
}

if (_ekf.warning_event_flags().vision_data_stopped) {
/* EVENT
* @group ekf2
*/
events::send<int32_t>(events::ID("ekf2_vision_data_stopped"), {events::Log::Debug, events::LogInternal::Warning},
"EKF2({1}): Vision data stopped", _instance);
}

if (_ekf.warning_event_flags().emergency_yaw_reset_mag_stopped) {
/* EVENT
* @group ekf2
*/
events::send<int32_t>(events::ID("ekf2_emergency_yaw_reset_mag_stopped"), {events::Log::Debug, events::LogInternal::Warning},
"EKF2({1}): Emergency yaw reset, stopping magnetometer use", _instance);
}

if (_ekf.warning_event_flags().emergency_yaw_reset_gps_yaw_stopped) {
/* EVENT
* @group ekf2
*/
events::send<int32_t>(events::ID("ekf2_emergency_yaw_reset_gnss_yaw_stopped"), {events::Log::Debug, events::LogInternal::Warning},
"EKF2({1}): Emergency yaw reset, stopping GNSS yaw use", _instance);
}

_ekf.clear_information_events();
_ekf.clear_warning_events();

} else if ((_last_event_flags_publish != 0) && (timestamp >= _last_event_flags_publish + 1_s)) {
// continue publishing periodically
_estimator_event_flags_pub.get().timestamp = _replay_mode ? timestamp : hrt_absolute_time();
_estimator_event_flags_pub.update();
_last_event_flags_publish = _estimator_event_flags_pub.get().timestamp;
}
}

Expand Down
3 changes: 0 additions & 3 deletions src/modules/ekf2/EKF2.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -67,7 +67,6 @@
#include <uORB/topics/ekf2_timestamps.h>
#include <uORB/topics/estimator_bias.h>
#include <uORB/topics/estimator_bias3d.h>
#include <uORB/topics/estimator_event_flags.h>
#include <uORB/topics/estimator_innovations.h>
#include <uORB/topics/estimator_sensor_bias.h>
#include <uORB/topics/estimator_states.h>
Expand Down Expand Up @@ -421,7 +420,6 @@ class EKF2 final : public ModuleParams, public px4::ScheduledWorkItem

bool _callback_registered{false};

hrt_abstime _last_event_flags_publish{0};
hrt_abstime _last_status_flags_publish{0};

uint64_t _filter_control_status{0};
Expand All @@ -435,7 +433,6 @@ class EKF2 final : public ModuleParams, public px4::ScheduledWorkItem
uint32_t _filter_information_event_changes{0};

uORB::PublicationMulti<ekf2_timestamps_s> _ekf2_timestamps_pub{ORB_ID(ekf2_timestamps)};
uORB::PublicationMultiData<estimator_event_flags_s> _estimator_event_flags_pub{ORB_ID(estimator_event_flags)};
uORB::PublicationMulti<estimator_innovations_s> _estimator_innovation_test_ratios_pub{ORB_ID(estimator_innovation_test_ratios)};
uORB::PublicationMulti<estimator_innovations_s> _estimator_innovation_variances_pub{ORB_ID(estimator_innovation_variances)};
uORB::PublicationMulti<estimator_innovations_s> _estimator_innovations_pub{ORB_ID(estimator_innovations)};
Expand Down
Loading

0 comments on commit 80cefa3

Please sign in to comment.