sensor_combined.msg: make timestamps relative

This is needed for the new logger & saves some space as well.

msg/sensor_combined.msg

@@ -5,18 +5,20 @@
 # change with board revisions and sensor updates.
 #
 
+int32 RELATIVE_TIMESTAMP_INVALID = 2147483647 # (0x7fffffff) If one of the relative timestamp is set to this value, it means the associated sensor values are invalid
+
 # gyro timstamp is equal to the timestamp of the message
 float32[3] gyro_integral_rad		# delta angle in the NED body frame in rad in the integration time frame
 uint32 gyro_integral_dt			# delta time for gyro integral in us
 
-uint64 accelerometer_timestamp	# Accelerometer timestamp
+int32 accelerometer_timestamp_relative	# timestamp + accelerometer_timestamp_relative = Accelerometer timestamp
 float32[3] accelerometer_integral_m_s		# velocity in NED body frame, in m/s^2
 uint32 accelerometer_integral_dt		# delta time for accel integral in us
 
-uint64 magnetometer_timestamp	# Magnetometer timestamp
+int32 magnetometer_timestamp_relative	# timestamp + magnetometer_timestamp_relative = Magnetometer timestamp
 float32[3] magnetometer_ga		# Magnetic field in NED body frame, in Gauss
 
-uint64 baro_timestamp		# Barometer timestamp
+int32 baro_timestamp_relative		# timestamp + baro_timestamp_relative = Barometer timestamp
 float32 baro_alt_meter			# Altitude, already temp. comp.
 float32 baro_temp_celcius		# Temperature in degrees celsius
 

src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp

@@ -438,10 +438,10 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
 					z_k[2] =  gyro_rad_s[2] - gyro_offsets[2];
 
 					/* update accelerometer measurements */
-					if (sensor_last_timestamp[1] != raw.accelerometer_timestamp) {
+					if (sensor_last_timestamp[1] != raw.timestamp + raw.accelerometer_timestamp_relative) {
 						update_vect[1] = 1;
 						// sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]);
-						sensor_last_timestamp[1] = raw.accelerometer_timestamp;
+						sensor_last_timestamp[1] = raw.timestamp + raw.accelerometer_timestamp_relative;
 					}
 
 					hrt_abstime vel_t = 0;
@@ -487,14 +487,14 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
 					z_k[5] = raw_accel(2) - acc(2);
 
 					/* update magnetometer measurements */
-					if (sensor_last_timestamp[2] != raw.magnetometer_timestamp &&
+					if (sensor_last_timestamp[2] != raw.timestamp + raw.magnetometer_timestamp_relative &&
 						/* check that the mag vector is > 0 */
 						fabsf(sqrtf(raw.magnetometer_ga[0] * raw.magnetometer_ga[0] +
 							raw.magnetometer_ga[1] * raw.magnetometer_ga[1] +
 							raw.magnetometer_ga[2] * raw.magnetometer_ga[2])) > 0.1f) {
 						update_vect[2] = 1;
 						// sensor_update_hz[2] = 1e6f / (raw.timestamp - sensor_last_timestamp[2]);
-						sensor_last_timestamp[2] = raw.magnetometer_timestamp;
+						sensor_last_timestamp[2] = raw.timestamp + raw.magnetometer_timestamp_relative;
 					}
 
 					bool vision_updated = false;

src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp

@@ -334,7 +334,7 @@ void AttitudeEstimatorQ::task_main()
 				_gyro(2) = sensors.gyro_integral_rad[2] / gyro_dt;
 			}
 
-			if (sensors.accelerometer_timestamp > 0) {
+			if (sensors.accelerometer_timestamp_relative != sensor_combined_s::RELATIVE_TIMESTAMP_INVALID) {
 				float accel_dt = sensors.accelerometer_integral_dt / 1.e6f;
 				_accel(0) = sensors.accelerometer_integral_m_s[0] / accel_dt;
 				_accel(1) = sensors.accelerometer_integral_m_s[1] / accel_dt;
@@ -346,7 +346,7 @@ void AttitudeEstimatorQ::task_main()
 				}
 			}
 
-			if (sensors.magnetometer_timestamp > 0) {
+			if (sensors.magnetometer_timestamp_relative != sensor_combined_s::RELATIVE_TIMESTAMP_INVALID) {
 				_mag(0) = sensors.magnetometer_ga[0];
 				_mag(1) = sensors.magnetometer_ga[1];
 				_mag(2) = sensors.magnetometer_ga[2];

src/modules/commander/commander.cpp

@@ -1788,7 +1788,8 @@ int commander_thread_main(int argc, char *argv[])
 			 * vertical separation from other airtraffic the operator has to know when the
 			 * barometer is inoperational.
 			 * */
-			if (hrt_elapsed_time(&sensors.baro_timestamp) < FAILSAFE_DEFAULT_TIMEOUT) {
+			hrt_abstime baro_timestamp = sensors.timestamp + sensors.baro_timestamp_relative;
+			if (hrt_elapsed_time(&baro_timestamp) < FAILSAFE_DEFAULT_TIMEOUT) {
 				/* handle the case where baro was regained */
 				if (status_flags.barometer_failure) {
 					status_flags.barometer_failure = false;

src/modules/ekf2/ekf2_main.cpp

@@ -509,10 +509,18 @@ void Ekf2::task_main()
 				sensors.gyro_integral_rad, sensors.accelerometer_integral_m_s);
 
 		// read mag data
-		_ekf.setMagData(sensors.magnetometer_timestamp, sensors.magnetometer_ga);
+		if (sensors.magnetometer_timestamp_relative == sensor_combined_s::RELATIVE_TIMESTAMP_INVALID) {
+			_ekf.setMagData(0, sensors.magnetometer_ga);
+		} else {
+			_ekf.setMagData(sensors.timestamp + sensors.magnetometer_timestamp_relative, sensors.magnetometer_ga);
+		}
 
 		// read baro data
-		_ekf.setBaroData(sensors.baro_timestamp, &sensors.baro_alt_meter);
+		if (sensors.baro_timestamp_relative == sensor_combined_s::RELATIVE_TIMESTAMP_INVALID) {
+			_ekf.setBaroData(0, &sensors.baro_alt_meter);
+		} else {
+			_ekf.setBaroData(sensors.timestamp + sensors.baro_timestamp_relative, &sensors.baro_alt_meter);
+		}
 
 		// read gps data if available
 		if (gps_updated) {
@@ -907,8 +915,8 @@ void Ekf2::task_main()
 			replay.time_ref = now;
 			replay.gyro_integral_dt = sensors.gyro_integral_dt;
 			replay.accelerometer_integral_dt = sensors.accelerometer_integral_dt;
-			replay.magnetometer_timestamp = sensors.magnetometer_timestamp;
-			replay.baro_timestamp = sensors.baro_timestamp;
+			replay.magnetometer_timestamp = sensors.timestamp + sensors.magnetometer_timestamp_relative;
+			replay.baro_timestamp = sensors.timestamp + sensors.baro_timestamp_relative;
 			memcpy(replay.gyro_integral_rad, sensors.gyro_integral_rad, sizeof(replay.gyro_integral_rad));
 			memcpy(replay.accelerometer_integral_m_s, sensors.accelerometer_integral_m_s,
 			       sizeof(replay.accelerometer_integral_m_s));

src/modules/ekf2_replay/ekf2_replay_main.cpp

@@ -379,8 +379,8 @@ void Ekf2Replay::setEstimatorInput(uint8_t *data, uint8_t type)
 		_sensors.timestamp = replay_part1.time_ref;
 		_sensors.gyro_integral_dt = replay_part1.gyro_integral_dt;
 		_sensors.accelerometer_integral_dt = (uint32_t)replay_part1.accelerometer_integral_dt;
-		_sensors.magnetometer_timestamp = replay_part1.magnetometer_timestamp;
-		_sensors.baro_timestamp = replay_part1.baro_timestamp;
+		_sensors.magnetometer_timestamp_relative = (int32_t)(replay_part1.magnetometer_timestamp - _sensors.timestamp);
+		_sensors.baro_timestamp_relative = (int32_t)(replay_part1.baro_timestamp - _sensors.timestamp);
 		_sensors.gyro_integral_rad[0] = replay_part1.gyro_integral_x_rad;
 		_sensors.gyro_integral_rad[1] = replay_part1.gyro_integral_y_rad;
 		_sensors.gyro_integral_rad[2] = replay_part1.gyro_integral_z_rad;

src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp

@@ -644,8 +644,9 @@ void AttitudePositionEstimatorEKF::task_main()
 			 *    We run the filter only once all data has been fetched
 			 **/
 
-			if (_baro_init && _sensor_combined.accelerometer_timestamp && _sensor_combined.timestamp
-					&& _sensor_combined.magnetometer_timestamp) {
+			if (_baro_init && _sensor_combined.timestamp &&
+					_sensor_combined.accelerometer_timestamp_relative != sensor_combined_s::RELATIVE_TIMESTAMP_INVALID &&
+					_sensor_combined.magnetometer_timestamp_relative != sensor_combined_s::RELATIVE_TIMESTAMP_INVALID) {
 
 				// maintain filtered baro and gps altitudes to calculate weather offset
 				// baro sample rate is ~70Hz and measurement bandwidth is high
@@ -1365,7 +1366,7 @@ void AttitudePositionEstimatorEKF::pollData()
 
 	perf_count(_perf_gyro);
 
-	if (_last_accel != _sensor_combined.accelerometer_timestamp) {
+	if (_last_accel != _sensor_combined.timestamp + _sensor_combined.accelerometer_timestamp_relative) {
 
 		_ekf->dVelIMU.x = _sensor_combined.accelerometer_integral_m_s[0];
 		_ekf->dVelIMU.y = _sensor_combined.accelerometer_integral_m_s[1];
@@ -1374,18 +1375,18 @@ void AttitudePositionEstimatorEKF::pollData()
 		float accel_dt = _sensor_combined.accelerometer_integral_dt / 1.e6f;
 		_ekf->accel = _ekf->dVelIMU / accel_dt;
 
-		_last_accel = _sensor_combined.accelerometer_timestamp;
+		_last_accel = _sensor_combined.timestamp + _sensor_combined.accelerometer_timestamp_relative;
 	}
 
 	Vector3f mag(_sensor_combined.magnetometer_ga[0], _sensor_combined.magnetometer_ga[1],
 			_sensor_combined.magnetometer_ga[2]);
 
-	if (mag.length() > 0.1f && _last_mag != _sensor_combined.magnetometer_timestamp) {
+	if (mag.length() > 0.1f && _last_mag != _sensor_combined.timestamp + _sensor_combined.magnetometer_timestamp_relative) {
 		_ekf->magData.x = mag.x;
 		_ekf->magData.y = mag.y;
 		_ekf->magData.z = mag.z;
 		_newDataMag = true;
-		_last_mag = _sensor_combined.magnetometer_timestamp;
+		_last_mag = _sensor_combined.timestamp + _sensor_combined.magnetometer_timestamp_relative;
 		perf_count(_perf_mag);
 	}
 

src/modules/mavlink/mavlink_messages.cpp

@@ -724,10 +724,10 @@ protected:
 		if (_sensor_sub->update(&_sensor_time, &sensor)) {
 			uint16_t fields_updated = 0;
 
-			if (_accel_timestamp != sensor.accelerometer_timestamp) {
+			if (_accel_timestamp != sensor.timestamp + sensor.accelerometer_timestamp_relative) {
 				/* mark first three dimensions as changed */
 				fields_updated |= (1 << 0) | (1 << 1) | (1 << 2);
-				_accel_timestamp = sensor.accelerometer_timestamp;
+				_accel_timestamp = sensor.timestamp + sensor.accelerometer_timestamp_relative;
 			}
 
 			if (_gyro_timestamp != sensor.timestamp) {
@@ -736,16 +736,16 @@ protected:
 				_gyro_timestamp = sensor.timestamp;
 			}
 
-			if (_mag_timestamp != sensor.magnetometer_timestamp) {
+			if (_mag_timestamp != sensor.timestamp + sensor.magnetometer_timestamp_relative) {
 				/* mark third group dimensions as changed */
 				fields_updated |= (1 << 6) | (1 << 7) | (1 << 8);
-				_mag_timestamp = sensor.magnetometer_timestamp;
+				_mag_timestamp = sensor.timestamp + sensor.magnetometer_timestamp_relative;
 			}
 
-			if (_baro_timestamp != sensor.baro_timestamp) {
+			if (_baro_timestamp != sensor.timestamp + sensor.baro_timestamp_relative) {
 				/* mark last group dimensions as changed */
 				fields_updated |= (1 << 9) | (1 << 11) | (1 << 12);
-				_baro_timestamp = sensor.baro_timestamp;
+				_baro_timestamp = sensor.timestamp + sensor.baro_timestamp_relative;
 			}
 			_differential_pressure_sub->update(&_differential_pressure_time, &differential_pressure);
 

src/modules/mavlink/mavlink_receiver.cpp

@@ -1689,16 +1689,16 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
 		_hil_prev_accel[1] = imu.yacc;
 		_hil_prev_accel[2] = imu.zacc;
 		hil_sensors.accelerometer_integral_dt = dt * 1e6f;
-		hil_sensors.accelerometer_timestamp = timestamp;
+		hil_sensors.accelerometer_timestamp_relative = 0;
 
 		hil_sensors.magnetometer_ga[0] = imu.xmag;
 		hil_sensors.magnetometer_ga[1] = imu.ymag;
 		hil_sensors.magnetometer_ga[2] = imu.zmag;
-		hil_sensors.magnetometer_timestamp = timestamp;
+		hil_sensors.magnetometer_timestamp_relative = 0;
 
 		hil_sensors.baro_alt_meter = imu.pressure_alt;
 		hil_sensors.baro_temp_celcius = imu.temperature;
-		hil_sensors.baro_timestamp = timestamp;
+		hil_sensors.baro_timestamp_relative = 0;
 
 		/* publish combined sensor topic */
 		if (_sensors_pub == nullptr) {

src/modules/position_estimator_inav/position_estimator_inav_main.cpp

@@ -425,8 +425,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 			if (fds_init[0].revents & POLLIN) {
 				orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor);
 
-				if (wait_baro && sensor.baro_timestamp != baro_timestamp) {
-					baro_timestamp = sensor.baro_timestamp;
+				if (wait_baro && sensor.timestamp + sensor.baro_timestamp_relative != baro_timestamp) {
+					baro_timestamp = sensor.timestamp + sensor.baro_timestamp_relative;
 					baro_wait_for_sample_time = hrt_absolute_time();
 
 					/* mean calculation over several measurements */
@@ -502,7 +502,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 			if (updated) {
 				orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor);
 
-				if (sensor.accelerometer_timestamp != accel_timestamp) {
+				if (sensor.timestamp + sensor.accelerometer_timestamp_relative != accel_timestamp) {
 					if (att.R_valid) {
 						float sensor_accel[3];
 						float accel_dt = sensor.accelerometer_integral_dt / 1.e6f;
@@ -525,13 +525,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 						memset(acc, 0, sizeof(acc));
 					}
 
-					accel_timestamp = sensor.accelerometer_timestamp;
+					accel_timestamp = sensor.timestamp + sensor.accelerometer_timestamp_relative;
 					accel_updates++;
 				}
 
-				if (sensor.baro_timestamp != baro_timestamp) {
+				if (sensor.timestamp + sensor.baro_timestamp_relative != baro_timestamp) {
 					corr_baro = baro_offset - sensor.baro_alt_meter - z_est[0];
-					baro_timestamp = sensor.baro_timestamp;
+					baro_timestamp = sensor.timestamp + sensor.baro_timestamp_relative;
 					baro_updates++;
 				}
 			}

src/modules/sdlog2/sdlog2.c

@@ -1642,18 +1642,18 @@ int sdlog2_thread_main(int argc, char *argv[])
 				write_IMU = true;
 			}
 
-			if (buf.sensor.accelerometer_timestamp != accelerometer_timestamp) {
-				accelerometer_timestamp = buf.sensor.accelerometer_timestamp;
+			if (buf.sensor.timestamp + buf.sensor.accelerometer_timestamp_relative != accelerometer_timestamp) {
+				accelerometer_timestamp = buf.sensor.timestamp + buf.sensor.accelerometer_timestamp_relative;
 				write_IMU = true;
 			}
 
-			if (buf.sensor.magnetometer_timestamp != magnetometer_timestamp) {
-				magnetometer_timestamp = buf.sensor.magnetometer_timestamp;
+			if (buf.sensor.timestamp + buf.sensor.magnetometer_timestamp_relative != magnetometer_timestamp) {
+				magnetometer_timestamp = buf.sensor.timestamp + buf.sensor.magnetometer_timestamp_relative;
 				write_IMU = true;
 			}
 
-			if (buf.sensor.baro_timestamp != barometer_timestamp) {
-				barometer_timestamp = buf.sensor.baro_timestamp;
+			if (buf.sensor.timestamp + buf.sensor.baro_timestamp_relative != barometer_timestamp) {
+				barometer_timestamp = buf.sensor.timestamp + buf.sensor.baro_timestamp_relative;
 				write_SENS = true;
 			}
 

src/modules/sensors/sensors.cpp

@@ -210,7 +210,8 @@ private:
 
 	struct SensorData {
 		SensorData()
-			: subscription_count(0),
+			: last_best_vote(0),
+			  subscription_count(0),
 			  voter(SENSOR_COUNT_MAX),
 			  last_failover_count(0)
 		{
@@ -221,6 +222,7 @@ private:
 
 		int subscription[SENSOR_COUNT_MAX]; /**< raw sensor data subscription */
 		uint8_t priority[SENSOR_COUNT_MAX]; /**< sensor priority */
+		uint8_t last_best_vote; /**< index of the latest best vote */
 		int subscription_count;
 		DataValidatorGroup voter;
 		unsigned int last_failover_count;
@@ -267,6 +269,9 @@ private:
 	float _last_baro_pressure[SENSOR_COUNT_MAX]; /**< pressure from last baro sensors */
 	float _last_best_baro_pressure; /**< pressure from last best baro */
 	sensor_combined_s _last_sensor_data[SENSOR_COUNT_MAX]; /**< latest sensor data from all sensors instances */
+	uint64_t _last_accel_timestamp[SENSOR_COUNT_MAX]; /**< latest full timestamp */
+	uint64_t _last_mag_timestamp[SENSOR_COUNT_MAX]; /**< latest full timestamp */
+	uint64_t _last_baro_timestamp[SENSOR_COUNT_MAX]; /**< latest full timestamp */
 
 	hrt_abstime _vibration_warning_timestamp;
 	bool _vibration_warning;
@@ -595,6 +600,9 @@ Sensors::Sensors() :
 	memset(&_parameters, 0, sizeof(_parameters));
 	memset(&_rc_parameter_map, 0, sizeof(_rc_parameter_map));
 	memset(&_last_sensor_data, 0, sizeof(_last_sensor_data));
+	memset(&_last_accel_timestamp, 0, sizeof(_last_accel_timestamp));
+	memset(&_last_mag_timestamp, 0, sizeof(_last_mag_timestamp));
+	memset(&_last_baro_timestamp, 0, sizeof(_last_baro_timestamp));
 
 	/* basic r/c parameters */
 	for (unsigned i = 0; i < _rc_max_chan_count; i++) {
@@ -1088,19 +1096,19 @@ Sensors::accel_poll(struct sensor_combined_s &raw)
 
 				sensor_value = vect_val;
 
-				if (_last_sensor_data[i].accelerometer_timestamp == 0) {
-					_last_sensor_data[i].accelerometer_timestamp = accel_report.timestamp - 1000;
+				if (_last_accel_timestamp[i] == 0) {
+					_last_accel_timestamp[i] = accel_report.timestamp - 1000;
 				}
 
 				_last_sensor_data[i].accelerometer_integral_dt =
-					(uint32_t)(accel_report.timestamp - _last_sensor_data[i].accelerometer_timestamp);
+					(uint32_t)(accel_report.timestamp - _last_accel_timestamp[i]);
 				float dt = _last_sensor_data[i].accelerometer_integral_dt / 1.e6f;
 				_last_sensor_data[i].accelerometer_integral_m_s[0] = vect_val(0) * dt;
 				_last_sensor_data[i].accelerometer_integral_m_s[1] = vect_val(1) * dt;
 				_last_sensor_data[i].accelerometer_integral_m_s[2] = vect_val(2) * dt;
 			}
 
-			_last_sensor_data[i].accelerometer_timestamp = accel_report.timestamp;
+			_last_accel_timestamp[i] = accel_report.timestamp;
 			_accel.voter.put(i, accel_report.timestamp, sensor_value.data,
 					 accel_report.error_count, _accel.priority[i]);
 		}
@@ -1115,7 +1123,7 @@ Sensors::accel_poll(struct sensor_combined_s &raw)
 			raw.accelerometer_integral_m_s[1] = _last_sensor_data[best_index].accelerometer_integral_m_s[1];
 			raw.accelerometer_integral_m_s[2] = _last_sensor_data[best_index].accelerometer_integral_m_s[2];
 			raw.accelerometer_integral_dt = _last_sensor_data[best_index].accelerometer_integral_dt;
-			raw.accelerometer_timestamp = _last_sensor_data[best_index].accelerometer_timestamp;
+			_accel.last_best_vote = (uint8_t)best_index;
 		}
 	}
 }
@@ -1189,6 +1197,7 @@ Sensors::gyro_poll(struct sensor_combined_s &raw)
 			raw.gyro_integral_rad[2] = _last_sensor_data[best_index].gyro_integral_rad[2];
 			raw.gyro_integral_dt = _last_sensor_data[best_index].gyro_integral_dt;
 			raw.timestamp = _last_sensor_data[best_index].timestamp;
+			_gyro.last_best_vote = (uint8_t)best_index;
 		}
 	}
 }
@@ -1219,7 +1228,7 @@ Sensors::mag_poll(struct sensor_combined_s &raw)
 			_last_sensor_data[i].magnetometer_ga[1] = vect(1);
 			_last_sensor_data[i].magnetometer_ga[2] = vect(2);
 
-			_last_sensor_data[i].magnetometer_timestamp = mag_report.timestamp;
+			_last_mag_timestamp[i] = mag_report.timestamp;
 			_mag.voter.put(i, mag_report.timestamp, vect.data,
 				       mag_report.error_count, _mag.priority[i]);
 		}
@@ -1233,7 +1242,7 @@ Sensors::mag_poll(struct sensor_combined_s &raw)
 			raw.magnetometer_ga[0] = _last_sensor_data[best_index].magnetometer_ga[0];
 			raw.magnetometer_ga[1] = _last_sensor_data[best_index].magnetometer_ga[1];
 			raw.magnetometer_ga[2] = _last_sensor_data[best_index].magnetometer_ga[2];
-			raw.magnetometer_timestamp = _last_sensor_data[best_index].magnetometer_timestamp;
+			_mag.last_best_vote = (uint8_t)best_index;
 		}
 	}
 }
@@ -1263,7 +1272,7 @@ Sensors::baro_poll(struct sensor_combined_s &raw)
 			_last_sensor_data[i].baro_temp_celcius = baro_report.temperature;
 			_last_baro_pressure[i] = baro_report.pressure;
 
-			_last_sensor_data[i].baro_timestamp = baro_report.timestamp;
+			_last_baro_timestamp[i] = baro_report.timestamp;
 			_baro.voter.put(i, baro_report.timestamp, vect.data,
 					baro_report.error_count, _baro.priority[i]);
 		}
@@ -1276,8 +1285,8 @@ Sensors::baro_poll(struct sensor_combined_s &raw)
 		if (best_index >= 0) {
 			raw.baro_alt_meter = _last_sensor_data[best_index].baro_alt_meter;
 			raw.baro_temp_celcius = _last_sensor_data[best_index].baro_temp_celcius;
-			raw.baro_timestamp = _last_sensor_data[best_index].baro_timestamp;
 			_last_best_baro_pressure = _last_baro_pressure[best_index];
+			_baro.last_best_vote = (uint8_t)best_index;
 		}
 	}
 }
@@ -2267,6 +2276,12 @@ Sensors::task_main()
 
 	struct sensor_combined_s raw = {};
 
+	raw.accelerometer_timestamp_relative = sensor_combined_s::RELATIVE_TIMESTAMP_INVALID;
+
+	raw.magnetometer_timestamp_relative = sensor_combined_s::RELATIVE_TIMESTAMP_INVALID;
+
+	raw.baro_timestamp_relative = sensor_combined_s::RELATIVE_TIMESTAMP_INVALID;
+
 	/*
 	 * do subscriptions
 	 */
@@ -2376,6 +2391,20 @@ Sensors::task_main()
 		diff_pres_poll(raw);
 
 		if (_publishing && raw.timestamp > 0) {
+
+			/* construct relative timestamps */
+			if (_last_accel_timestamp[_accel.last_best_vote]) {
+				raw.accelerometer_timestamp_relative = (int32_t)(_last_accel_timestamp[_accel.last_best_vote] - raw.timestamp);
+			}
+
+			if (_last_mag_timestamp[_mag.last_best_vote]) {
+				raw.magnetometer_timestamp_relative = (int32_t)(_last_mag_timestamp[_mag.last_best_vote] - raw.timestamp);
+			}
+
+			if (_last_baro_timestamp[_baro.last_best_vote]) {
+				raw.baro_timestamp_relative = (int32_t)(_last_baro_timestamp[_baro.last_best_vote] - raw.timestamp);
+			}
+
 			orb_publish(ORB_ID(sensor_combined), _sensor_pub, &raw);
 
 			check_failover(_accel, "Accel");