sensor_combined cleanup: remove many unneeded fields

Decreases the message size from 780 to 280 bytes.
In particular, all modules using sensor_combined must use the integral now.
The sensor value can easily be reconstructed by dividing with dt.

Voters now need to be moved into sensors module, because error count and
priority is removed from the topic.

Any module that requires additional data from a sensor can subscribe to
the raw sensor topics.

At two places, values are set to zero instead of subscribing to the raw
sensors (with the assumption that no one reads them):
- mavlink mavlink_highres_imu_t::abs_pressure
- sdlog2: sensor temperatures

msg/sensor_combined.msg

@@ -1,63 +1,22 @@
-# Definition of the sensor_combined uORB topic.
-
-int32 MAGNETOMETER_MODE_NORMAL = 0
-int32 MAGNETOMETER_MODE_POSITIVE_BIAS = 1
-int32 MAGNETOMETER_MODE_NEGATIVE_BIAS = 2
-
-uint32 SENSOR_PRIO_MIN = 0
-uint32 SENSOR_PRIO_VERY_LOW = 25
-uint32 SENSOR_PRIO_LOW = 50
-uint32 SENSOR_PRIO_DEFAULT = 75
-uint32 SENSOR_PRIO_HIGH = 100
-uint32 SENSOR_PRIO_VERY_HIGH = 125
-uint32 SENSOR_PRIO_MAX = 255
-
-# Sensor readings in raw and SI-unit form.
 #
-# These values are read from the sensors. Raw values are in sensor-specific units,
-# the scaled values are in SI-units, as visible from the ending of the variable
-# or the comments. The use of the SI fields is in general advised, as these fields
-# are scaled and offset-compensated where possible and do not change with board
-# revisions and sensor updates.
+# Sensor readings in SI-unit form.
 #
-# Actual data, this is specific to the type of data which is stored in this struct
-# A line containing L0GME will be added by the Python logging code generator to the logged dataset.
+# These fields are scaled and offset-compensated where possible and do not
+# change with board revisions and sensor updates.
 #
 
 uint64[3] gyro_timestamp			# Gyro timestamps
-int16[9] gyro_raw			# Raw sensor values of angular velocity
-float32[9] gyro_rad_s			# Angular velocity in radian per seconds
-uint32[3] gyro_priority			# Sensor priority
-float32[9] gyro_integral_rad		# delta angle in radians
+float32[9] gyro_integral_rad		# delta angle in the NED body frame in rad in the integration time frame
 uint64[3] gyro_integral_dt			# delta time for gyro integral in us
-uint32[3] gyro_errcount			# Error counter for gyro 0
-float32[3] gyro_temp			# Temperature of gyro 0
 
-int16[9] accelerometer_raw		# Raw acceleration in NED body frame
-float32[9] accelerometer_m_s2		# Acceleration in NED body frame, in m/s^2
 float32[9] accelerometer_integral_m_s		# velocity in NED body frame, in m/s^2
 uint64[3] accelerometer_integral_dt		# delta time for accel integral in us
-int16[3] accelerometer_mode			# Accelerometer measurement mode
-float32[3] accelerometer_range_m_s2		# Accelerometer measurement range in m/s^2
 uint64[3] accelerometer_timestamp	# Accelerometer timestamp
-uint32[3] accelerometer_priority	# Sensor priority
-uint32[3] accelerometer_errcount	# Error counter for accel 0
-float32[3] accelerometer_temp		# Temperature of accel 0
 
-int16[9] magnetometer_raw		# Raw magnetic field in NED body frame
 float32[9] magnetometer_ga		# Magnetic field in NED body frame, in Gauss
-int16[3] magnetometer_mode			# Magnetometer measurement mode
-float32[3] magnetometer_range_ga		# measurement range in Gauss
-float32[3] magnetometer_cuttoff_freq_hz	# Internal analog low pass frequency of sensor
 uint64[3] magnetometer_timestamp	# Magnetometer timestamp
-uint32[3] magnetometer_priority			# Sensor priority
-uint32[3] magnetometer_errcount		# Error counter for mag 0
-float32[3] magnetometer_temp		# Temperature of mag 0
 
-float32[3] baro_pres_mbar			# Barometric pressure, already temp. comp.
 float32[3] baro_alt_meter			# Altitude, already temp. comp.
 float32[3] baro_temp_celcius		# Temperature in degrees celsius
 uint64[3] baro_timestamp		# Barometer timestamp
-uint32[3] baro_priority			# Sensor priority
-uint32[3] baro_errcount			# Error count in communication
 

src/drivers/frsky_telemetry/frsky_data.c

@@ -204,12 +204,14 @@ void frsky_update_topics()
 void frsky_send_frame1(int uart)
 {
 	/* send formatted frame */
-	frsky_send_data(uart, FRSKY_ID_ACCEL_X,
-			roundf(sensor_combined->accelerometer_m_s2[0] * 1000.0f));
-	frsky_send_data(uart, FRSKY_ID_ACCEL_Y,
-			roundf(sensor_combined->accelerometer_m_s2[1] * 1000.0f));
-	frsky_send_data(uart, FRSKY_ID_ACCEL_Z,
-			roundf(sensor_combined->accelerometer_m_s2[2] * 1000.0f));
+	float acceleration[3];
+	float accel_dt = sensor_combined->accelerometer_integral_dt[0] / 1.e6f;
+	acceleration[0] = sensor_combined->accelerometer_integral_m_s[0] / accel_dt;
+	acceleration[1] = sensor_combined->accelerometer_integral_m_s[1] / accel_dt;
+	acceleration[2] = sensor_combined->accelerometer_integral_m_s[2] / accel_dt;
+	frsky_send_data(uart, FRSKY_ID_ACCEL_X, roundf(acceleration[0] * 1000.0f));
+	frsky_send_data(uart, FRSKY_ID_ACCEL_Y, roundf(acceleration[1] * 1000.0f));
+	frsky_send_data(uart, FRSKY_ID_ACCEL_Z, roundf(acceleration[2] * 1000.0f));
 
 	frsky_send_data(uart, FRSKY_ID_BARO_ALT_BP,
 			sensor_combined->baro_alt_meter[0]);

src/examples/px4_simple_app/px4_simple_app.c

@@ -101,15 +101,18 @@ int px4_simple_app_main(int argc, char *argv[])
 				struct sensor_combined_s raw;
 				/* copy sensors raw data into local buffer */
 				orb_copy(ORB_ID(sensor_combined), sensor_sub_fd, &raw);
+				float sensor_accel[3];
+				float accel_dt = raw.accelerometer_integral_dt[0] / 1.e6f;
+				sensor_accel[0] = raw.accelerometer_integral_m_s[0] / accel_dt;
+				sensor_accel[1] = raw.accelerometer_integral_m_s[1] / accel_dt;
+				sensor_accel[2] = raw.accelerometer_integral_m_s[2] / accel_dt;
 				PX4_WARN("[px4_simple_app] Accelerometer:\t%8.4f\t%8.4f\t%8.4f",
-					 (double)raw.accelerometer_m_s2[0],
-					 (double)raw.accelerometer_m_s2[1],
-					 (double)raw.accelerometer_m_s2[2]);
+					 (double)sensor_accel[0], (double)sensor_accel[1], (double)sensor_accel[2]);
 
 				/* set att and publish this information for other apps */
-				att.roll = raw.accelerometer_m_s2[0];
-				att.pitch = raw.accelerometer_m_s2[1];
-				att.yaw = raw.accelerometer_m_s2[2];
+				att.roll = sensor_accel[0];
+				att.pitch = sensor_accel[1];
+				att.yaw = sensor_accel[2];
 				orb_publish(ORB_ID(vehicle_attitude), att_pub, &att);
 			}
 

src/lib/terrain_estimation/terrain_estimator.cpp

@@ -67,7 +67,11 @@ void TerrainEstimator::predict(float dt, const struct vehicle_attitude_s *attitu
 {
 	if (attitude->R_valid) {
 		matrix::Matrix<float, 3, 3> R_att(attitude->R);
-		matrix::Vector<float, 3> a(&sensor->accelerometer_m_s2[0]);
+		matrix::Vector<float, 3> a;
+		float accel_dt = sensor->accelerometer_integral_dt[0] / 1.e6f;
+		a(0) = sensor->accelerometer_integral_m_s[0] / accel_dt;
+		a(1) = sensor->accelerometer_integral_m_s[1] / accel_dt;
+		a(2) = sensor->accelerometer_integral_m_s[2] / accel_dt;
 		matrix::Vector<float, 3> u;
 		u = R_att * a;
 		_u_z = u(2) + 9.81f; // compensate for gravity

src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp

@@ -395,13 +395,19 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
 					orb_copy(ORB_ID(vehicle_global_position), sub_global_pos, &global_pos);
 				}
 
+				float gyro_rad_s[3];
+				float gyro_dt = raw.gyro_integral_dt[0] / 1.e6f;
+				gyro_rad_s[0] = raw.gyro_integral_rad[0] / gyro_dt;
+				gyro_rad_s[1] = raw.gyro_integral_rad[1] / gyro_dt;
+				gyro_rad_s[2] = raw.gyro_integral_rad[2] / gyro_dt;
+
 				if (!initialized) {
 					// XXX disabling init for now
 					initialized = true;
 
-					// gyro_offsets[0] += raw.gyro_rad_s[0];
-					// gyro_offsets[1] += raw.gyro_rad_s[1];
-					// gyro_offsets[2] += raw.gyro_rad_s[2];
+					// gyro_offsets[0] += gyro_rad_s[0];
+					// gyro_offsets[1] += gyro_rad_s[1];
+					// gyro_offsets[2] += gyro_rad_s[2];
 					// offset_count++;
 
 					// if (hrt_absolute_time() - start_time > 3000000LL) {
@@ -427,9 +433,9 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
 						sensor_last_timestamp[0] = raw.gyro_timestamp[0];
 					}
 
-					z_k[0] =  raw.gyro_rad_s[0] - gyro_offsets[0];
-					z_k[1] =  raw.gyro_rad_s[1] - gyro_offsets[1];
-					z_k[2] =  raw.gyro_rad_s[2] - gyro_offsets[2];
+					z_k[0] =  gyro_rad_s[0] - gyro_offsets[0];
+					z_k[1] =  gyro_rad_s[1] - gyro_offsets[1];
+					z_k[2] =  gyro_rad_s[2] - gyro_offsets[2];
 
 					/* update accelerometer measurements */
 					if (sensor_last_timestamp[1] != raw.accelerometer_timestamp[0]) {
@@ -470,9 +476,15 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
 						last_vel_t = 0;
 					}
 
-					z_k[3] = raw.accelerometer_m_s2[0] - acc(0);
-					z_k[4] = raw.accelerometer_m_s2[1] - acc(1);
-					z_k[5] = raw.accelerometer_m_s2[2] - acc(2);
+					matrix::Vector3f raw_accel;
+					float accel_dt = raw.accelerometer_integral_dt[0] / 1.e6f;
+					raw_accel(0) = raw.accelerometer_integral_m_s[0] / accel_dt;
+					raw_accel(1) = raw.accelerometer_integral_m_s[1] / accel_dt;
+					raw_accel(2) = raw.accelerometer_integral_m_s[2] / accel_dt;
+
+					z_k[3] = raw_accel(0) - acc(0);
+					z_k[4] = raw_accel(1) - acc(1);
+					z_k[5] = raw_accel(2) - acc(2);
 
 					/* update magnetometer measurements */
 					if (sensor_last_timestamp[2] != raw.magnetometer_timestamp[0] &&
@@ -616,9 +628,9 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
 					att.pitchacc = x_aposteriori[4];
 					att.yawacc = x_aposteriori[5];
 
-					att.g_comp[0] = raw.accelerometer_m_s2[0] - acc(0);
-					att.g_comp[1] = raw.accelerometer_m_s2[1] - acc(1);
-					att.g_comp[2] = raw.accelerometer_m_s2[2] - acc(2);
+					att.g_comp[0] = raw_accel(0) - acc(0);
+					att.g_comp[1] = raw_accel(1) - acc(1);
+					att.g_comp[2] = raw_accel(2) - acc(2);
 
 					/* copy offsets */
 					memcpy(&att.rate_offsets, &(x_aposteriori[3]), sizeof(att.rate_offsets));

src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp

@@ -359,30 +359,27 @@ void AttitudeEstimatorQ::task_main()
 				if (sensors.gyro_timestamp[i] > 0) {
 
 					float gyro[3];
-
-					for (unsigned j = 0; j < 3; j++) {
-						if (sensors.gyro_integral_dt[i] > 0) {
-							gyro[j] = (double)sensors.gyro_integral_rad[i * 3 + j] / (sensors.gyro_integral_dt[i] / 1e6);
-
-						} else {
-							/* fall back to angular rate */
-							gyro[j] = sensors.gyro_rad_s[i * 3 + j];
-						}
-					}
-
-					_voter_gyro.put(i, sensors.gyro_timestamp[i], &gyro[0], sensors.gyro_errcount[i], sensors.gyro_priority[i]);
+					float gyro_dt = sensors.gyro_integral_dt[i] / 1e6;
+					gyro[0] = sensors.gyro_integral_rad[i * 3 + 0] / gyro_dt;
+					gyro[1] = sensors.gyro_integral_rad[i * 3 + 1] / gyro_dt;
+					gyro[2] = sensors.gyro_integral_rad[i * 3 + 2] / gyro_dt;
+
+					//TODO: note: voter will be moved into sensors module
+					//_voter_gyro.put(i, sensors.gyro_timestamp[i], &gyro[0], sensors.gyro_errcount[i], sensors.gyro_priority[i]);
+					_voter_gyro.put(i, sensors.gyro_timestamp[i], &gyro[0], 0, 75);
 				}
 
-				/* ignore empty fields */
 				if (sensors.accelerometer_timestamp[i] > 0) {
-					_voter_accel.put(i, sensors.accelerometer_timestamp[i], &sensors.accelerometer_m_s2[i * 3],
-							 sensors.accelerometer_errcount[i], sensors.accelerometer_priority[i]);
+					float acceleration[3];
+					float accel_dt = sensors.accelerometer_integral_dt[i] / 1.e6f;
+					acceleration[0] = sensors.accelerometer_integral_m_s[i * 3 + 0] / accel_dt;
+					acceleration[1] = sensors.accelerometer_integral_m_s[i * 3 + 1] / accel_dt;
+					acceleration[2] = sensors.accelerometer_integral_m_s[i * 3 + 2] / accel_dt;
+					_voter_accel.put(i, sensors.accelerometer_timestamp[i], acceleration, 0, 75);
 				}
 
-				/* ignore empty fields */
 				if (sensors.magnetometer_timestamp[i] > 0) {
-					_voter_mag.put(i, sensors.magnetometer_timestamp[i], &sensors.magnetometer_ga[i * 3],
-						       sensors.magnetometer_errcount[i], sensors.magnetometer_priority[i]);
+					_voter_mag.put(i, sensors.magnetometer_timestamp[i], &sensors.magnetometer_ga[i * 3], 0, 75);
 				}
 			}
 

src/modules/commander/calibration_routines.cpp

@@ -273,18 +273,7 @@ enum detect_orientation_return detect_orientation(orb_advert_t *mavlink_log_pub,
 
 			for (unsigned i = 0; i < ndim; i++) {
 
-				float di = 0.0f;
-				switch (i) {
-					case 0:
-						di = sensor.accelerometer_m_s2[0];
-						break;
-					case 1:
-						di = sensor.accelerometer_m_s2[1];
-						break;
-					case 2:
-						di = sensor.accelerometer_m_s2[2];
-						break;
-				}
+				float di = sensor.accelerometer_integral_m_s[i] / (sensor.accelerometer_integral_dt[0] / 1.e6f);
 
 				float d = di - accel_ema[i];
 				accel_ema[i] += d * w;

src/modules/ekf2/ekf2_main.cpp

@@ -614,10 +614,15 @@ void Ekf2::task_main()
 			control_state_s ctrl_state = {};
 			float gyro_bias[3] = {};
 			_ekf.get_gyro_bias(gyro_bias);
+			float gyro_rad_s[3];
+			float gyro_dt = sensors.gyro_integral_dt[0] / 1.e6f;
+			gyro_rad_s[0] = sensors.gyro_integral_rad[0] / gyro_dt - gyro_bias[0];
+			gyro_rad_s[1] = sensors.gyro_integral_rad[1] / gyro_dt - gyro_bias[1];
+			gyro_rad_s[2] = sensors.gyro_integral_rad[2] / gyro_dt - gyro_bias[2];
 			ctrl_state.timestamp = hrt_absolute_time();
-			ctrl_state.roll_rate = _lp_roll_rate.apply(sensors.gyro_rad_s[0]) - gyro_bias[0];
-			ctrl_state.pitch_rate = _lp_pitch_rate.apply(sensors.gyro_rad_s[1]) - gyro_bias[1];
-			ctrl_state.yaw_rate = _lp_yaw_rate.apply(sensors.gyro_rad_s[2]) - gyro_bias[2];
+			ctrl_state.roll_rate = _lp_roll_rate.apply(gyro_rad_s[0]);
+			ctrl_state.pitch_rate = _lp_pitch_rate.apply(gyro_rad_s[1]);
+			ctrl_state.yaw_rate = _lp_yaw_rate.apply(gyro_rad_s[2]);
 
 			// Velocity in body frame
 			float velocity[3];
@@ -644,7 +649,11 @@ void Ekf2::task_main()
 			ctrl_state.q[3] = q(3);
 
 			// Acceleration data
-			matrix::Vector<float, 3> acceleration = {&sensors.accelerometer_m_s2[0]};
+			matrix::Vector<float, 3> acceleration;
+			float accel_dt = sensors.accelerometer_integral_dt[0] / 1.e6f;
+			acceleration(0) = sensors.accelerometer_integral_m_s[0] / accel_dt;
+			acceleration(1) = sensors.accelerometer_integral_m_s[1] / accel_dt;
+			acceleration(2) = sensors.accelerometer_integral_m_s[2] / accel_dt;
 
 			float accel_bias[3];
 			_ekf.get_accel_bias(accel_bias);
@@ -705,9 +714,9 @@ void Ekf2::task_main()
 			att.q[3] = q(3);
 			att.q_valid = true;
 
-			att.rollspeed = sensors.gyro_rad_s[0];
-			att.pitchspeed = sensors.gyro_rad_s[1];
-			att.yawspeed = sensors.gyro_rad_s[2];
+			att.rollspeed = gyro_rad_s[0];
+			att.pitchspeed = gyro_rad_s[1];
+			att.yawspeed = gyro_rad_s[2];
 
 			// publish vehicle attitude data
 			if (_att_pub == nullptr) {

src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp

@@ -1384,13 +1384,21 @@ void AttitudePositionEstimatorEKF::pollData()
 
 	// Feed validator with recent sensor data
 
+	//TODO: note, we will move voters into sensors module
 	for (unsigned i = 0; i < (sizeof(_sensor_combined.gyro_timestamp) / sizeof(_sensor_combined.gyro_timestamp[0])); i++) {
-		_voter_gyro.put(i, _sensor_combined.gyro_timestamp[i], &_sensor_combined.gyro_rad_s[i * 3],
-				_sensor_combined.gyro_errcount[i], _sensor_combined.gyro_priority[i]);
-		_voter_accel.put(i, _sensor_combined.accelerometer_timestamp[i], &_sensor_combined.accelerometer_m_s2[i * 3],
-				 _sensor_combined.accelerometer_errcount[i], _sensor_combined.accelerometer_priority[i]);
-		_voter_mag.put(i, _sensor_combined.magnetometer_timestamp[i], &_sensor_combined.magnetometer_ga[i * 3],
-			       _sensor_combined.magnetometer_errcount[i], _sensor_combined.magnetometer_priority[i]);
+		float gyro_rad_s[3];
+		float gyro_dt = _sensor_combined.gyro_integral_dt[i] / 1.e6f;
+		gyro_rad_s[0] = _sensor_combined.gyro_integral_rad[i * 3 + 0] / gyro_dt;
+		gyro_rad_s[1] = _sensor_combined.gyro_integral_rad[i * 3 + 1] / gyro_dt;
+		gyro_rad_s[2] = _sensor_combined.gyro_integral_rad[i * 3 + 2] / gyro_dt;
+		_voter_gyro.put(i, _sensor_combined.gyro_timestamp[i], gyro_rad_s, 0, 75);
+		float acceleration[3];
+		float accel_dt = _sensor_combined.accelerometer_integral_dt[i] / 1.e6f;
+		acceleration[0] = _sensor_combined.accelerometer_integral_m_s[i * 3 + 0] / accel_dt;
+		acceleration[1] = _sensor_combined.accelerometer_integral_m_s[i * 3 + 1] / accel_dt;
+		acceleration[2] = _sensor_combined.accelerometer_integral_m_s[i * 3 + 2] / accel_dt;
+		_voter_accel.put(i, _sensor_combined.accelerometer_timestamp[i], acceleration, 0, 75);
+		_voter_mag.put(i, _sensor_combined.magnetometer_timestamp[i], &_sensor_combined.magnetometer_ga[i * 3], 0, 75);
 	}
 
 	// Get best measurement values
@@ -1399,28 +1407,13 @@ void AttitudePositionEstimatorEKF::pollData()
 
 	if (_gyro_main >= 0) {
 
-		// Use pre-integrated values if possible
-		if (_sensor_combined.gyro_integral_dt[_gyro_main] > 0) {
-			_ekf->dAngIMU.x = _sensor_combined.gyro_integral_rad[_gyro_main * 3 + 0];
-			_ekf->dAngIMU.y = _sensor_combined.gyro_integral_rad[_gyro_main * 3 + 1];
-			_ekf->dAngIMU.z = _sensor_combined.gyro_integral_rad[_gyro_main * 3 + 2];
+		_ekf->dAngIMU.x = _sensor_combined.gyro_integral_rad[_gyro_main * 3 + 0];
+		_ekf->dAngIMU.y = _sensor_combined.gyro_integral_rad[_gyro_main * 3 + 1];
+		_ekf->dAngIMU.z = _sensor_combined.gyro_integral_rad[_gyro_main * 3 + 2];
 
-		} else {
-			float dt_gyro = _sensor_combined.gyro_integral_dt[_gyro_main] / 1e6f;
-
-			if (PX4_ISFINITE(dt_gyro) && (dt_gyro < 0.5f) && (dt_gyro > 0.00001f)) {
-				deltaT = dt_gyro;
-				_ekf->dtIMU = deltaT;
-			}
-
-			_ekf->dAngIMU.x = 0.5f * (_ekf->angRate.x + _sensor_combined.gyro_rad_s[_gyro_main * 3 + 0]) * _ekf->dtIMU;
-			_ekf->dAngIMU.y = 0.5f * (_ekf->angRate.y + _sensor_combined.gyro_rad_s[_gyro_main * 3 + 1]) * _ekf->dtIMU;
-			_ekf->dAngIMU.z = 0.5f * (_ekf->angRate.z + _sensor_combined.gyro_rad_s[_gyro_main * 3 + 2]) * _ekf->dtIMU;
-		}
+		float gyro_dt = _sensor_combined.gyro_integral_dt[_gyro_main] / 1.e6f;
+		_ekf->angRate = _ekf->dAngIMU / gyro_dt;
 
-		_ekf->angRate.x = _sensor_combined.gyro_rad_s[_gyro_main * 3 + 0];
-		_ekf->angRate.y = _sensor_combined.gyro_rad_s[_gyro_main * 3 + 1];
-		_ekf->angRate.z = _sensor_combined.gyro_rad_s[_gyro_main * 3 + 2];
 		perf_count(_perf_gyro);
 	}
 
@@ -1428,21 +1421,13 @@ void AttitudePositionEstimatorEKF::pollData()
 
 	if (_accel_main >= 0 && (_last_accel != _sensor_combined.accelerometer_timestamp[_accel_main])) {
 
-		// Use pre-integrated values if possible
-		if (_sensor_combined.accelerometer_integral_dt[_accel_main] > 0) {
-			_ekf->dVelIMU.x = _sensor_combined.accelerometer_integral_m_s[_accel_main * 3 + 0];
-			_ekf->dVelIMU.y = _sensor_combined.accelerometer_integral_m_s[_accel_main * 3 + 1];
-			_ekf->dVelIMU.z = _sensor_combined.accelerometer_integral_m_s[_accel_main * 3 + 2];
+		_ekf->dVelIMU.x = _sensor_combined.accelerometer_integral_m_s[_accel_main * 3 + 0];
+		_ekf->dVelIMU.y = _sensor_combined.accelerometer_integral_m_s[_accel_main * 3 + 1];
+		_ekf->dVelIMU.z = _sensor_combined.accelerometer_integral_m_s[_accel_main * 3 + 2];
 
-		} else {
-			_ekf->dVelIMU.x = 0.5f * (_ekf->accel.x + _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 0]) * _ekf->dtIMU;
-			_ekf->dVelIMU.y = 0.5f * (_ekf->accel.y + _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 1]) * _ekf->dtIMU;
-			_ekf->dVelIMU.z = 0.5f * (_ekf->accel.z + _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 2]) * _ekf->dtIMU;
-		}
+		float accel_dt = _sensor_combined.accelerometer_integral_dt[_accel_main] / 1.e6f;
+		_ekf->accel = _ekf->dVelIMU / accel_dt;
 
-		_ekf->accel.x = _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 0];
-		_ekf->accel.y = _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 1];
-		_ekf->accel.z = _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 2];
 		_last_accel = _sensor_combined.accelerometer_timestamp[_accel_main];
 	}
 
@@ -1518,20 +1503,9 @@ void AttitudePositionEstimatorEKF::pollData()
 			 (double)(_sensor_combined.accelerometer_integral_m_s[1]),
 			 (double)(_sensor_combined.accelerometer_integral_m_s[2]));
 
-		PX4_WARN("DRV: dang: %8.4f %8.4f dvel: %8.4f %8.4f %8.4f",
-			 (double)(_sensor_combined.gyro_rad_s[0] * deltaT), (double)(_sensor_combined.gyro_rad_s[2] * deltaT),
-			 (double)(_sensor_combined.accelerometer_m_s2[0] * deltaT),
-			 (double)(_sensor_combined.accelerometer_m_s2[1] * deltaT),
-			 (double)(_sensor_combined.accelerometer_m_s2[2] * deltaT));
-
 		PX4_WARN("EKF rate: %8.4f, %8.4f, %8.4f",
 			 (double)_att.rollspeed, (double)_att.pitchspeed, (double)_att.yawspeed);
 
-		PX4_WARN("DRV rate: %8.4f, %8.4f, %8.4f",
-			 (double)_sensor_combined.gyro_rad_s[0],
-			 (double)_sensor_combined.gyro_rad_s[1],
-			 (double)_sensor_combined.gyro_rad_s[2]);
-
 		lastprint = hrt_absolute_time();
 	}
 

src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp

@@ -1230,7 +1230,11 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 	float eas2tas = 1.0f; // XXX calculate actual number based on current measurements
 
 	/* filter speed and altitude for controller */
-	math::Vector<3> accel_body(_sensor_combined.accelerometer_m_s2);
+	math::Vector<3> accel_body;
+	float accel_dt = _sensor_combined.accelerometer_integral_dt[0] / 1.e6f;
+	accel_body(0) = _sensor_combined.accelerometer_integral_m_s[0] / accel_dt;
+	accel_body(1) = _sensor_combined.accelerometer_integral_m_s[1] / accel_dt;
+	accel_body(2) = _sensor_combined.accelerometer_integral_m_s[2] / accel_dt;
 	math::Vector<3> accel_earth = _R_nb * accel_body;
 
 	/* tell TECS to update its state, but let it know when it cannot actually control the plane */
@@ -1703,7 +1707,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 					}
 
 					/* Detect launch */
-					launchDetector.update(_sensor_combined.accelerometer_m_s2[0]);
+					launchDetector.update(accel_body(0));
 
 					/* update our copy of the launch detection state */
 					launch_detection_state = launchDetector.getLaunchDetected();

src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp

@@ -844,7 +844,11 @@ void BlockLocalPositionEstimator::predict()
 
 	if (integrate && _sub_att.get().R_valid) {
 		Matrix3f R_att(_sub_att.get().R);
-		Vector3f a(_sub_sensor.get().accelerometer_m_s2);
+		Vector3f a;
+		float accel_dt = _sub_sensor.get().accelerometer_integral_dt[0] / 1.e6f;
+		a(0) = _sub_sensor.get().accelerometer_integral_m_s[0] / accel_dt;
+		a(1) = _sub_sensor.get().accelerometer_integral_m_s[1] / accel_dt;
+		a(2) = _sub_sensor.get().accelerometer_integral_m_s[2] / accel_dt;
 		_u = R_att * a;
 		_u(U_az) += 9.81f; // add g
 

src/modules/mavlink/mavlink_messages.cpp

@@ -752,16 +752,18 @@ protected:
 			mavlink_highres_imu_t msg;
 
 			msg.time_usec = sensor.timestamp;
-			msg.xacc = sensor.accelerometer_m_s2[0];
-			msg.yacc = sensor.accelerometer_m_s2[1];
-			msg.zacc = sensor.accelerometer_m_s2[2];
-			msg.xgyro = sensor.gyro_rad_s[0];
-			msg.ygyro = sensor.gyro_rad_s[1];
-			msg.zgyro = sensor.gyro_rad_s[2];
+			float accel_dt = sensor.accelerometer_integral_dt[0] / 1.e6f;
+			msg.xacc = sensor.accelerometer_integral_m_s[0] / accel_dt;
+			msg.yacc = sensor.accelerometer_integral_m_s[1] / accel_dt;
+			msg.zacc = sensor.accelerometer_integral_m_s[2] / accel_dt;
+			float gyro_dt = sensor.gyro_integral_dt[0] / 1.e6f;
+			msg.xgyro = sensor.gyro_integral_rad[0] / gyro_dt;
+			msg.ygyro = sensor.gyro_integral_rad[1] / gyro_dt;
+			msg.zgyro = sensor.gyro_integral_rad[2] / gyro_dt;
 			msg.xmag = sensor.magnetometer_ga[0];
 			msg.ymag = sensor.magnetometer_ga[1];
 			msg.zmag = sensor.magnetometer_ga[2];
-			msg.abs_pressure = sensor.baro_pres_mbar[0];
+			msg.abs_pressure = 0;
 			msg.diff_pressure = differential_pressure.differential_pressure_raw_pa;
 			msg.pressure_alt = sensor.baro_alt_meter[0];
 			msg.temperature = sensor.baro_temp_celcius[0];

src/modules/mavlink/mavlink_receiver.cpp

@@ -1675,49 +1675,29 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
 
 		hil_sensors.timestamp = timestamp;
 
-		hil_sensors.gyro_raw[0] = imu.xgyro * 1000.0f;
-		hil_sensors.gyro_raw[1] = imu.ygyro * 1000.0f;
-		hil_sensors.gyro_raw[2] = imu.zgyro * 1000.0f;
-		hil_sensors.gyro_rad_s[0] = imu.xgyro;
-		hil_sensors.gyro_rad_s[1] = imu.ygyro;
-		hil_sensors.gyro_rad_s[2] = imu.zgyro;
 		hil_sensors.gyro_integral_rad[0] = 0.5f * (imu.xgyro + _hil_prev_gyro[0]) * dt;
 		hil_sensors.gyro_integral_rad[1] = 0.5f * (imu.ygyro + _hil_prev_gyro[1]) * dt;
 		hil_sensors.gyro_integral_rad[2] = 0.5f * (imu.zgyro + _hil_prev_gyro[2]) * dt;
-		memcpy(&_hil_prev_gyro[0], &hil_sensors.gyro_rad_s[0], sizeof(_hil_prev_gyro));
+		_hil_prev_gyro[0] = imu.xgyro;
+		_hil_prev_gyro[1] = imu.ygyro;
+		_hil_prev_gyro[2] = imu.zgyro;
 		hil_sensors.gyro_integral_dt[0] = dt * 1e6f;
 		hil_sensors.gyro_timestamp[0] = timestamp;
-		hil_sensors.gyro_priority[0] = ORB_PRIO_HIGH;
-
-		hil_sensors.accelerometer_raw[0] = imu.xacc / mg2ms2;
-		hil_sensors.accelerometer_raw[1] = imu.yacc / mg2ms2;
-		hil_sensors.accelerometer_raw[2] = imu.zacc / mg2ms2;
-		hil_sensors.accelerometer_m_s2[0] = imu.xacc;
-		hil_sensors.accelerometer_m_s2[1] = imu.yacc;
-		hil_sensors.accelerometer_m_s2[2] = imu.zacc;
+
 		hil_sensors.accelerometer_integral_m_s[0] = 0.5f * (imu.xacc + _hil_prev_accel[0]) * dt;
 		hil_sensors.accelerometer_integral_m_s[1] = 0.5f * (imu.yacc + _hil_prev_accel[1]) * dt;
 		hil_sensors.accelerometer_integral_m_s[2] = 0.5f * (imu.zacc + _hil_prev_accel[2]) * dt;
-		memcpy(&_hil_prev_accel[0], &hil_sensors.accelerometer_m_s2[0], sizeof(_hil_prev_accel));
+		_hil_prev_accel[0] = imu.xacc;
+		_hil_prev_accel[1] = imu.yacc;
+		_hil_prev_accel[2] = imu.zacc;
 		hil_sensors.accelerometer_integral_dt[0] = dt * 1e6f;
-		hil_sensors.accelerometer_mode[0] = 0; // TODO what is this?
-		hil_sensors.accelerometer_range_m_s2[0] = 32.7f; // int16
 		hil_sensors.accelerometer_timestamp[0] = timestamp;
-		hil_sensors.accelerometer_priority[0] = ORB_PRIO_HIGH;
 
-		hil_sensors.magnetometer_raw[0] = imu.xmag * 1000.0f;
-		hil_sensors.magnetometer_raw[1] = imu.ymag * 1000.0f;
-		hil_sensors.magnetometer_raw[2] = imu.zmag * 1000.0f;
 		hil_sensors.magnetometer_ga[0] = imu.xmag;
 		hil_sensors.magnetometer_ga[1] = imu.ymag;
 		hil_sensors.magnetometer_ga[2] = imu.zmag;
-		hil_sensors.magnetometer_range_ga[0] = 32.7f; // int16
-		hil_sensors.magnetometer_mode[0] = 0; // TODO what is this
-		hil_sensors.magnetometer_cuttoff_freq_hz[0] = 50.0f;
 		hil_sensors.magnetometer_timestamp[0] = timestamp;
-		hil_sensors.magnetometer_priority[0] = ORB_PRIO_HIGH;
 
-		hil_sensors.baro_pres_mbar[0] = imu.abs_pressure;
 		hil_sensors.baro_alt_meter[0] = imu.pressure_alt;
 		hil_sensors.baro_temp_celcius[0] = imu.temperature;
 		hil_sensors.baro_timestamp[0] = timestamp;

src/modules/position_estimator_inav/position_estimator_inav_main.cpp

@@ -504,17 +504,18 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 
 				if (sensor.accelerometer_timestamp[0] != accel_timestamp) {
 					if (att.R_valid) {
-						/* correct accel bias */
-						sensor.accelerometer_m_s2[0] -= acc_bias[0];
-						sensor.accelerometer_m_s2[1] -= acc_bias[1];
-						sensor.accelerometer_m_s2[2] -= acc_bias[2];
+						float sensor_accel[3];
+						float accel_dt = sensor.accelerometer_integral_dt[0] / 1.e6f;
+						sensor_accel[0] = sensor.accelerometer_integral_m_s[0] / accel_dt - acc_bias[0];
+						sensor_accel[1] = sensor.accelerometer_integral_m_s[1] / accel_dt - acc_bias[1];
+						sensor_accel[2] = sensor.accelerometer_integral_m_s[2] / accel_dt - acc_bias[2];
 
 						/* transform acceleration vector from body frame to NED frame */
 						for (int i = 0; i < 3; i++) {
 							acc[i] = 0.0f;
 
 							for (int j = 0; j < 3; j++) {
-								acc[i] += PX4_R(att.R, i, j) * sensor.accelerometer_m_s2[j];
+								acc[i] += PX4_R(att.R, i, j) * sensor_accel[j];
 							}
 						}
 

src/modules/sdlog2/sdlog2.c

@@ -1671,18 +1671,20 @@ int sdlog2_thread_main(int argc, char *argv[])
 							break;
 					}
 
-					log_msg.body.log_IMU.gyro_x = buf.sensor.gyro_rad_s[i * 3 + 0];
-					log_msg.body.log_IMU.gyro_y = buf.sensor.gyro_rad_s[i * 3 + 1];
-					log_msg.body.log_IMU.gyro_z = buf.sensor.gyro_rad_s[i * 3 + 2];
-					log_msg.body.log_IMU.acc_x = buf.sensor.accelerometer_m_s2[i * 3 + 0];
-					log_msg.body.log_IMU.acc_y = buf.sensor.accelerometer_m_s2[i * 3 + 1];
-					log_msg.body.log_IMU.acc_z = buf.sensor.accelerometer_m_s2[i * 3 + 2];
+					float gyro_dt = buf.sensor.gyro_integral_dt[i] / 1.e6f;
+					log_msg.body.log_IMU.gyro_x = buf.sensor.gyro_integral_rad[i * 3 + 0] / gyro_dt;
+					log_msg.body.log_IMU.gyro_y = buf.sensor.gyro_integral_rad[i * 3 + 1] / gyro_dt;
+					log_msg.body.log_IMU.gyro_z = buf.sensor.gyro_integral_rad[i * 3 + 2] / gyro_dt;
+					float accel_dt = buf.sensor.accelerometer_integral_dt[i] / 1.e6f;
+					log_msg.body.log_IMU.acc_x = buf.sensor.accelerometer_integral_m_s[i * 3 + 0] / accel_dt;
+					log_msg.body.log_IMU.acc_y = buf.sensor.accelerometer_integral_m_s[i * 3 + 1] / accel_dt;
+					log_msg.body.log_IMU.acc_z = buf.sensor.accelerometer_integral_m_s[i * 3 + 2] / accel_dt;
 					log_msg.body.log_IMU.mag_x = buf.sensor.magnetometer_ga[i * 3 + 0];
 					log_msg.body.log_IMU.mag_y = buf.sensor.magnetometer_ga[i * 3 + 1];
 					log_msg.body.log_IMU.mag_z = buf.sensor.magnetometer_ga[i * 3 + 2];
-					log_msg.body.log_IMU.temp_gyro = buf.sensor.gyro_temp[i];
-					log_msg.body.log_IMU.temp_acc = buf.sensor.accelerometer_temp[i];
-					log_msg.body.log_IMU.temp_mag = buf.sensor.magnetometer_temp[i];
+					log_msg.body.log_IMU.temp_gyro = 0;
+					log_msg.body.log_IMU.temp_acc = 0;
+					log_msg.body.log_IMU.temp_mag = 0;
 					LOGBUFFER_WRITE_AND_COUNT(IMU);
 				}
 
@@ -1699,7 +1701,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 							break;
 					}
 
-					log_msg.body.log_SENS.baro_pres = buf.sensor.baro_pres_mbar[i];
+					log_msg.body.log_SENS.baro_pres = 0;
 					log_msg.body.log_SENS.baro_alt = buf.sensor.baro_alt_meter[i];
 					log_msg.body.log_SENS.baro_temp = buf.sensor.baro_temp_celcius[i];
 					log_msg.body.log_SENS.diff_pres = 0;

src/modules/sensors/sensors.cpp

@@ -252,6 +252,10 @@ private:
 
 	Battery		_battery;			/**< Helper lib to publish battery_status topic. */
 
+	float		_latest_baro_pressure[SENSOR_COUNT_MAX];
+	hrt_abstime	_last_accel_timestamp[SENSOR_COUNT_MAX];
+	hrt_abstime	_last_gyro_timestamp[SENSOR_COUNT_MAX];
+
 	struct {
 		float min[_rc_max_chan_count];
 		float trim[_rc_max_chan_count];
@@ -393,8 +397,7 @@ private:
 	}		_parameter_handles;		/**< handles for interesting parameters */
 
 
-	int		init_sensor_class(const struct orb_metadata *meta, int *subs,
-					  uint32_t *priorities, uint32_t *errcount);
+	int		init_sensor_class(const struct orb_metadata *meta, int *subs);
 
 	/**
 	 * Update our local parameter cache.
@@ -564,6 +567,9 @@ Sensors::Sensors() :
 	memset(&_diff_pres, 0, sizeof(_diff_pres));
 	memset(&_parameters, 0, sizeof(_parameters));
 	memset(&_rc_parameter_map, 0, sizeof(_rc_parameter_map));
+	memset(&_latest_baro_pressure, 0, sizeof(_latest_baro_pressure));
+	memset(&_last_accel_timestamp, 0, sizeof(_last_accel_timestamp));
+	memset(&_last_gyro_timestamp, 0, sizeof(_last_gyro_timestamp));
 
 	/* basic r/c parameters */
 	for (unsigned i = 0; i < _rc_max_chan_count; i++) {
@@ -1023,29 +1029,34 @@ Sensors::accel_poll(struct sensor_combined_s &raw)
 
 			orb_copy(ORB_ID(sensor_accel), _accel_sub[i], &accel_report);
 
-			math::Vector<3> vect(accel_report.x, accel_report.y, accel_report.z);
-			vect = _board_rotation * vect;
+			if (accel_report.integral_dt != 0) {
+				math::Vector<3> vect_int(accel_report.x_integral, accel_report.y_integral, accel_report.z_integral);
+				vect_int = _board_rotation * vect_int;
 
-			raw.accelerometer_m_s2[i * 3 + 0] = vect(0);
-			raw.accelerometer_m_s2[i * 3 + 1] = vect(1);
-			raw.accelerometer_m_s2[i * 3 + 2] = vect(2);
+				raw.accelerometer_integral_m_s[i * 3 + 0] = vect_int(0);
+				raw.accelerometer_integral_m_s[i * 3 + 1] = vect_int(1);
+				raw.accelerometer_integral_m_s[i * 3 + 2] = vect_int(2);
 
-			math::Vector<3> vect_int(accel_report.x_integral, accel_report.y_integral, accel_report.z_integral);
-			vect_int = _board_rotation * vect_int;
+				raw.accelerometer_integral_dt[i] = accel_report.integral_dt;
 
-			raw.accelerometer_integral_m_s[i * 3 + 0] = vect_int(0);
-			raw.accelerometer_integral_m_s[i * 3 + 1] = vect_int(1);
-			raw.accelerometer_integral_m_s[i * 3 + 2] = vect_int(2);
+			} else {
+				//this is undesirable: a driver did not set the integral, so we have to construct it ourselves
+				math::Vector<3> vect_val(accel_report.x, accel_report.y, accel_report.z);
+				vect_val = _board_rotation * vect_val;
 
-			raw.accelerometer_integral_dt[i] = accel_report.integral_dt;
+				if (_last_accel_timestamp[i] == 0) {
+					_last_accel_timestamp[i] = accel_report.timestamp - 1000;
+				}
 
-			raw.accelerometer_raw[i * 3 + 0] = accel_report.x_raw;
-			raw.accelerometer_raw[i * 3 + 1] = accel_report.y_raw;
-			raw.accelerometer_raw[i * 3 + 2] = accel_report.z_raw;
+				raw.accelerometer_integral_dt[i] = accel_report.timestamp - _last_accel_timestamp[i];
+				_last_accel_timestamp[i] = accel_report.timestamp;
+				float dt = raw.accelerometer_integral_dt[i] / 1.e6f;
+				raw.accelerometer_integral_m_s[i * 3 + 0] = vect_val(0) * dt;
+				raw.accelerometer_integral_m_s[i * 3 + 1] = vect_val(1) * dt;
+				raw.accelerometer_integral_m_s[i * 3 + 2] = vect_val(2) * dt;
+			}
 
 			raw.accelerometer_timestamp[i] = accel_report.timestamp;
-			raw.accelerometer_errcount[i] = accel_report.error_count;
-			raw.accelerometer_temp[i] = accel_report.temperature;
 		}
 	}
 }
@@ -1062,34 +1073,38 @@ Sensors::gyro_poll(struct sensor_combined_s &raw)
 
 			orb_copy(ORB_ID(sensor_gyro), _gyro_sub[i], &gyro_report);
 
-			math::Vector<3> vect(gyro_report.x, gyro_report.y, gyro_report.z);
-			vect = _board_rotation * vect;
+			if (gyro_report.integral_dt != 0) {
+				math::Vector<3> vect_int(gyro_report.x_integral, gyro_report.y_integral, gyro_report.z_integral);
+				vect_int = _board_rotation * vect_int;
 
-			raw.gyro_rad_s[i * 3 + 0] = vect(0);
-			raw.gyro_rad_s[i * 3 + 1] = vect(1);
-			raw.gyro_rad_s[i * 3 + 2] = vect(2);
+				raw.gyro_integral_rad[i * 3 + 0] = vect_int(0);
+				raw.gyro_integral_rad[i * 3 + 1] = vect_int(1);
+				raw.gyro_integral_rad[i * 3 + 2] = vect_int(2);
 
-			math::Vector<3> vect_int(gyro_report.x_integral, gyro_report.y_integral, gyro_report.z_integral);
-			vect_int = _board_rotation * vect_int;
+				raw.gyro_integral_dt[i] = gyro_report.integral_dt;
+				raw.gyro_timestamp[i] = gyro_report.timestamp;
 
-			raw.gyro_integral_rad[i * 3 + 0] = vect_int(0);
-			raw.gyro_integral_rad[i * 3 + 1] = vect_int(1);
-			raw.gyro_integral_rad[i * 3 + 2] = vect_int(2);
+			} else {
+				//this is undesirable: a driver did not set the integral, so we have to construct it ourselves
+				math::Vector<3> vect_val(gyro_report.x, gyro_report.y, gyro_report.z);
+				vect_val = _board_rotation * vect_val;
 
-			raw.gyro_integral_dt[i] = gyro_report.integral_dt;
+				if (_last_gyro_timestamp[i] == 0) {
+					_last_gyro_timestamp[i] = gyro_report.timestamp - 1000;
+				}
 
-			raw.gyro_raw[i * 3 + 0] = gyro_report.x_raw;
-			raw.gyro_raw[i * 3 + 1] = gyro_report.y_raw;
-			raw.gyro_raw[i * 3 + 2] = gyro_report.z_raw;
+				raw.gyro_integral_dt[i] = gyro_report.timestamp - _last_gyro_timestamp[i];
+				_last_gyro_timestamp[i] = gyro_report.timestamp;
+				float dt = raw.gyro_integral_dt[i] / 1.e6f;
+				raw.gyro_integral_rad[i * 3 + 0] = vect_val(0) * dt;
+				raw.gyro_integral_rad[i * 3 + 1] = vect_val(1) * dt;
+				raw.gyro_integral_rad[i * 3 + 2] = vect_val(2) * dt;
 
-			raw.gyro_timestamp[i] = gyro_report.timestamp;
+			}
 
 			if (i == 0) {
 				raw.timestamp = gyro_report.timestamp;
 			}
-
-			raw.gyro_errcount[i] = gyro_report.error_count;
-			raw.gyro_temp[i] = gyro_report.temperature;
 		}
 	}
 }
@@ -1114,13 +1129,7 @@ Sensors::mag_poll(struct sensor_combined_s &raw)
 			raw.magnetometer_ga[i * 3 + 1] = vect(1);
 			raw.magnetometer_ga[i * 3 + 2] = vect(2);
 
-			raw.magnetometer_raw[i * 3 + 0] = mag_report.x_raw;
-			raw.magnetometer_raw[i * 3 + 1] = mag_report.y_raw;
-			raw.magnetometer_raw[i * 3 + 2] = mag_report.z_raw;
-
 			raw.magnetometer_timestamp[i] = mag_report.timestamp;
-			raw.magnetometer_errcount[i] = mag_report.error_count;
-			raw.magnetometer_temp[i] = mag_report.temperature;
 		}
 	}
 }
@@ -1136,7 +1145,7 @@ Sensors::baro_poll(struct sensor_combined_s &raw)
 
 			orb_copy(ORB_ID(sensor_baro), _baro_sub[i], &_barometer);
 
-			raw.baro_pres_mbar[i] = _barometer.pressure; // Pressure in mbar
+			_latest_baro_pressure[i] = _barometer.pressure;
 			raw.baro_alt_meter[i] = _barometer.altitude; // Altitude in meters
 			raw.baro_temp_celcius[i] = _barometer.temperature; // Temperature in degrees celcius
 
@@ -1172,12 +1181,13 @@ Sensors::diff_pres_poll(struct sensor_combined_s &raw)
 		_airspeed.indicated_airspeed_m_s = math::max(0.0f,
 						   calc_indicated_airspeed(_diff_pres.differential_pressure_filtered_pa));
 
+		//FIXME: we just use the baro pressure from the first baro. we should do voting instead.
 		_airspeed.true_airspeed_m_s = math::max(0.0f,
-							calc_true_airspeed(_diff_pres.differential_pressure_filtered_pa + raw.baro_pres_mbar[0] * 1e2f,
-									raw.baro_pres_mbar[0] * 1e2f, air_temperature_celsius));
+							calc_true_airspeed(_diff_pres.differential_pressure_filtered_pa + _latest_baro_pressure[0] * 1e2f,
+									_latest_baro_pressure[0] * 1e2f, air_temperature_celsius));
 		_airspeed.true_airspeed_unfiltered_m_s = math::max(0.0f,
-				calc_true_airspeed(_diff_pres.differential_pressure_raw_pa + raw.baro_pres_mbar[0] * 1e2f,
-						   raw.baro_pres_mbar[0] * 1e2f, air_temperature_celsius));
+				calc_true_airspeed(_diff_pres.differential_pressure_raw_pa + _latest_baro_pressure[0] * 1e2f,
+						   _latest_baro_pressure[0] * 1e2f, air_temperature_celsius));
 
 		_airspeed.air_temperature_celsius = air_temperature_celsius;
 
@@ -2050,8 +2060,7 @@ Sensors::task_main_trampoline(int argc, char *argv[])
 }
 
 int
-Sensors::init_sensor_class(const struct orb_metadata *meta, int *subs,
-			   uint32_t *priorities, uint32_t *errcount)
+Sensors::init_sensor_class(const struct orb_metadata *meta, int *subs)
 {
 	unsigned group_count = orb_group_count(meta);
 
@@ -2062,7 +2071,6 @@ Sensors::init_sensor_class(const struct orb_metadata *meta, int *subs,
 	for (unsigned i = 0; i < group_count; i++) {
 		if (subs[i] < 0) {
 			subs[i] = orb_subscribe_multi(meta, i);
-			orb_priority(subs[i], (int32_t *)&priorities[i]);
 		}
 	}
 
@@ -2106,14 +2114,13 @@ Sensors::task_main()
 
 	unsigned bcount_prev = _baro_count;
 
-	_gyro_count = init_sensor_class(ORB_ID(sensor_gyro), _gyro_sub, raw.gyro_priority, raw.gyro_errcount);
+	_gyro_count = init_sensor_class(ORB_ID(sensor_gyro), _gyro_sub);
 
-	_mag_count = init_sensor_class(ORB_ID(sensor_mag), _mag_sub, raw.magnetometer_priority, raw.magnetometer_errcount);
+	_mag_count = init_sensor_class(ORB_ID(sensor_mag), _mag_sub);
 
-	_accel_count = init_sensor_class(ORB_ID(sensor_accel), _accel_sub, raw.accelerometer_priority,
-					 raw.accelerometer_errcount);
+	_accel_count = init_sensor_class(ORB_ID(sensor_accel), _accel_sub);
 
-	_baro_count = init_sensor_class(ORB_ID(sensor_baro), _baro_sub, raw.baro_priority, raw.baro_errcount);
+	_baro_count = init_sensor_class(ORB_ID(sensor_baro), _baro_sub);
 
 	if (gcount_prev != _gyro_count ||
 	    mcount_prev != _mag_count ||
@@ -2178,8 +2185,7 @@ Sensors::task_main()
 			 * then attempt to subscribe once again
 			 */
 			if (_gyro_count == 0) {
-				_gyro_count = init_sensor_class(ORB_ID(sensor_gyro), _gyro_sub,
-								raw.gyro_priority, raw.gyro_errcount);
+				_gyro_count = init_sensor_class(ORB_ID(sensor_gyro), _gyro_sub);
 				fds[0].fd = _gyro_sub[0];
 			}
 
@@ -2237,17 +2243,13 @@ Sensors::task_main()
 		 * when not adding sensors poll for param updates
 		 */
 		if (!_armed && hrt_elapsed_time(&_last_config_update) > 500 * 1000) {
-			_gyro_count = init_sensor_class(ORB_ID(sensor_gyro), _gyro_sub,
-							raw.gyro_priority, raw.gyro_errcount);
+			_gyro_count = init_sensor_class(ORB_ID(sensor_gyro), _gyro_sub);
 
-			_mag_count = init_sensor_class(ORB_ID(sensor_mag), _mag_sub,
-						       raw.magnetometer_priority, raw.magnetometer_errcount);
+			_mag_count = init_sensor_class(ORB_ID(sensor_mag), _mag_sub);
 
-			_accel_count = init_sensor_class(ORB_ID(sensor_accel), _accel_sub,
-							 raw.accelerometer_priority, raw.accelerometer_errcount);
+			_accel_count = init_sensor_class(ORB_ID(sensor_accel), _accel_sub);
 
-			_baro_count = init_sensor_class(ORB_ID(sensor_baro), _baro_sub,
-							raw.baro_priority, raw.baro_errcount);
+			_baro_count = init_sensor_class(ORB_ID(sensor_baro), _baro_sub);
 
 			_last_config_update = hrt_absolute_time();