fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-29 15:30:34 +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-18 15:04:19 -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/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()