Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-01 19:07:45 +08:00
Code Issues Actions 5 Packages Projects Releases Wiki Activity

ekf2: use single uORB::Subscription, but change instance

Browse Source
This commit is contained in:
Daniel Agar
2020-09-01 13:54:40 -04:00
parent 59e66e5be9
commit 4879a4e2ef
1 changed files with 6 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/ekf2_main.cpp
+6 -13
View File
@@ -247,11 +247,7 @@ private:
uORB::SubscriptionCallbackWorkItem _sensor_combined_sub{this, ORB_ID(sensor_combined)}; uORB::SubscriptionCallbackWorkItem _sensor_combined_sub{this, ORB_ID(sensor_combined)};
static constexpr int MAX_SENSOR_COUNT = 3; static constexpr int MAX_SENSOR_COUNT = 3;
uORB::SubscriptionCallbackWorkItem _vehicle_imu_subs[MAX_SENSOR_COUNT] { uORB::SubscriptionCallbackWorkItem _vehicle_imu_sub{this, ORB_ID(vehicle_imu)};
{this, ORB_ID(vehicle_imu), 0},
{this, ORB_ID(vehicle_imu), 1},
{this, ORB_ID(vehicle_imu), 2}
};
int _imu_sub_index{-1}; int _imu_sub_index{-1};
bool _callback_registered{false}; bool _callback_registered{false};
int _lockstep_component{-1}; int _lockstep_component{-1};
@@ -670,11 +666,11 @@ bool Ekf2::init()
// if EKF2_IMU_ID is non-zero we use the corresponding IMU, otherwise the voted primary (sensor_combined) // if EKF2_IMU_ID is non-zero we use the corresponding IMU, otherwise the voted primary (sensor_combined)
if (device_id != 0) { if (device_id != 0) {
for (int i = 0; i < MAX_SENSOR_COUNT; i++) { for (int i = 0; i < MAX_SENSOR_COUNT; i++) {
vehicle_imu_s imu{}; vehicle_imu_s imu;
if (_vehicle_imu_subs[i].copy(&imu)) { if (_vehicle_imu_sub.ChangeInstance(i) && _vehicle_imu_sub.copy(&imu)) {
if ((imu.accel_device_id > 0) && (imu.accel_device_id == device_id)) { if ((imu.accel_device_id > 0) && (imu.accel_device_id == device_id)) {
if (_vehicle_imu_subs[i].registerCallback()) { if (_vehicle_imu_sub.registerCallback()) {
PX4_INFO("subscribed to vehicle_imu:%d (%d)", i, device_id); PX4_INFO("subscribed to vehicle_imu:%d (%d)", i, device_id);
_imu_sub_index = i; _imu_sub_index = i;
_callback_registered = true; _callback_registered = true;
@@ -749,10 +745,7 @@ void Ekf2::Run()
{ {
if (should_exit()) { if (should_exit()) {
_sensor_combined_sub.unregisterCallback(); _sensor_combined_sub.unregisterCallback();
_vehicle_imu_sub.unregisterCallback();
for (auto &i : _vehicle_imu_subs) {
i.unregisterCallback();
}
exit_and_cleanup(); exit_and_cleanup();
return; return;
@@ -771,7 +764,7 @@ void Ekf2::Run()
if (_imu_sub_index >= 0) { if (_imu_sub_index >= 0) {
vehicle_imu_s imu; vehicle_imu_s imu;
updated = _vehicle_imu_subs[_imu_sub_index].update(&imu); updated = _vehicle_imu_sub.update(&imu);
imu_sample_new.time_us = imu.timestamp_sample; imu_sample_new.time_us = imu.timestamp_sample;
imu_sample_new.delta_ang_dt = imu.delta_angle_dt * 1.e-6f; imu_sample_new.delta_ang_dt = imu.delta_angle_dt * 1.e-6f;