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

init adjustments to ensure used topics are advertised early (primarily for logging)

Browse Source 
- multi-EKF create each instance as soon as IMU & mag are advertised (before device id populated)
This commit is contained in:
Daniel Agar
2022-03-23 16:51:16 -04:00
parent e6ed595651
commit f4c3084c26
22 changed files with 171 additions and 127 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/CMakeLists.txt
+2
View File
@@ -42,6 +42,8 @@ px4_add_module(
#-DDEBUG_BUILD
INCLUDES
EKF
PRIORITY
"SCHED_PRIORITY_MAX - 18" # max priority below high priority WQ threads
STACK_MAX
3600
SRCS
src/modules/ekf2/EKF2.cpp
+44 -30
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2015-2021 PX4 Development Team. All rights reserved.
* Copyright (c) 2015-2022 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -164,6 +164,18 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
_param_ekf2_synthetic_mag_z(_params->synthesize_mag_z),
_param_ekf2_gsf_tas_default(_params->EKFGSF_tas_default)
{
// advertise expected minimal topic set immediately to ensure logging
_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();
_estimator_sensor_bias_pub.advertise();
_estimator_states_pub.advertise();
_estimator_status_flags_pub.advertise();
_estimator_status_pub.advertise();
}
EKF2::~EKF2()
@@ -183,13 +195,7 @@ EKF2::~EKF2()
bool EKF2::multi_init(int imu, int mag)
{
// advertise immediately to ensure consistent uORB instance numbering
_attitude_pub.advertise();
_local_position_pub.advertise();
_global_position_pub.advertise();
_odometry_pub.advertise();
_wind_pub.advertise();
// advertise all topics to ensure consistent uORB instance numbering
_ekf2_timestamps_pub.advertise();
_estimator_baro_bias_pub.advertise();
_estimator_event_flags_pub.advertise();
@@ -205,6 +211,12 @@ bool EKF2::multi_init(int imu, int mag)
_estimator_visual_odometry_aligned_pub.advertise();
_yaw_est_pub.advertise();
_attitude_pub.advertise();
_local_position_pub.advertise();
_global_position_pub.advertise();
_odometry_pub.advertise();
_wind_pub.advertise();
bool changed_instance = _vehicle_imu_sub.ChangeInstance(imu) && _magnetometer_sub.ChangeInstance(mag);
const int status_instance = _estimator_states_pub.get_instance();
@@ -320,8 +332,7 @@ void EKF2::Run()
}
if (!_callback_registered) {
PX4_WARN("%d - failed to register callback, retrying", _instance);
ScheduleDelayed(1_s);
ScheduleDelayed(10_ms);
return;
}
}
@@ -593,6 +604,9 @@ void EKF2::Run()
// publish ekf2_timestamps
_ekf2_timestamps_pub.publish(ekf2_timestamps);
}
// re-schedule as backup timeout
ScheduleDelayed(100_ms);
}
void EKF2::PublishAttitude(const hrt_abstime &timestamp)
@@ -692,11 +706,19 @@ void EKF2::PublishEventFlags(const hrt_abstime &timestamp)
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.publish(event_flags);
}
_estimator_event_flags_pub.update(event_flags);
_ekf.clear_information_events();
_ekf.clear_warning_events();
_last_event_flags_publish = event_flags.timestamp;
_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;
}
}
void EKF2::PublishGlobalPosition(const hrt_abstime &timestamp)
@@ -1198,7 +1220,7 @@ void EKF2::PublishStatus(const hrt_abstime &timestamp)
void EKF2::PublishStatusFlags(const hrt_abstime &timestamp)
{
// publish at ~ 1 Hz (or immediately if filter control status or fault status changes)
bool update = (hrt_elapsed_time(&_last_status_flag_update) >= 1_s);
bool update = (timestamp >= _last_status_flags_publish + 1_s);
// filter control status
if (_ekf.control_status().value != _filter_control_status) {
@@ -1296,7 +1318,7 @@ void EKF2::PublishStatusFlags(const hrt_abstime &timestamp)
status_flags.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
_estimator_status_flags_pub.publish(status_flags);
_last_status_flag_update = status_flags.timestamp;
_last_status_flags_publish = status_flags.timestamp;
}
}
@@ -2027,9 +2049,7 @@ int EKF2::task_spawn(int argc, char *argv[])
vehicle_mag_sub.update();
// Mag & IMU data must be valid, first mag can be ignored initially
if ((vehicle_mag_sub.get().device_id != 0 || mag == 0)
&& (vehicle_imu_sub.get().accel_device_id != 0)
&& (vehicle_imu_sub.get().gyro_device_id != 0)) {
if ((vehicle_mag_sub.advertised() || mag == 0) && (vehicle_imu_sub.advertised())) {
if (!ekf2_instance_created[imu][mag]) {
EKF2 *ekf2_inst = new EKF2(true, px4::ins_instance_to_wq(imu), false);
@@ -2043,17 +2063,11 @@ int EKF2::task_spawn(int argc, char *argv[])
multi_instances_allocated++;
ekf2_instance_created[imu][mag] = true;
if (actual_instance == 0) {
// force selector to run immediately if first instance started
_ekf2_selector.load()->ScheduleNow();
}
PX4_DEBUG("starting instance %d, IMU:%" PRIu8 " (%" PRIu32 "), MAG:%" PRIu8 " (%" PRIu32 ")", actual_instance,
imu, vehicle_imu_sub.get().accel_device_id,
mag, vehicle_mag_sub.get().device_id);
// sleep briefly before starting more instances
px4_usleep(10000);
_ekf2_selector.load()->ScheduleNow();
} else {
PX4_ERR("instance numbering problem instance: %d", actual_instance);
@@ -2063,20 +2077,20 @@ int EKF2::task_spawn(int argc, char *argv[])
} else {
PX4_ERR("alloc and init failed imu: %" PRIu8 " mag:%" PRIu8, imu, mag);
px4_usleep(1000000);
px4_usleep(100000);
break;
}
}
} else {
px4_usleep(50000); // give the sensors extra time to start
continue;
px4_usleep(1000); // give the sensors extra time to start
break;
}
}
}
if (multi_instances_allocated < multi_instances) {
px4_usleep(100000);
px4_usleep(10000);
}
}
src/modules/ekf2/EKF2.hpp
+4 -2
View File
@@ -267,7 +267,9 @@ private:
bool _callback_registered{false};
hrt_abstime _last_status_flag_update{0};
hrt_abstime _last_event_flags_publish{0};
hrt_abstime _last_status_flags_publish{0};
hrt_abstime _last_range_sensor_update{0};
uint32_t _filter_control_status{0};
@@ -282,7 +284,7 @@ private:
uORB::PublicationMulti<ekf2_timestamps_s> _ekf2_timestamps_pub{ORB_ID(ekf2_timestamps)};
uORB::PublicationMulti<estimator_baro_bias_s> _estimator_baro_bias_pub{ORB_ID(estimator_baro_bias)};
uORB::PublicationMulti<estimator_event_flags_s> _estimator_event_flags_pub{ORB_ID(estimator_event_flags)};
uORB::PublicationMultiData<estimator_event_flags_s> _estimator_event_flags_pub{ORB_ID(estimator_event_flags)};
uORB::PublicationMulti<estimator_gps_status_s> _estimator_gps_status_pub{ORB_ID(estimator_gps_status)};
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)};
src/modules/ekf2/EKF2Selector.cpp
+12 -10
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
* Copyright (c) 2020-2022 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -43,6 +43,13 @@ EKF2Selector::EKF2Selector() :
ModuleParams(nullptr),
ScheduledWorkItem("ekf2_selector", px4::wq_configurations::nav_and_controllers)
{
_estimator_selector_status_pub.advertise();
_sensor_selection_pub.advertise();
_vehicle_attitude_pub.advertise();
_vehicle_global_position_pub.advertise();
_vehicle_local_position_pub.advertise();
_vehicle_odometry_pub.advertise();
_wind_pub.advertise();
}
EKF2Selector::~EKF2Selector()
@@ -50,12 +57,6 @@ EKF2Selector::~EKF2Selector()
Stop();
}
bool EKF2Selector::Start()
{
ScheduleNow();
return true;
}
void EKF2Selector::Stop()
{
for (int i = 0; i < EKF2_MAX_INSTANCES; i++) {
@@ -673,9 +674,6 @@ void EKF2Selector::PublishWindEstimate()
void EKF2Selector::Run()
{
// re-schedule as backup timeout
ScheduleDelayed(FILTER_UPDATE_PERIOD);
// check for parameter updates
if (_parameter_update_sub.updated()) {
// clear update
@@ -703,6 +701,7 @@ void EKF2Selector::Run()
// if still invalid return early and check again on next scheduled run
if (_selected_instance == INVALID_INSTANCE) {
ScheduleDelayed(100_ms);
return;
}
}
@@ -803,6 +802,9 @@ void EKF2Selector::Run()
PublishVehicleGlobalPosition();
PublishVehicleOdometry();
PublishWindEstimate();
// re-schedule as backup timeout
ScheduleDelayed(FILTER_UPDATE_PERIOD);
}
void EKF2Selector::PublishEstimatorSelectorStatus()
src/modules/ekf2/EKF2Selector.hpp
-1
View File
@@ -70,7 +70,6 @@ public:
EKF2Selector();
~EKF2Selector() override;
bool Start();
void Stop();
void PrintStatus();