Merge branch 'beta_mavlink2' of github.com:PX4/Firmware into mavlink2_hil

ROMFS/px4fmu_common/init.d/rcS

@@ -474,7 +474,7 @@ then
 		sh /etc/init.d/rc.interface
 		
 		# Start standard fixedwing apps
-		if [ LOAD_DEFAULT_APPS == yes ]
+		if [ $LOAD_DEFAULT_APPS == yes ]
 		then
 			sh /etc/init.d/rc.fw_apps
 		fi
@@ -533,7 +533,7 @@ then
 		sh /etc/init.d/rc.interface
 		
 		# Start standard multicopter apps
-		if [ LOAD_DEFAULT_APPS == yes ]
+		if [ $LOAD_DEFAULT_APPS == yes ]
 		then
 			sh /etc/init.d/rc.mc_apps
 		fi

src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp

@@ -277,7 +277,6 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 	// XXX write this out to perf regs
 
 	/* keep track of sensor updates */
-	uint32_t sensor_last_count[3] = {0, 0, 0};
 	uint64_t sensor_last_timestamp[3] = {0, 0, 0};
 
 	struct attitude_estimator_ekf_params ekf_params;
@@ -380,9 +379,8 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 					uint8_t update_vect[3] = {0, 0, 0};
 
 					/* Fill in gyro measurements */
-					if (sensor_last_count[0] != raw.gyro_counter) {
+					if (sensor_last_timestamp[0] != raw.timestamp) {
 						update_vect[0] = 1;
-						sensor_last_count[0] = raw.gyro_counter;
 						sensor_update_hz[0] = 1e6f / (raw.timestamp - sensor_last_timestamp[0]);
 						sensor_last_timestamp[0] = raw.timestamp;
 					}
@@ -392,9 +390,8 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 					z_k[2] =  raw.gyro_rad_s[2] - gyro_offsets[2];
 
 					/* update accelerometer measurements */
-					if (sensor_last_count[1] != raw.accelerometer_counter) {
+					if (sensor_last_timestamp[1] != raw.accelerometer_timestamp) {
 						update_vect[1] = 1;
-						sensor_last_count[1] = raw.accelerometer_counter;
 						sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]);
 						sensor_last_timestamp[1] = raw.timestamp;
 					}
@@ -445,9 +442,8 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 					z_k[5] = raw.accelerometer_m_s2[2] - acc(2);
 
 					/* update magnetometer measurements */
-					if (sensor_last_count[2] != raw.magnetometer_counter) {
+					if (sensor_last_timestamp[2] != raw.magnetometer_timestamp) {
 						update_vect[2] = 1;
-						sensor_last_count[2] = raw.magnetometer_counter;
 						sensor_update_hz[2] = 1e6f / (raw.timestamp - sensor_last_timestamp[2]);
 						sensor_last_timestamp[2] = raw.timestamp;
 					}

src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp

@@ -445,7 +445,6 @@ int attitude_estimator_so3_thread_main(int argc, char *argv[])
 	// XXX write this out to perf regs
 
 	/* keep track of sensor updates */
-	uint32_t sensor_last_count[3] = {0, 0, 0};
 	uint64_t sensor_last_timestamp[3] = {0, 0, 0};
 
 	struct attitude_estimator_so3_params so3_comp_params;
@@ -526,9 +525,8 @@ int attitude_estimator_so3_thread_main(int argc, char *argv[])
 					uint8_t update_vect[3] = {0, 0, 0};
 
 					/* Fill in gyro measurements */
-					if (sensor_last_count[0] != raw.gyro_counter) {
+					if (sensor_last_timestamp[0] != raw.timestamp) {
 						update_vect[0] = 1;
-						sensor_last_count[0] = raw.gyro_counter;
 						sensor_update_hz[0] = 1e6f / (raw.timestamp - sensor_last_timestamp[0]);
 						sensor_last_timestamp[0] = raw.timestamp;
 					}
@@ -538,9 +536,8 @@ int attitude_estimator_so3_thread_main(int argc, char *argv[])
 					gyro[2] = raw.gyro_rad_s[2] - gyro_offsets[2];
 
 					/* update accelerometer measurements */
-					if (sensor_last_count[1] != raw.accelerometer_counter) {
+					if (sensor_last_timestamp[1] != raw.accelerometer_timestamp) {
 						update_vect[1] = 1;
-						sensor_last_count[1] = raw.accelerometer_counter;
 						sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]);
 						sensor_last_timestamp[1] = raw.timestamp;
 					}
@@ -550,9 +547,8 @@ int attitude_estimator_so3_thread_main(int argc, char *argv[])
 					acc[2] = raw.accelerometer_m_s2[2];
 
 					/* update magnetometer measurements */
-					if (sensor_last_count[2] != raw.magnetometer_counter) {
+					if (sensor_last_timestamp[2] != raw.magnetometer_timestamp) {
 						update_vect[2] = 1;
-						sensor_last_count[2] = raw.magnetometer_counter;
 						sensor_update_hz[2] = 1e6f / (raw.timestamp - sensor_last_timestamp[2]);
 						sensor_last_timestamp[2] = raw.timestamp;
 					}

src/modules/mavlink/mavlink_messages.cpp

@@ -216,8 +216,8 @@ protected:
 
 	void send(const hrt_abstime t)
 	{
-		status_sub->update(t);
+		(void)status_sub->update(t);
-		pos_sp_triplet_sub->update(t);
+		(void)pos_sp_triplet_sub->update(t);
 
 		uint8_t mavlink_state = 0;
 		uint8_t mavlink_base_mode = 0;
@@ -261,7 +261,7 @@ protected:
 
 	void send(const hrt_abstime t)
 	{
-		status_sub->update(t);
+		if (status_sub->update(t)) {
 
 			mavlink_msg_sys_status_send(_channel,
 						    status->onboard_control_sensors_present,
@@ -278,13 +278,14 @@ protected:
 						    status->errors_count3,
 						    status->errors_count4);
 		}
+	}
 };
 
 
 class MavlinkStreamHighresIMU : public MavlinkStream
 {
 public:
-	MavlinkStreamHighresIMU() : MavlinkStream(), accel_counter(0), gyro_counter(0), mag_counter(0), baro_counter(0)
+	MavlinkStreamHighresIMU() : MavlinkStream(), accel_timestamp(0), gyro_timestamp(0), mag_timestamp(0), baro_timestamp(0)
 	{
 	}
 
@@ -302,10 +303,10 @@ private:
 	MavlinkOrbSubscription *sensor_sub;
 	struct sensor_combined_s *sensor;
 
-	uint32_t accel_counter;
+	uint64_t accel_timestamp;
-	uint32_t gyro_counter;
+	uint64_t gyro_timestamp;
-	uint32_t mag_counter;
+	uint64_t mag_timestamp;
-	uint32_t baro_counter;
+	uint64_t baro_timestamp;
 
 protected:
 	void subscribe(Mavlink *mavlink)
@@ -316,32 +317,32 @@ protected:
 
 	void send(const hrt_abstime t)
 	{
-		sensor_sub->update(t);
+		if (sensor_sub->update(t)) {
 
 			uint16_t fields_updated = 0;
 
-		if (accel_counter != sensor->accelerometer_counter) {
+			if (accel_timestamp != sensor->accelerometer_timestamp) {
 				/* mark first three dimensions as changed */
 				fields_updated |= (1 << 0) | (1 << 1) | (1 << 2);
-			accel_counter = sensor->accelerometer_counter;
+				accel_timestamp = sensor->accelerometer_timestamp;
 			}
 
-		if (gyro_counter != sensor->gyro_counter) {
+			if (gyro_timestamp != sensor->timestamp) {
 				/* mark second group dimensions as changed */
 				fields_updated |= (1 << 3) | (1 << 4) | (1 << 5);
-			gyro_counter = sensor->gyro_counter;
+				gyro_timestamp = sensor->timestamp;
 			}
 
-		if (mag_counter != sensor->magnetometer_counter) {
+			if (mag_timestamp != sensor->magnetometer_timestamp) {
 				/* mark third group dimensions as changed */
 				fields_updated |= (1 << 6) | (1 << 7) | (1 << 8);
-			mag_counter = sensor->magnetometer_counter;
+				mag_timestamp = sensor->magnetometer_timestamp;
 			}
 
-		if (baro_counter != sensor->baro_counter) {
+			if (baro_timestamp != sensor->baro_timestamp) {
 				/* mark last group dimensions as changed */
 				fields_updated |= (1 << 9) | (1 << 11) | (1 << 12);
-			baro_counter = sensor->baro_counter;
+				baro_timestamp = sensor->baro_timestamp;
 			}
 
 			mavlink_msg_highres_imu_send(_channel,
@@ -353,6 +354,7 @@ protected:
 						     sensor->baro_alt_meter, sensor->baro_temp_celcius,
 						     fields_updated);
 		}
+	}
 };
 
 
@@ -382,13 +384,14 @@ protected:
 
 	void send(const hrt_abstime t)
 	{
-		att_sub->update(t);
+		if (att_sub->update(t)) {
 
 			mavlink_msg_attitude_send(_channel,
 						  att->timestamp / 1000,
 						  att->roll, att->pitch, att->yaw,
 						  att->rollspeed, att->pitchspeed, att->yawspeed);
 		}
+	}
 };
 
 
@@ -418,7 +421,7 @@ protected:
 
 	void send(const hrt_abstime t)
 	{
-		att_sub->update(t);
+		if (att_sub->update(t)) {
 
 			mavlink_msg_attitude_quaternion_send(_channel,
 							     att->timestamp / 1000,
@@ -430,6 +433,7 @@ protected:
 							     att->pitchspeed,
 							     att->yawspeed);
 		}
+	}
 };
 
 
@@ -483,11 +487,13 @@ protected:
 
 	void send(const hrt_abstime t)
 	{
-		att_sub->update(t);
+		bool updated = att_sub->update(t);
-		pos_sub->update(t);
+		updated |= pos_sub->update(t);
-		armed_sub->update(t);
+		updated |= armed_sub->update(t);
-		act_sub->update(t);
+		updated |= act_sub->update(t);
-		airspeed_sub->update(t);
+		updated |= airspeed_sub->update(t);
+
+		if (updated) {
 
 			float groundspeed = sqrtf(pos->vel_n * pos->vel_n + pos->vel_e * pos->vel_e);
 			uint16_t heading = _wrap_2pi(att->yaw) * M_RAD_TO_DEG_F;
@@ -501,6 +507,7 @@ protected:
 						 pos->alt,
 						 -pos->vel_d);
 		}
+	}
 };
 
 
@@ -530,7 +537,7 @@ protected:
 
 	void send(const hrt_abstime t)
 	{
-		gps_sub->update(t);
+		if (gps_sub->update(t)) {
 
 			mavlink_msg_gps_raw_int_send(_channel,
 						     gps->timestamp_position,
@@ -544,6 +551,7 @@ protected:
 						     _wrap_2pi(gps->cog_rad) * M_RAD_TO_DEG_F * 1e2f,
 						     gps->satellites_visible);
 		}
+	}
 };
 
 
@@ -579,9 +587,10 @@ protected:
 
 	void send(const hrt_abstime t)
 	{
-		pos_sub->update(t);
+		bool updated = pos_sub->update(t);
-		home_sub->update(t);
+		updated |= home_sub->update(t);
 
+		if (updated) {
 			mavlink_msg_global_position_int_send(_channel,
 						     pos->timestamp / 1000,
 						     pos->lat * 1e7,
@@ -593,6 +602,7 @@ protected:
 						     pos->vel_d * 100.0f,
 						     _wrap_2pi(pos->yaw) * M_RAD_TO_DEG_F * 100.0f);
 		}
+	}
 };
 
 
@@ -622,7 +632,7 @@ protected:
 
 	void send(const hrt_abstime t)
 	{
-		pos_sub->update(t);
+		if (pos_sub->update(t)) {
 
 			mavlink_msg_local_position_ned_send(_channel,
 							    pos->timestamp / 1000,
@@ -633,6 +643,7 @@ protected:
 							    pos->vy,
 							    pos->vz);
 		}
+	}
 };
 
 
@@ -662,6 +673,10 @@ protected:
 
 	void send(const hrt_abstime t)
 	{
+
+		/* we're sending the GPS home periodically to ensure the
+		 * the GCS does pick it up at one point */
+		if (home_sub->is_published()) {
 			home_sub->update(t);
 
 			mavlink_msg_gps_global_origin_send(_channel,
@@ -669,6 +684,7 @@ protected:
 							   (int32_t)(home->lon * 1e7),
 							   (int32_t)(home->alt) * 1000.0f);
 		}
+	}
 };
 
 
@@ -713,7 +729,7 @@ protected:
 
 	void send(const hrt_abstime t)
 	{
-		act_sub->update(t);
+		if (act_sub->update(t)) {
 
 			mavlink_msg_servo_output_raw_send(_channel,
 							  act->timestamp / 1000,
@@ -727,6 +743,7 @@ protected:
 							  act->output[6],
 							  act->output[7]);
 		}
+	}
 };
 
 
@@ -768,9 +785,11 @@ protected:
 
 	void send(const hrt_abstime t)
 	{
-		status_sub->update(t);
+		bool updated = status_sub->update(t);
-		pos_sp_triplet_sub->update(t);
+		updated |= pos_sp_triplet_sub->update(t);
-		act_sub->update(t);
+		updated |= act_sub->update(t);
+
+		if (updated) {
 
 			/* translate the current syste state to mavlink state and mode */
 			uint8_t mavlink_state;
@@ -820,6 +839,7 @@ protected:
 						      mavlink_base_mode,
 						      0);
 		}
+	}
 };
 
 
@@ -849,7 +869,7 @@ protected:
 
 	void send(const hrt_abstime t)
 	{
-		pos_sp_triplet_sub->update(t);
+		if (pos_sp_triplet_sub->update(t)) {
 
 			mavlink_msg_global_position_setpoint_int_send(_channel,
 					MAV_FRAME_GLOBAL,
@@ -858,6 +878,7 @@ protected:
 					(int32_t)(pos_sp_triplet->current.alt * 1000),
 					(int16_t)(pos_sp_triplet->current.yaw * M_RAD_TO_DEG_F * 100.0f));
 		}
+	}
 };
 
 
@@ -925,7 +946,7 @@ protected:
 
 	void send(const hrt_abstime t)
 	{
-		att_sp_sub->update(t);
+		if (att_sp_sub->update(t)) {
 
 			mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(_channel,
 					att_sp->timestamp / 1000,
@@ -934,6 +955,7 @@ protected:
 					att_sp->yaw_body,
 					att_sp->thrust);
 		}
+	}
 };
 
 
@@ -963,7 +985,7 @@ protected:
 
 	void send(const hrt_abstime t)
 	{
-		att_rates_sp_sub->update(t);
+		if (att_rates_sp_sub->update(t)) {
 
 			mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_send(_channel,
 					att_rates_sp->timestamp / 1000,
@@ -972,6 +994,7 @@ protected:
 					att_rates_sp->yaw,
 					att_rates_sp->thrust);
 		}
+	}
 };
 
 
@@ -1001,7 +1024,7 @@ protected:
 
 	void send(const hrt_abstime t)
 	{
-		rc_sub->update(t);
+		if (rc_sub->update(t)) {
 
 			const unsigned port_width = 8;
 
@@ -1021,6 +1044,7 @@ protected:
 								 rc->rssi);
 			}
 		}
+	}
 };
 
 
@@ -1050,7 +1074,7 @@ protected:
 
 	void send(const hrt_abstime t)
 	{
-		manual_sub->update(t);
+		if (manual_sub->update(t)) {
 
 			mavlink_msg_manual_control_send(_channel,
 							mavlink_system.sysid,
@@ -1060,6 +1084,7 @@ protected:
 							manual->throttle * 1000,
 							0);
 		}
+	}
 };
 
 
@@ -1089,7 +1114,7 @@ protected:
 
 	void send(const hrt_abstime t)
 	{
-		flow_sub->update(t);
+		if (flow_sub->update(t)) {
 
 			mavlink_msg_optical_flow_send(_channel,
 						      flow->timestamp,
@@ -1099,6 +1124,7 @@ protected:
 						      flow->quality,
 						      flow->ground_distance_m);
 		}
+	}
 };
 
 

src/modules/mavlink/mavlink_orb_subscription.cpp

@@ -46,11 +46,15 @@
 
 #include "mavlink_orb_subscription.h"
 
-MavlinkOrbSubscription::MavlinkOrbSubscription(const orb_id_t topic) : _topic(topic), _last_check(0), next(nullptr)
+MavlinkOrbSubscription::MavlinkOrbSubscription(const orb_id_t topic) :
+	_fd(orb_subscribe(_topic)),
+	_published(false),
+	_topic(topic),
+	_last_check(0),
+	next(nullptr)
 {
 	_data = malloc(topic->o_size);
 	memset(_data, 0, topic->o_size);
-	_fd = orb_subscribe(_topic);
 }
 
 MavlinkOrbSubscription::~MavlinkOrbSubscription()
@@ -87,3 +91,16 @@ MavlinkOrbSubscription::update(const hrt_abstime t)
 
 	return false;
 }
+
+bool
+MavlinkOrbSubscription::is_published()
+{
+	bool updated;
+	orb_check(_fd, &updated);
+
+	if (updated) {
+		_published = true;
+	}
+
+	return _published;
+}

src/modules/mavlink/mavlink_orb_subscription.h

@@ -54,12 +54,21 @@ public:
 	~MavlinkOrbSubscription();
 
 	bool update(const hrt_abstime t);
+
+	/**
+	 * Check if the topic has been published.
+	 *
+	 * This call will return true if the topic was ever published.
+	 * @param true if the topic has been published at least once.
+	 */
+	bool is_published();
 	void *get_data();
 	const orb_id_t get_topic();
 
 private:
 	const orb_id_t _topic;
 	int _fd;
+	bool _published;
 	void *_data;
 	hrt_abstime _last_check;
 };

src/modules/mavlink/mavlink_receiver.cpp

@@ -586,7 +586,6 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
 		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_counter = _hil_counter;
 
 		hil_sensors.accelerometer_raw[0] = imu.xacc / mg2ms2;
 		hil_sensors.accelerometer_raw[1] = imu.yacc / mg2ms2;
@@ -596,7 +595,7 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
 		hil_sensors.accelerometer_m_s2[2] = imu.zacc;
 		hil_sensors.accelerometer_mode = 0; // TODO what is this?
 		hil_sensors.accelerometer_range_m_s2 = 32.7f; // int16
-		hil_sensors.accelerometer_counter = _hil_counter;
+		hil_sensors.accelerometer_timestamp = timestamp;
 
 		hil_sensors.adc_voltage_v[0] = 0.0f;
 		hil_sensors.adc_voltage_v[1] = 0.0f;
@@ -611,15 +610,15 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
 		hil_sensors.magnetometer_range_ga = 32.7f; // int16
 		hil_sensors.magnetometer_mode = 0; // TODO what is this
 		hil_sensors.magnetometer_cuttoff_freq_hz = 50.0f;
-		hil_sensors.magnetometer_counter = _hil_counter;
+		hil_sensors.magnetometer_timestamp = timestamp;
 
 		hil_sensors.baro_pres_mbar = imu.abs_pressure;
 		hil_sensors.baro_alt_meter = imu.pressure_alt;
 		hil_sensors.baro_temp_celcius = imu.temperature;
-		hil_sensors.baro_counter = _hil_counter;
+		hil_sensors.baro_timestamp = timestamp;
 
 		hil_sensors.differential_pressure_pa = imu.diff_pressure * 1e2f; //from hPa to Pa
-		hil_sensors.differential_pressure_counter = _hil_counter;
+		hil_sensors.differential_pressure_timestamp = timestamp;
 
 		/* publish combined sensor topic */
 		if (_sensors_pub > 0) {

src/modules/position_estimator_inav/position_estimator_inav_main.c

@@ -200,8 +200,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 	bool landed = true;
 	hrt_abstime landed_time = 0;
 
-	uint32_t accel_counter = 0;
+	hrt_abstime accel_timestamp = 0;
-	uint32_t baro_counter = 0;
+	hrt_abstime baro_timestamp = 0;
 
 	bool ref_inited = false;
 	hrt_abstime ref_init_start = 0;
@@ -310,8 +310,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_counter != baro_counter) {
+				if (wait_baro && sensor.baro_timestamp != baro_timestamp) {
-					baro_counter = sensor.baro_counter;
+					baro_timestamp = sensor.baro_timestamp;
 
 					/* mean calculation over several measurements */
 					if (baro_init_cnt < baro_init_num) {
@@ -384,7 +384,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_counter != accel_counter) {
+				if (sensor.accelerometer_timestamp != accel_timestamp) {
 					if (att.R_valid) {
 						/* correct accel bias */
 						sensor.accelerometer_m_s2[0] -= acc_bias[0];
@@ -408,13 +408,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 						memset(corr_acc, 0, sizeof(corr_acc));
 					}
 
-					accel_counter = sensor.accelerometer_counter;
+					accel_timestamp = sensor.accelerometer_timestamp;
 					accel_updates++;
 				}
 
-				if (sensor.baro_counter != baro_counter) {
+				if (sensor.baro_timestamp != baro_timestamp) {
 					corr_baro = baro_offset - sensor.baro_alt_meter - z_est[0];
-					baro_counter = sensor.baro_counter;
+					baro_timestamp = sensor.baro_timestamp;
 					baro_updates++;
 				}
 			}

src/modules/sdlog2/sdlog2.c

@@ -887,11 +887,11 @@ int sdlog2_thread_main(int argc, char *argv[])
 	pthread_cond_init(&logbuffer_cond, NULL);
 
 	/* track changes in sensor_combined topic */
-	uint16_t gyro_counter = 0;
+	hrt_abstime gyro_timestamp = 0;
-	uint16_t accelerometer_counter = 0;
+	hrt_abstime accelerometer_timestamp = 0;
-	uint16_t magnetometer_counter = 0;
+	hrt_abstime magnetometer_timestamp = 0;
-	uint16_t baro_counter = 0;
+	hrt_abstime barometer_timestamp = 0;
-	uint16_t differential_pressure_counter = 0;
+	hrt_abstime differential_pressure_timestamp = 0;
 
 	/* track changes in distance status */
 	bool dist_bottom_present = false;
@@ -976,28 +976,28 @@ int sdlog2_thread_main(int argc, char *argv[])
 			bool write_IMU = false;
 			bool write_SENS = false;
 
-			if (buf.sensor.gyro_counter != gyro_counter) {
+			if (buf.sensor.timestamp != gyro_timestamp) {
-				gyro_counter = buf.sensor.gyro_counter;
+				gyro_timestamp = buf.sensor.timestamp;
 				write_IMU = true;
 			}
 
-			if (buf.sensor.accelerometer_counter != accelerometer_counter) {
+			if (buf.sensor.accelerometer_timestamp != accelerometer_timestamp) {
-				accelerometer_counter = buf.sensor.accelerometer_counter;
+				accelerometer_timestamp = buf.sensor.accelerometer_timestamp;
 				write_IMU = true;
 			}
 
-			if (buf.sensor.magnetometer_counter != magnetometer_counter) {
+			if (buf.sensor.magnetometer_timestamp != magnetometer_timestamp) {
-				magnetometer_counter = buf.sensor.magnetometer_counter;
+				magnetometer_timestamp = buf.sensor.magnetometer_timestamp;
 				write_IMU = true;
 			}
 
-			if (buf.sensor.baro_counter != baro_counter) {
+			if (buf.sensor.baro_timestamp != barometer_timestamp) {
-				baro_counter = buf.sensor.baro_counter;
+				barometer_timestamp = buf.sensor.baro_timestamp;
 				write_SENS = true;
 			}
 
-			if (buf.sensor.differential_pressure_counter != differential_pressure_counter) {
+			if (buf.sensor.differential_pressure_timestamp != differential_pressure_timestamp) {
-				differential_pressure_counter = buf.sensor.differential_pressure_counter;
+				differential_pressure_timestamp = buf.sensor.differential_pressure_timestamp;
 				write_SENS = true;
 			}
 
@@ -1023,6 +1023,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 				log_msg.body.log_SENS.diff_pres = buf.sensor.differential_pressure_pa;
 				LOGBUFFER_WRITE_AND_COUNT(SENS);
 			}
+
 		}
 
 		/* --- ATTITUDE --- */

src/modules/sensors/sensors.cpp

@@ -940,7 +940,7 @@ Sensors::accel_poll(struct sensor_combined_s &raw)
 		raw.accelerometer_raw[1] = accel_report.y_raw;
 		raw.accelerometer_raw[2] = accel_report.z_raw;
 
-		raw.accelerometer_counter++;
+		raw.accelerometer_timestamp = accel_report.timestamp;
 	}
 }
 
@@ -966,7 +966,7 @@ Sensors::gyro_poll(struct sensor_combined_s &raw)
 		raw.gyro_raw[1] = gyro_report.y_raw;
 		raw.gyro_raw[2] = gyro_report.z_raw;
 
-		raw.gyro_counter++;
+		raw.timestamp = gyro_report.timestamp;
 	}
 }
 
@@ -996,7 +996,7 @@ Sensors::mag_poll(struct sensor_combined_s &raw)
 		raw.magnetometer_raw[1] = mag_report.y_raw;
 		raw.magnetometer_raw[2] = mag_report.z_raw;
 
-		raw.magnetometer_counter++;
+		raw.magnetometer_timestamp = mag_report.timestamp;
 	}
 }
 
@@ -1014,7 +1014,7 @@ Sensors::baro_poll(struct sensor_combined_s &raw)
 		raw.baro_alt_meter = _barometer.altitude; // Altitude in meters
 		raw.baro_temp_celcius = _barometer.temperature; // Temperature in degrees celcius
 
-		raw.baro_counter++;
+		raw.baro_timestamp = _barometer.timestamp;
 	}
 }
 
@@ -1028,7 +1028,7 @@ Sensors::diff_pres_poll(struct sensor_combined_s &raw)
 		orb_copy(ORB_ID(differential_pressure), _diff_pres_sub, &_diff_pres);
 
 		raw.differential_pressure_pa = _diff_pres.differential_pressure_pa;
-		raw.differential_pressure_counter++;
+		raw.differential_pressure_timestamp = _diff_pres.timestamp;
 
 		_airspeed.indicated_airspeed_m_s = calc_indicated_airspeed(_diff_pres.differential_pressure_pa);
 		_airspeed.true_airspeed_m_s = calc_true_airspeed(_diff_pres.differential_pressure_pa + raw.baro_pres_mbar * 1e2f,

src/modules/uORB/topics/sensor_combined.h

@@ -77,34 +77,33 @@ struct sensor_combined_s {
 
 	/* NOTE: Ordering of fields optimized to align to 32 bit / 4 bytes Change with consideration only   */
 
-	uint64_t timestamp;			/**< Timestamp in microseconds since boot         */
+	uint64_t timestamp;			/**< Timestamp in microseconds since boot, from gyro         */
 
 	int16_t	gyro_raw[3];			/**< Raw sensor values of angular velocity        */
-	uint16_t gyro_counter;			/**< Number of raw measurments taken              */
 	float gyro_rad_s[3];			/**< Angular velocity in radian per seconds       */
 
 	int16_t accelerometer_raw[3];		/**< Raw acceleration in NED body frame           */
-	uint32_t accelerometer_counter;		/**< Number of raw acc measurements taken         */
 	float accelerometer_m_s2[3];		/**< Acceleration in NED body frame, in m/s^2     */
 	int accelerometer_mode;			/**< Accelerometer measurement mode */
 	float accelerometer_range_m_s2;		/**< Accelerometer measurement range in m/s^2 */
+	uint64_t accelerometer_timestamp;	/**< Accelerometer timestamp        */
 
 	int16_t	magnetometer_raw[3];		/**< Raw magnetic field in NED body frame         */
 	float magnetometer_ga[3];		/**< Magnetic field in NED body frame, in Gauss   */
 	int magnetometer_mode;			/**< Magnetometer measurement mode */
 	float magnetometer_range_ga;		/**< ± measurement range in Gauss */
 	float magnetometer_cuttoff_freq_hz;	/**< Internal analog low pass frequency of sensor */
-	uint32_t magnetometer_counter;		/**< Number of raw mag measurements taken         */
+	uint64_t magnetometer_timestamp;	/**< Magnetometer timestamp         */
 
 	float baro_pres_mbar;			/**< Barometric pressure, already temp. comp.     */
 	float baro_alt_meter;			/**< Altitude, already temp. comp.                */
 	float baro_temp_celcius;		/**< Temperature in degrees celsius               */
 	float adc_voltage_v[4];			/**< ADC voltages of ADC Chan 10/11/12/13 or -1      */
 	float mcu_temp_celcius;			/**< Internal temperature measurement of MCU */
-	uint32_t baro_counter;			/**< Number of raw baro measurements taken        */
+	uint64_t baro_timestamp;		/**< Barometer timestamp        */
 
 	float differential_pressure_pa;				/**< Airspeed sensor differential pressure                  */
-	uint32_t differential_pressure_counter;		/**< Number of raw differential pressure measurements taken */
+	uint64_t differential_pressure_timestamp;		/**< Last measurement timestamp */
 };
 
 /**