Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-02 20:28:37 +08:00
Code Issues Actions 21 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
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/sensors/voted_sensors_update.cpp
+15 -3
View File
@@ -544,7 +544,7 @@ void VotedSensorsUpdate::accel_poll(struct sensor_combined_s &raw)
bool accel_updated; bool accel_updated;
orb_check(_accel.subscription[uorb_index], &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; struct accel_report accel_report;
int ret = orb_copy(ORB_ID(sensor_accel), _accel.subscription[uorb_index], &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 continue; //ignore invalid data
} }
if (!_accel.enabled[uorb_index]) {
continue;
}
// First publication with data // First publication with data
if (_accel.priority[uorb_index] == 0) { if (_accel.priority[uorb_index] == 0) {
int32_t priority = 0; int32_t priority = 0;
@@ -649,7 +653,7 @@ void VotedSensorsUpdate::gyro_poll(struct sensor_combined_s &raw)
bool gyro_updated; bool gyro_updated;
orb_check(_gyro.subscription[uorb_index], &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; struct gyro_report gyro_report;
int ret = orb_copy(ORB_ID(sensor_gyro), _gyro.subscription[uorb_index], &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 continue; //ignore invalid data
} }
if (!_gyro.enabled[uorb_index]) {
continue;
}
// First publication with data // First publication with data
if (_gyro.priority[uorb_index] == 0) { if (_gyro.priority[uorb_index] == 0) {
int32_t priority = 0; int32_t priority = 0;
@@ -752,7 +760,7 @@ void VotedSensorsUpdate::mag_poll(vehicle_magnetometer_s &magnetometer)
bool mag_updated; bool mag_updated;
orb_check(_mag.subscription[uorb_index], &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; struct mag_report mag_report;
int ret = orb_copy(ORB_ID(sensor_mag), _mag.subscription[uorb_index], &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 continue; //ignore invalid data
} }
if (!_mag.enabled[uorb_index]) {
continue;
}
// First publication with data // First publication with data
if (_mag.priority[uorb_index] == 0) { if (_mag.priority[uorb_index] == 0) {
int32_t priority = 0; int32_t priority = 0;