From 4dea79b2d676b9a009ba68e72c599b77ea8821d2 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Mon, 18 Feb 2019 16:10:46 +0100
Subject: [PATCH] sensors: prevent double orb_copy of gyro topic
By using the uORB::Subscription API we use a separate subscription
rather than `orb_copy` on the existing file descriptor used in
sensors through `px4_poll`.
This fixes a very peculiar problem that we observed in SITL in CI for
fixedwing. The events were as follows:
1. `sensors` does `px4_poll` on the gyro topic (as normal), and gets
the latest sample using `orb_copy`.
2. A parameter update happens when the mag is initialized and triggers
`VotedSensorsUpdate::parameters_update()` where `orb_copy` happens
before the main loop in `sensors` has started a `px4_poll`.
3. `sensors` now does the `px4_poll`, however waits indefinitely because
it has already copied the latest sample.
Also, the `px4_poll` will never time out because in lockstep the
simulator waits for the next actuator control message which it never
gets and therefore it never sends the next sensor message with a new
timestamp to advance the time.
This only happens for fixedwing because there is only one "uORB path"
through the system unlike for multicopter where a gyro sample can get
picked up by either `sensors` or directly `mc_att_control`, so the
system can survive if `sensors` has "drops".
---
src/modules/sensors/voted_sensors_update.cpp | 78 +++++++++-----------
1 file changed, 34 insertions(+), 44 deletions(-)
diff --git a/src/modules/sensors/voted_sensors_update.cpp b/src/modules/sensors/voted_sensors_update.cpp
index bc628bab86..597313ebba 100644
--- a/src/modules/sensors/voted_sensors_update.cpp
+++ b/src/modules/sensors/voted_sensors_update.cpp
@@ -41,6 +41,7 @@
#include <systemlib/mavlink_log.h>
+#include <uORB/Subscription.hpp>
#include <conversion/rotation.h>
#include <ecl/geo/geo.h>
@@ -151,79 +152,68 @@ void VotedSensorsUpdate::parameters_update()
_temperature_compensation.parameters_update(_hil_enabled);
/* gyro */
- for (int topic_instance = 0; topic_instance < GYRO_COUNT_MAX; ++topic_instance) {
+ for (int topic_instance = 0; topic_instance < _gyro.subscription_count; ++topic_instance) {
- if (topic_instance < _gyro.subscription_count) {
- // valid subscription, so get the driver id by getting the published sensor data
- sensor_gyro_s report;
+ uORB::Subscription<sensor_gyro_s> report{ORB_ID(sensor_gyro), 0, (unsigned)topic_instance};
- if (orb_copy(ORB_ID(sensor_gyro), _gyro.subscription[topic_instance], &report) == 0) {
- int temp = _temperature_compensation.set_sensor_id_gyro(report.device_id, topic_instance);
+ int temp = _temperature_compensation.set_sensor_id_gyro(report.get().device_id, topic_instance);
- if (temp < 0) {
- PX4_ERR("%s temp compensation init: failed to find device ID %u for instance %i",
- "gyro", report.device_id, topic_instance);
- _corrections.gyro_mapping[topic_instance] = 0;
+ if (temp < 0) {
+ PX4_ERR("%s temp compensation init: failed to find device ID %u for instance %i", "gyro", report.get().device_id,
+ topic_instance);
- } else {
- _corrections.gyro_mapping[topic_instance] = temp;
+ _corrections.gyro_mapping[topic_instance] = 0;
+
+ } else {
+ _corrections.gyro_mapping[topic_instance] = temp;
- }
- }
}
}
/* accel */
- for (int topic_instance = 0; topic_instance < ACCEL_COUNT_MAX; ++topic_instance) {
+ for (int topic_instance = 0; topic_instance < _accel.subscription_count; ++topic_instance) {
- if (topic_instance < _accel.subscription_count) {
- // valid subscription, so get the driver id by getting the published sensor data
- sensor_accel_s report;
+ uORB::Subscription<sensor_accel_s> report{ORB_ID(sensor_accel), 0, (unsigned)topic_instance};
- if (orb_copy(ORB_ID(sensor_accel), _accel.subscription[topic_instance], &report) == 0) {
- int temp = _temperature_compensation.set_sensor_id_accel(report.device_id, topic_instance);
+ int temp = _temperature_compensation.set_sensor_id_accel(report.get().device_id, topic_instance);
- if (temp < 0) {
- PX4_ERR("%s temp compensation init: failed to find device ID %u for instance %i",
- "accel", report.device_id, topic_instance);
- _corrections.accel_mapping[topic_instance] = 0;
+ if (temp < 0) {
+ PX4_ERR("%s temp compensation init: failed to find device ID %u for instance %i", "accel", report.get().device_id,
+ topic_instance);
- } else {
- _corrections.accel_mapping[topic_instance] = temp;
+ _corrections.accel_mapping[topic_instance] = 0;
+
+ } else {
+ _corrections.accel_mapping[topic_instance] = temp;
- }
- }
}
}
+
/* baro */
- for (int topic_instance = 0; topic_instance < BARO_COUNT_MAX; ++topic_instance) {
+ for (int topic_instance = 0; topic_instance < _baro.subscription_count; ++topic_instance) {
- if (topic_instance < _baro.subscription_count) {
- // valid subscription, so get the driver id by getting the published sensor data
- sensor_baro_s report;
+ uORB::Subscription<sensor_baro_s> report{ORB_ID(sensor_baro), 0, (unsigned)topic_instance};
- if (orb_copy(ORB_ID(sensor_baro), _baro.subscription[topic_instance], &report) == 0) {
- int temp = _temperature_compensation.set_sensor_id_baro(report.device_id, topic_instance);
+ int temp = _temperature_compensation.set_sensor_id_baro(report.get().device_id, topic_instance);
- if (temp < 0) {
- PX4_ERR("%s temp compensation init: failed to find device ID %u for instance %i",
- "baro", report.device_id, topic_instance);
- _corrections.baro_mapping[topic_instance] = 0;
+ if (temp < 0) {
+ PX4_ERR("%s temp compensation init: failed to find device ID %u for instance %i", "baro", report.get().device_id,
+ topic_instance);
- } else {
- _corrections.baro_mapping[topic_instance] = temp;
+ _corrections.baro_mapping[topic_instance] = 0;
+
+ } else {
+ _corrections.baro_mapping[topic_instance] = temp;
- }
- }
}
}
/* set offset parameters to new values */
- bool failed;
- char str[30];
+ bool failed = false;
+ char str[30] {};
unsigned gyro_count = 0;
unsigned accel_count = 0;
unsigned gyro_cal_found_count = 0;
--
GitLab