fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

sensors: make sure to do an orb_copy, even if a gyro is disabled

Browse Source 
This makes sure that poll() never returns immediately.
accel & mag are only updated to keep the code in sync.
This commit is contained in:
Beat Küng 2018-07-18 07:31:12 +02:00 committed by Lorenz Meier
parent 28d70f5f79
commit 891a09f488
1 changed files with 15 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
src/modules/sensors/voted_sensors_update.cpp
View File

@ -544,7 +544,7 @@ void VotedSensorsUpdate::accel_poll(struct sensor_combined_s &raw)
bool accel_updated;
orb_check(_accel.subscription[uorb_index], &accel_updated);
if (accel_updated && _accel.enabled[uorb_index]) {
if (accel_updated) {
struct accel_report accel_report;
int ret = orb_copy(ORB_ID(sensor_accel), _accel.subscription[uorb_index], &accel_report);
@ -553,6 +553,10 @@ void VotedSensorsUpdate::accel_poll(struct sensor_combined_s &raw)
continue; //ignore invalid data
}
if (!_accel.enabled[uorb_index]) {
continue;
}
// First publication with data
if (_accel.priority[uorb_index] == 0) {
int32_t priority = 0;
@ -649,7 +653,7 @@ void VotedSensorsUpdate::gyro_poll(struct sensor_combined_s &raw)
bool gyro_updated;
orb_check(_gyro.subscription[uorb_index], &gyro_updated);
if (gyro_updated && _gyro.enabled[uorb_index]) {
if (gyro_updated) {
struct gyro_report gyro_report;
int ret = orb_copy(ORB_ID(sensor_gyro), _gyro.subscription[uorb_index], &gyro_report);
@ -658,6 +662,10 @@ void VotedSensorsUpdate::gyro_poll(struct sensor_combined_s &raw)
continue; //ignore invalid data
}
if (!_gyro.enabled[uorb_index]) {
continue;
}
// First publication with data
if (_gyro.priority[uorb_index] == 0) {
int32_t priority = 0;
@ -752,7 +760,7 @@ void VotedSensorsUpdate::mag_poll(vehicle_magnetometer_s &magnetometer)
bool mag_updated;
orb_check(_mag.subscription[uorb_index], &mag_updated);
if (mag_updated && _mag.enabled[uorb_index]) {
if (mag_updated) {
struct mag_report mag_report;
int ret = orb_copy(ORB_ID(sensor_mag), _mag.subscription[uorb_index], &mag_report);
@ -761,6 +769,10 @@ void VotedSensorsUpdate::mag_poll(vehicle_magnetometer_s &magnetometer)
continue; //ignore invalid data
}
if (!_mag.enabled[uorb_index]) {
continue;
}
// First publication with data
if (_mag.priority[uorb_index] == 0) {
int32_t priority = 0;