Fixed correct setting of field update flag

1 files changed, 48 insertions, 27 deletions

diff --git a/apps/sensors/sensors.cpp b/apps/sensors/sensors.cpp
index 77dc0aa31..217b05e9d 100644
--- a/apps/sensors/sensors.cpp
+++ b/apps/sensors/sensors.cpp
@@ -653,9 +653,16 @@ Sensors::accel_poll(struct sensor_combined_s &raw)
 		accel_report.x = (((accel_report.x_raw - _parameters.acc_offset[0]) * range_g) / 8192.0f) / 9.81f;
 		accel_report.y = (((accel_report.y_raw - _parameters.acc_offset[0]) * range_g) / 8192.0f) / 9.81f;
 		accel_report.z = (((accel_report.z_raw - _parameters.acc_offset[0]) * range_g) / 8192.0f) / 9.81f;
+		raw.accelerometer_counter++;
 
 	} else {
-		orb_copy(ORB_ID(sensor_accel), _accel_sub, &accel_report);
+		bool accel_updated;
+		orb_check(_accel_sub, &accel_updated);
+
+		if (accel_updated) {
+			orb_copy(ORB_ID(sensor_accel), _accel_sub, &accel_report);
+			raw.accelerometer_counter++;
+		}
 	}
 
 	raw.accelerometer_m_s2[0] = accel_report.x;
@@ -665,8 +672,6 @@ Sensors::accel_poll(struct sensor_combined_s &raw)
 	raw.accelerometer_raw[0] = accel_report.x_raw;
 	raw.accelerometer_raw[1] = accel_report.y_raw;
 	raw.accelerometer_raw[2] = accel_report.z_raw;
-
-	raw.accelerometer_counter++;
 }
 
 void
@@ -692,50 +697,66 @@ Sensors::gyro_poll(struct sensor_combined_s &raw)
 		gyro_report.z = (gyro_report.z_raw - _parameters.gyro_offset[2]) * 0.000266316109f;
 
 	} else {
-		orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &gyro_report);
 
-		raw.gyro_rad_s[0] = gyro_report.x;
-		raw.gyro_rad_s[1] = gyro_report.y;
-		raw.gyro_rad_s[2] = gyro_report.z;
+		bool gyro_updated;
+		orb_check(_gyro_sub, &gyro_updated);
 
-		raw.gyro_raw[0] = gyro_report.x_raw;
-		raw.gyro_raw[1] = gyro_report.y_raw;
-		raw.gyro_raw[2] = gyro_report.z_raw;
+		if (gyro_updated) {
+			orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &gyro_report);
 
-		raw.gyro_counter++;		
+			raw.gyro_rad_s[0] = gyro_report.x;
+			raw.gyro_rad_s[1] = gyro_report.y;
+			raw.gyro_rad_s[2] = gyro_report.z;
+
+			raw.gyro_raw[0] = gyro_report.x_raw;
+			raw.gyro_raw[1] = gyro_report.y_raw;
+			raw.gyro_raw[2] = gyro_report.z_raw;
+
+			raw.gyro_counter++;
+		}
 	}
 }
 
 void
 Sensors::mag_poll(struct sensor_combined_s &raw)
 {
-	struct mag_report	mag_report;
+	bool mag_updated;
+	orb_check(_mag_sub, &mag_updated);
 
-	orb_copy(ORB_ID(sensor_mag), _mag_sub, &mag_report);
+	if (mag_updated) {
+		struct mag_report	mag_report;
 
-	raw.magnetometer_ga[0] = mag_report.x;
-	raw.magnetometer_ga[1] = mag_report.y;
-	raw.magnetometer_ga[2] = mag_report.z;
+		orb_copy(ORB_ID(sensor_mag), _mag_sub, &mag_report);
 
-	raw.magnetometer_raw[0] = mag_report.x_raw;
-	raw.magnetometer_raw[1] = mag_report.y_raw;
-	raw.magnetometer_raw[2] = mag_report.z_raw;
-	
-	raw.magnetometer_counter++;
+		raw.magnetometer_ga[0] = mag_report.x;
+		raw.magnetometer_ga[1] = mag_report.y;
+		raw.magnetometer_ga[2] = mag_report.z;
+
+		raw.magnetometer_raw[0] = mag_report.x_raw;
+		raw.magnetometer_raw[1] = mag_report.y_raw;
+		raw.magnetometer_raw[2] = mag_report.z_raw;
+		
+		raw.magnetometer_counter++;
+	}
 }
 
 void
 Sensors::baro_poll(struct sensor_combined_s &raw)
 {
-	struct baro_report	baro_report;
+	bool baro_updated;
+	orb_check(_baro_sub, &baro_updated);
+
+	if (baro_updated) {
+		struct baro_report	baro_report;
 
-	orb_copy(ORB_ID(sensor_baro), _baro_sub, &baro_report);
+		orb_copy(ORB_ID(sensor_baro), _baro_sub, &baro_report);
 
-	raw.baro_pres_mbar = baro_report.pressure; // Pressure in mbar
-	raw.baro_alt_meter = baro_report.altitude; // Altitude in meters
-	raw.baro_temp_celcius = baro_report.temperature; // Temperature in degrees celcius
+		raw.baro_pres_mbar = baro_report.pressure; // Pressure in mbar
+		raw.baro_alt_meter = baro_report.altitude; // Altitude in meters
+		raw.baro_temp_celcius = baro_report.temperature; // Temperature in degrees celcius
 
-	raw.baro_counter++;
+		raw.baro_counter++;
+	}
 }
 
 void