lib/drivers/{accelerometer,gyroscope}: ensure sane monotonically increasing timestamp_sample

src/lib/drivers/accelerometer/PX4Accelerometer.cpp

@@ -113,10 +113,11 @@ void PX4Accelerometer::update(const hrt_abstime &timestamp_sample, float x, floa
 	// Apply rotation (before scaling)
 	rotate_3f(_rotation, x, y, z);
 
-	// publish
 	sensor_accel_s report;
 
-	report.timestamp_sample = timestamp_sample;
+	// ensure monotonically increasing timestamp_sample within reasonable range
+	report.timestamp_sample = math::constrain(timestamp_sample, _timestamp_prev, hrt_absolute_time());
+
 	report.device_id = _device_id;
 	report.temperature = _temperature;
 	report.error_count = _error_count;
@@ -130,10 +131,15 @@ void PX4Accelerometer::update(const hrt_abstime &timestamp_sample, float x, floa
 	report.timestamp = hrt_absolute_time();
 
 	_sensor_pub.publish(report);
+
+	_timestamp_prev = report.timestamp;
 }
 
 void PX4Accelerometer::updateFIFO(sensor_accel_fifo_s &sample)
 {
+	// ensure monotonically increasing timestamp_sample within reasonable range
+	sample.timestamp_sample = math::constrain(sample.timestamp_sample, _timestamp_prev, hrt_absolute_time());
+
 	// rotate all raw samples and publish fifo
 	const uint8_t N = sample.samples;
 
@@ -146,6 +152,8 @@ void PX4Accelerometer::updateFIFO(sensor_accel_fifo_s &sample)
 	sample.timestamp = hrt_absolute_time();
 	_sensor_fifo_pub.publish(sample);
 
+	_timestamp_prev = sample.timestamp;
+
 
 	// publish
 	sensor_accel_s report;

src/lib/drivers/accelerometer/PX4Accelerometer.hpp

@@ -70,6 +70,8 @@ private:
 	uORB::PublicationMulti<sensor_accel_s> _sensor_pub{ORB_ID(sensor_accel)};
 	uORB::PublicationMulti<sensor_accel_fifo_s>  _sensor_fifo_pub{ORB_ID(sensor_accel_fifo)};
 
+	hrt_abstime _timestamp_prev{0};
+
 	uint32_t		_device_id{0};
 	const enum Rotation	_rotation;
 

src/lib/drivers/gyroscope/PX4Gyroscope.cpp

@@ -100,7 +100,9 @@ void PX4Gyroscope::update(const hrt_abstime &timestamp_sample, float x, float y,
 
 	sensor_gyro_s report;
 
-	report.timestamp_sample = timestamp_sample;
+	// ensure monotonically increasing timestamp_sample within reasonable range
+	report.timestamp_sample = math::constrain(timestamp_sample, _timestamp_prev, hrt_absolute_time());
+
 	report.device_id = _device_id;
 	report.temperature = _temperature;
 	report.error_count = _error_count;
@@ -111,10 +113,15 @@ void PX4Gyroscope::update(const hrt_abstime &timestamp_sample, float x, float y,
 	report.timestamp = hrt_absolute_time();
 
 	_sensor_pub.publish(report);
+
+	_timestamp_prev = report.timestamp;
 }
 
 void PX4Gyroscope::updateFIFO(sensor_gyro_fifo_s &sample)
 {
+	// ensure monotonically increasing timestamp_sample within reasonable range
+	sample.timestamp_sample = math::constrain(sample.timestamp_sample, _timestamp_prev, hrt_absolute_time());
+
 	// rotate all raw samples and publish fifo
 	const uint8_t N = sample.samples;
 
@@ -127,6 +134,8 @@ void PX4Gyroscope::updateFIFO(sensor_gyro_fifo_s &sample)
 	sample.timestamp = hrt_absolute_time();
 	_sensor_fifo_pub.publish(sample);
 
+	_timestamp_prev = sample.timestamp;
+
 
 	// publish
 	sensor_gyro_s report;

src/lib/drivers/gyroscope/PX4Gyroscope.hpp

@@ -1,4 +1,3 @@
-
 /****************************************************************************
  *
  *   Copyright (c) 2018-2021 PX4 Development Team. All rights reserved.
@@ -68,6 +67,8 @@ private:
 	uORB::PublicationMulti<sensor_gyro_s> _sensor_pub{ORB_ID(sensor_gyro)};
 	uORB::PublicationMulti<sensor_gyro_fifo_s>  _sensor_fifo_pub{ORB_ID(sensor_gyro_fifo)};
 
+	hrt_abstime _timestamp_prev{0};
+
 	uint32_t		_device_id{0};
 	const enum Rotation	_rotation;