PX4Accelerometer/PX4Gyroscope update FIFO case to trapezoidal integration

src/drivers/imu/invensense/icm20602/ICM20602.cpp

@@ -300,6 +300,11 @@ void ICM20602::Run()
 {
 	perf_count(_interval_perf);
 
+	// use timestamp from the data ready interrupt if available,
+	//  otherwise use the time now roughly corresponding with the last sample we'll pull from the FIFO
+	const hrt_abstime timestamp_sample = (hrt_elapsed_time(&_time_data_ready) < FIFO_INTERVAL) ? _time_data_ready :
+					     hrt_absolute_time();
+
 	// read FIFO count
 	uint8_t fifo_count_buf[3] {};
 	fifo_count_buf[0] = static_cast<uint8_t>(Register::FIFO_COUNTH) | DIR_READ;
@@ -351,15 +356,11 @@ void ICM20602::Run()
 
 	perf_end(_transfer_perf);
 
-	static constexpr uint32_t gyro_dt = FIFO_INTERVAL / FIFO_GYRO_SAMPLES;
-	// estimate timestamp of first sample in the FIFO from number of samples and fill rate
-	const hrt_abstime timestamp_sample = _time_data_ready - ((samples - 1) * gyro_dt);
-
-	PX4Accelerometer::FIFOSample accel{};
+	PX4Accelerometer::FIFOSample accel;
 	accel.timestamp_sample = timestamp_sample;
 	accel.dt = FIFO_INTERVAL / FIFO_ACCEL_SAMPLES;
 
-	PX4Gyroscope::FIFOSample gyro{};
+	PX4Gyroscope::FIFOSample gyro;
 	gyro.timestamp_sample = timestamp_sample;
 	gyro.samples = samples;
 	gyro.dt = FIFO_INTERVAL / FIFO_GYRO_SAMPLES;

src/drivers/imu/invensense/icm20608-g/ICM20608G.cpp

@@ -276,6 +276,11 @@ void ICM20608G::Run()
 {
 	perf_count(_interval_perf);
 
+	// use timestamp from the data ready interrupt if available,
+	//  otherwise use the time now roughly corresponding with the last sample we'll pull from the FIFO
+	const hrt_abstime timestamp_sample = (hrt_elapsed_time(&_time_data_ready) < FIFO_INTERVAL) ? _time_data_ready :
+					     hrt_absolute_time();
+
 	// read FIFO count
 	uint8_t fifo_count_buf[3] {};
 	fifo_count_buf[0] = static_cast<uint8_t>(Register::FIFO_COUNTH) | DIR_READ;
@@ -327,15 +332,11 @@ void ICM20608G::Run()
 
 	perf_end(_transfer_perf);
 
-	static constexpr uint32_t gyro_dt = FIFO_INTERVAL / FIFO_GYRO_SAMPLES;
-	// estimate timestamp of first sample in the FIFO from number of samples and fill rate
-	const hrt_abstime timestamp_sample = _time_data_ready - ((samples - 1) * gyro_dt);
-
-	PX4Accelerometer::FIFOSample accel{};
+	PX4Accelerometer::FIFOSample accel;
 	accel.timestamp_sample = timestamp_sample;
 	accel.dt = FIFO_INTERVAL / FIFO_ACCEL_SAMPLES;
 
-	PX4Gyroscope::FIFOSample gyro{};
+	PX4Gyroscope::FIFOSample gyro;
 	gyro.timestamp_sample = timestamp_sample;
 	gyro.samples = samples;
 	gyro.dt = FIFO_INTERVAL / FIFO_GYRO_SAMPLES;

src/drivers/imu/st/ism330dlc/ISM330DLC.cpp

@@ -40,6 +40,8 @@ using namespace ST_ISM330DLC;
 
 static constexpr int16_t combine(uint8_t lsb, uint8_t msb) { return (msb << 8u) | lsb; }
 
+static constexpr uint32_t FIFO_INTERVAL{1000}; // 1000 us / 1000 Hz interval
+
 ISM330DLC::ISM330DLC(int bus, uint32_t device, enum Rotation rotation) :
 	SPI(MODULE_NAME, nullptr, bus, device, SPIDEV_MODE3, SPI_SPEED),
 	ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(get_device_id())),
@@ -53,8 +55,8 @@ ISM330DLC::ISM330DLC(int bus, uint32_t device, enum Rotation rotation) :
 	_px4_accel.set_sample_rate(ST_ISM330DLC::LA_ODR);
 	_px4_gyro.set_sample_rate(ST_ISM330DLC::G_ODR);
 
-	_px4_accel.set_update_rate(1000000 / _fifo_interval);
-	_px4_gyro.set_update_rate(1000000 / _fifo_interval);
+	_px4_accel.set_update_rate(1000000 / FIFO_INTERVAL);
+	_px4_gyro.set_update_rate(1000000 / FIFO_INTERVAL);
 }
 
 ISM330DLC::~ISM330DLC()
@@ -227,8 +229,7 @@ void ISM330DLC::Start()
 	RegisterWrite(Register::INT1_CTRL, INT1_CTRL_BIT::INT1_FULL_FLAG | INT1_CTRL_BIT::INT1_FIFO_OVR |
 		      INT1_CTRL_BIT::INT1_FTH);
 #else
-
-	ScheduleOnInterval(_fifo_interval, _fifo_interval);
+	ScheduleOnInterval(FIFO_INTERVAL, FIFO_INTERVAL);
 #endif
 }
 
@@ -248,8 +249,12 @@ void ISM330DLC::Run()
 {
 	perf_count(_interval_perf);
 
+	// use timestamp from the data ready interrupt if available,
+	//  otherwise use the time now roughly corresponding with the last sample we'll pull from the FIFO
+	const hrt_abstime timestamp_sample = (hrt_elapsed_time(&_time_data_ready) < FIFO_INTERVAL) ? _time_data_ready :
+					     hrt_absolute_time();
+
 	// Number of unread words (16-bit axes) stored in FIFO.
-	const hrt_abstime timestamp_fifo_level = hrt_absolute_time();
 	const uint8_t fifo_words = RegisterRead(Register::FIFO_STATUS1);
 
 	// check for FIFO status
@@ -311,19 +316,15 @@ void ISM330DLC::Run()
 
 	perf_end(_transfer_perf);
 
-	static constexpr uint32_t gyro_dt = 1000000 / ST_ISM330DLC::G_ODR;
-	// estimate timestamp of first sample in the FIFO from number of samples and fill rate
-	const hrt_abstime timestamp_sample = timestamp_fifo_level - ((samples - 1) * gyro_dt);
-
-	PX4Accelerometer::FIFOSample accel{};
+	PX4Accelerometer::FIFOSample accel;
 	accel.timestamp_sample = timestamp_sample;
 	accel.samples = samples;
-	accel.dt = gyro_dt;
+	accel.dt = 1000000 / ST_ISM330DLC::LA_ODR;
 
-	PX4Gyroscope::FIFOSample gyro{};
+	PX4Gyroscope::FIFOSample gyro;
 	gyro.timestamp_sample = timestamp_sample;
 	gyro.samples = samples;
-	gyro.dt = gyro_dt;
+	gyro.dt = 1000000 / ST_ISM330DLC::G_ODR;
 
 	for (int i = 0; i < samples; i++) {
 		const FIFO::DATA &fifo_sample = report->f[i];
@@ -366,6 +367,8 @@ void ISM330DLC::PrintInfo()
 	perf_print_counter(_fifo_empty_perf);
 	perf_print_counter(_fifo_overflow_perf);
 	perf_print_counter(_fifo_reset_perf);
+	perf_print_counter(_drdy_count_perf);
+	perf_print_counter(_drdy_interval_perf);
 
 	_px4_accel.print_status();
 	_px4_gyro.print_status();

src/drivers/imu/st/ism330dlc/ISM330DLC.hpp

@@ -84,8 +84,6 @@ private:
 	PX4Accelerometer _px4_accel;
 	PX4Gyroscope _px4_gyro;
 
-	static constexpr uint32_t _fifo_interval{1000}; // 1000 us sample interval
-
 	perf_counter_t _interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": run interval")};
 	perf_counter_t _transfer_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": transfer")};
 	perf_counter_t _fifo_empty_perf{perf_alloc(PC_COUNT, MODULE_NAME": fifo empty")};

src/lib/drivers/accelerometer/PX4Accelerometer.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2018 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2018-2020 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -39,6 +39,30 @@
 using namespace time_literals;
 using matrix::Vector3f;
 
+static inline int32_t sum(const int16_t samples[16], uint8_t len)
+{
+	int32_t sum = 0;
+
+	for (int n = 0; n < len; n++) {
+		sum += samples[n];
+	}
+
+	return sum;
+}
+
+static inline unsigned clipping(const int16_t samples[16], int16_t clip_limit, uint8_t len)
+{
+	unsigned clip_count = 0;
+
+	for (int n = 0; n < len; n++) {
+		if (abs(samples[n]) > clip_limit) {
+			clip_count++;
+		}
+	}
+
+	return clip_count;
+}
+
 PX4Accelerometer::PX4Accelerometer(uint32_t device_id, uint8_t priority, enum Rotation rotation) :
 	CDev(nullptr),
 	ModuleParams(nullptr),
@@ -47,7 +71,8 @@ PX4Accelerometer::PX4Accelerometer(uint32_t device_id, uint8_t priority, enum Ro
 	_sensor_integrated_pub{ORB_ID(sensor_accel_integrated), priority},
 	_sensor_status_pub{ORB_ID(sensor_accel_status), priority},
 	_device_id{device_id},
-	_rotation{rotation}
+	_rotation{rotation},
+	_rotation_dcm{get_rot_matrix(rotation)}
 {
 	_class_device_instance = register_class_devname(ACCEL_BASE_DEVICE_PATH);
 
@@ -119,10 +144,8 @@ void PX4Accelerometer::update(hrt_abstime timestamp_sample, float x, float y, fl
 	const Vector3f raw{x, y, z};
 
 	// Clipping (check unscaled raw values)
-	const float clip_limit = (_range / _scale) * 0.95f;
-
 	for (int i = 0; i < 3; i++) {
-		if (fabsf(raw(i)) > clip_limit) {
+		if (fabsf(raw(i)) > _clip_limit) {
 			_clipping[i]++;
 			_integrator_clipping++;
 		}
@@ -139,10 +162,6 @@ void PX4Accelerometer::update(hrt_abstime timestamp_sample, float x, float y, fl
 	Vector3f delta_velocity;
 	uint32_t integral_dt = 0;
 
-	if (_integrator_samples == 0) {
-		_integrator_timestamp_sample = timestamp_sample;
-	}
-
 	_integrator_samples++;
 
 	if (_integrator.put(timestamp_sample, val_calibrated, delta_velocity, integral_dt)) {
@@ -165,7 +184,7 @@ void PX4Accelerometer::update(hrt_abstime timestamp_sample, float x, float y, fl
 		// fill sensor_accel_integrated and publish
 		sensor_accel_integrated_s report{};
 
-		report.timestamp_sample = _integrator_timestamp_sample;
+		report.timestamp_sample = timestamp_sample;
 		report.error_count = _error_count;
 		report.device_id = _device_id;
 		delta_velocity.copyTo(report.delta_velocity);
@@ -189,10 +208,13 @@ void PX4Accelerometer::update(hrt_abstime timestamp_sample, float x, float y, fl
 
 void PX4Accelerometer::updateFIFO(const FIFOSample &sample)
 {
+	const uint8_t N = sample.samples;
+	const float dt = sample.dt;
+
 	// filtered data (control)
-	float x_filtered = _filterArrayX.apply(sample.x, sample.samples);
-	float y_filtered = _filterArrayY.apply(sample.y, sample.samples);
-	float z_filtered = _filterArrayZ.apply(sample.z, sample.samples);
+	float x_filtered = _filterArrayX.apply(sample.x, N);
+	float y_filtered = _filterArrayY.apply(sample.y, N);
+	float z_filtered = _filterArrayZ.apply(sample.z, N);
 
 	// Apply rotation (before scaling)
 	rotate_3f(_rotation, x_filtered, y_filtered, z_filtered);
@@ -200,65 +222,41 @@ void PX4Accelerometer::updateFIFO(const FIFOSample &sample)
 	const Vector3f raw{x_filtered, y_filtered, z_filtered};
 
 	// Apply range scale and the calibrating offset/scale
-	const Vector3f val_calibrated{(((raw * _scale) - _calibration_offset).emult(_calibration_scale))};
+	const Vector3f val_calibrated{((raw * _scale) - _calibration_offset).emult(_calibration_scale)};
 
 
 	// clipping
-	const int16_t clip_limit = (_range / _scale) * 0.95f;
-
-	// x clipping
-	for (int n = 0; n < sample.samples; n++) {
-		if (abs(sample.x[n]) > clip_limit) {
-			_clipping[0]++;
-			_integrator_clipping++;
-		}
-	}
+	unsigned clip_count_x = clipping(sample.x, _clip_limit, N);
+	unsigned clip_count_y = clipping(sample.y, _clip_limit, N);
+	unsigned clip_count_z = clipping(sample.z, _clip_limit, N);
 
-	// y clipping
-	for (int n = 0; n < sample.samples; n++) {
-		if (abs(sample.y[n]) > clip_limit) {
-			_clipping[1]++;
-			_integrator_clipping++;
-		}
-	}
-
-	// z clipping
-	for (int n = 0; n < sample.samples; n++) {
-		if (abs(sample.z[n]) > clip_limit) {
-			_clipping[2]++;
-			_integrator_clipping++;
-		}
-	}
+	_clipping[0] += clip_count_x;
+	_clipping[1] += clip_count_y;
+	_clipping[2] += clip_count_z;
 
+	_integrator_clipping += clip_count_x + clip_count_y + clip_count_z;
 
 	// integrated data (INS)
 	{
 		// reset integrator if previous sample was too long ago
 		if ((sample.timestamp_sample > _timestamp_sample_prev)
-		    && ((sample.timestamp_sample - _timestamp_sample_prev) > (sample.samples * sample.dt * 2))) {
+		    && ((sample.timestamp_sample - _timestamp_sample_prev) > (N * dt * 2.0f))) {
 
 			ResetIntegrator();
 		}
 
-		if (_integrator_samples == 0) {
-			_integrator_timestamp_sample = sample.timestamp_sample;
-		}
-
 		// integrate
 		_integrator_samples += 1;
-		_integrator_fifo_samples += sample.samples;
-
-		for (int n = 0; n < sample.samples; n++) {
-			_integrator_accum[0] += sample.x[n];
-		}
+		_integrator_fifo_samples += N;
 
-		for (int n = 0; n < sample.samples; n++) {
-			_integrator_accum[1] += sample.y[n];
-		}
+		// trapezoidal integration (equally spaced, scaled by dt later)
+		_integration_raw(0) += (0.5f * (_last_sample[0] + sample.x[N - 1]) + sum(sample.x, N - 1));
+		_integration_raw(1) += (0.5f * (_last_sample[1] + sample.y[N - 1]) + sum(sample.y, N - 1));
+		_integration_raw(2) += (0.5f * (_last_sample[2] + sample.z[N - 1]) + sum(sample.z, N - 1));
+		_last_sample[0] = sample.x[N - 1];
+		_last_sample[1] = sample.y[N - 1];
+		_last_sample[2] = sample.z[N - 1];
 
-		for (int n = 0; n < sample.samples; n++) {
-			_integrator_accum[2] += sample.z[n];
-		}
 
 		if (_integrator_fifo_samples > 0 && (_integrator_samples >= _integrator_reset_samples)) {
 
@@ -266,7 +264,7 @@ void PX4Accelerometer::updateFIFO(const FIFOSample &sample)
 			{
 				sensor_accel_s report{};
 
-				report.timestamp_sample = sample.timestamp_sample + ((sample.samples - 1) * sample.dt); // timestamp of last sample
+				report.timestamp_sample = sample.timestamp_sample;
 				report.device_id = _device_id;
 				report.temperature = _temperature;
 				report.x = val_calibrated(0);
@@ -277,31 +275,25 @@ void PX4Accelerometer::updateFIFO(const FIFOSample &sample)
 				_sensor_pub.publish(report);
 			}
 
+			// Apply rotation and scale
+			// integrated in microseconds, convert to seconds
+			const Vector3f delta_velocity_uncalibrated{_rotation_dcm *_integration_raw * _scale};
 
-			const uint32_t integrator_dt_us = _integrator_fifo_samples * sample.dt; // time span in microseconds
-
-			// average integrated values to apply calibration
-			float x_int_avg = _integrator_accum[0] / _integrator_fifo_samples;
-			float y_int_avg = _integrator_accum[1] / _integrator_fifo_samples;
-			float z_int_avg = _integrator_accum[2] / _integrator_fifo_samples;
-
-			// Apply rotation (before scaling)
-			rotate_3f(_rotation, x_int_avg, y_int_avg, z_int_avg);
+			// scale calibration offset to number of samples
+			const Vector3f offset{_calibration_offset * _integrator_fifo_samples};
 
-			const Vector3f raw_int{x_int_avg, y_int_avg, z_int_avg};
-
-			// Apply range scale and the calibrating offset/scale
-			Vector3f delta_velocity{(((raw_int * _scale) - _calibration_offset).emult(_calibration_scale))};
-			delta_velocity *= (_integrator_fifo_samples * sample.dt * 1e-6f);	// restore
+			// Apply calibration and scale to seconds
+			Vector3f delta_velocity{((delta_velocity_uncalibrated - offset).emult(_calibration_scale))};
+			delta_velocity *= 1e-6f * dt;
 
 			// fill sensor_accel_integrated and publish
 			sensor_accel_integrated_s report{};
 
-			report.timestamp_sample = _integrator_timestamp_sample;
+			report.timestamp_sample = sample.timestamp_sample;
 			report.error_count = _error_count;
 			report.device_id = _device_id;
 			delta_velocity.copyTo(report.delta_velocity);
-			report.dt = integrator_dt_us;
+			report.dt = _integrator_fifo_samples * dt; // time span in microseconds
 			report.samples = _integrator_fifo_samples;
 			report.clip_count = _integrator_clipping;
 
@@ -323,13 +315,13 @@ void PX4Accelerometer::updateFIFO(const FIFOSample &sample)
 
 	fifo.device_id = _device_id;
 	fifo.timestamp_sample = sample.timestamp_sample;
-	fifo.dt = sample.dt;
+	fifo.dt = dt;
 	fifo.scale = _scale;
-	fifo.samples = sample.samples;
+	fifo.samples = N;
 
-	memcpy(fifo.x, sample.x, sizeof(sample.x[0]) * sample.samples);
-	memcpy(fifo.y, sample.y, sizeof(sample.y[0]) * sample.samples);
-	memcpy(fifo.z, sample.z, sizeof(sample.z[0]) * sample.samples);
+	memcpy(fifo.x, sample.x, sizeof(sample.x[0]) * N);
+	memcpy(fifo.y, sample.y, sizeof(sample.y[0]) * N);
+	memcpy(fifo.z, sample.z, sizeof(sample.z[0]) * N);
 
 	fifo.timestamp = hrt_absolute_time();
 	_sensor_fifo_pub.publish(fifo);
@@ -366,12 +358,9 @@ void PX4Accelerometer::ResetIntegrator()
 {
 	_integrator_samples = 0;
 	_integrator_fifo_samples = 0;
-	_integrator_accum[0] = 0;
-	_integrator_accum[1] = 0;
-	_integrator_accum[2] = 0;
+	_integration_raw.zero();
 	_integrator_clipping = 0;
 
-	_integrator_timestamp_sample = 0;
 	_timestamp_sample_prev = 0;
 }
 
@@ -384,6 +373,12 @@ void PX4Accelerometer::ConfigureFilter(float cutoff_freq)
 	_filterArrayZ.set_cutoff_frequency(_sample_rate, cutoff_freq);
 }
 
+void PX4Accelerometer::UpdateClipLimit()
+{
+	// 95% of potential max
+	_clip_limit = (_range / _scale) * 0.95f;
+}
+
 void PX4Accelerometer::UpdateVibrationMetrics(const Vector3f &delta_velocity)
 {
 	// Accel high frequency vibe = filtered length of (delta_velocity - prev_delta_velocity)

src/lib/drivers/accelerometer/PX4Accelerometer.hpp

@@ -61,9 +61,9 @@ public:
 	void set_device_id(uint32_t device_id) { _device_id = device_id; }
 	void set_device_type(uint8_t devtype);
 	void set_error_count(uint64_t error_count) { _error_count += error_count; }
-	void set_range(float range) { _range = range; }
+	void set_range(float range) { _range = range; UpdateClipLimit(); }
 	void set_sample_rate(uint16_t rate);
-	void set_scale(float scale) { _scale = scale; }
+	void set_scale(float scale) { _scale = scale; UpdateClipLimit(); }
 	void set_temperature(float temperature) { _temperature = temperature; }
 	void set_update_rate(uint16_t rate);
 
@@ -91,6 +91,7 @@ private:
 	void ConfigureFilter(float cutoff_freq);
 	void PublishStatus();
 	void ResetIntegrator();
+	void UpdateClipLimit();
 	void UpdateVibrationMetrics(const matrix::Vector3f &delta_velocity);
 
 	uORB::PublicationMulti<sensor_accel_s>            _sensor_pub;
@@ -100,28 +101,31 @@ private:
 
 	math::LowPassFilter2pVector3f _filter{1000, 100};
 
+	hrt_abstime	_status_last_publish{0};
+
 	math::LowPassFilter2pArray _filterArrayX{4000, 100};
 	math::LowPassFilter2pArray _filterArrayY{4000, 100};
 	math::LowPassFilter2pArray _filterArrayZ{4000, 100};
 
-	hrt_abstime	_status_last_publish{0};
-
 	Integrator		_integrator{4000, false};
 
-	matrix::Vector3f	_calibration_scale{1.0f, 1.0f, 1.0f};
-	matrix::Vector3f	_calibration_offset{0.0f, 0.0f, 0.0f};
+	matrix::Vector3f	_calibration_scale{1.f, 1.f, 1.f};
+	matrix::Vector3f	_calibration_offset{0.f, 0.f, 0.f};
 
-	matrix::Vector3f _delta_velocity_prev{0.0f, 0.0f, 0.0f};	// delta velocity from the previous IMU measurement
-	float _vibration_metric{0.0f};	// high frequency vibration level in the IMU delta velocity data (m/s)
+	matrix::Vector3f _delta_velocity_prev{0.f, 0.f, 0.f};	// delta velocity from the previous IMU measurement
+	float _vibration_metric{0.f};	// high frequency vibration level in the IMU delta velocity data (m/s)
 
 	int			_class_device_instance{-1};
 
 	uint32_t		_device_id{0};
 	const enum Rotation	_rotation;
+	const matrix::Dcmf	_rotation_dcm;
+
+	float			_range{16 * CONSTANTS_ONE_G};
+	float			_scale{1.f};
+	float			_temperature{0.f};
 
-	float			_range{16.0f * CONSTANTS_ONE_G};
-	float			_scale{1.0f};
-	float			_temperature{0.0f};
+	int16_t			_clip_limit{(int16_t)(_range / _scale)};
 
 	uint64_t		_error_count{0};
 
@@ -131,9 +135,9 @@ private:
 	uint16_t		_update_rate{1000};
 
 	// integrator
-	hrt_abstime		_integrator_timestamp_sample{0};
 	hrt_abstime		_timestamp_sample_prev{0};
-	float			_integrator_accum[3] {};
+	matrix::Vector3f	_integration_raw{};
+	int16_t			_last_sample[3] {};
 	uint8_t			_integrator_reset_samples{4};
 	uint8_t			_integrator_samples{0};
 	uint8_t			_integrator_fifo_samples{0};

src/lib/drivers/gyroscope/PX4Gyroscope.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2018 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2018-2020 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -39,6 +39,30 @@
 using namespace time_literals;
 using matrix::Vector3f;
 
+static inline int32_t sum(const int16_t samples[16], uint8_t len)
+{
+	int32_t sum = 0;
+
+	for (int n = 0; n < len; n++) {
+		sum += samples[n];
+	}
+
+	return sum;
+}
+
+static inline unsigned clipping(const int16_t samples[16], int16_t clip_limit, uint8_t len)
+{
+	unsigned clip_count = 0;
+
+	for (int n = 0; n < len; n++) {
+		if (abs(samples[n]) > clip_limit) {
+			clip_count++;
+		}
+	}
+
+	return clip_count;
+}
+
 PX4Gyroscope::PX4Gyroscope(uint32_t device_id, uint8_t priority, enum Rotation rotation) :
 	CDev(nullptr),
 	ModuleParams(nullptr),
@@ -47,7 +71,8 @@ PX4Gyroscope::PX4Gyroscope(uint32_t device_id, uint8_t priority, enum Rotation r
 	_sensor_integrated_pub{ORB_ID(sensor_gyro_integrated), priority},
 	_sensor_status_pub{ORB_ID(sensor_gyro_status), priority},
 	_device_id{device_id},
-	_rotation{rotation}
+	_rotation{rotation},
+	_rotation_dcm{get_rot_matrix(rotation)}
 {
 	_class_device_instance = register_class_devname(GYRO_BASE_DEVICE_PATH);
 
@@ -120,10 +145,8 @@ void PX4Gyroscope::update(hrt_abstime timestamp_sample, float x, float y, float
 	const Vector3f raw{x, y, z};
 
 	// Clipping (check unscaled raw values)
-	const float clip_limit = (_range / _scale) * 0.95f;
-
 	for (int i = 0; i < 3; i++) {
-		if (fabsf(raw(i)) > clip_limit) {
+		if (fabsf(raw(i)) > _clip_limit) {
 			_clipping[i]++;
 			_integrator_clipping++;
 		}
@@ -156,10 +179,6 @@ void PX4Gyroscope::update(hrt_abstime timestamp_sample, float x, float y, float
 	Vector3f delta_angle;
 	uint32_t integral_dt = 0;
 
-	if (_integrator_samples == 0) {
-		_integrator_timestamp_sample = timestamp_sample;
-	}
-
 	_integrator_samples++;
 
 	if (_integrator.put(timestamp_sample, val_calibrated, delta_angle, integral_dt)) {
@@ -167,7 +186,7 @@ void PX4Gyroscope::update(hrt_abstime timestamp_sample, float x, float y, float
 		// fill sensor_gyro_integrated and publish
 		sensor_gyro_integrated_s report{};
 
-		report.timestamp_sample = _integrator_timestamp_sample;
+		report.timestamp_sample = timestamp_sample;
 		report.error_count = _error_count;
 		report.device_id = _device_id;
 		delta_angle.copyTo(report.delta_angle);
@@ -191,18 +210,19 @@ void PX4Gyroscope::update(hrt_abstime timestamp_sample, float x, float y, float
 
 void PX4Gyroscope::updateFIFO(const FIFOSample &sample)
 {
+	const uint8_t N = sample.samples;
+	const float dt = sample.dt;
+
 	// filtered data (control)
-	float x_filtered = _filterArrayX.apply(sample.x, sample.samples);
-	float y_filtered = _filterArrayY.apply(sample.y, sample.samples);
-	float z_filtered = _filterArrayZ.apply(sample.z, sample.samples);
+	float x_filtered = _filterArrayX.apply(sample.x, N);
+	float y_filtered = _filterArrayY.apply(sample.y, N);
+	float z_filtered = _filterArrayZ.apply(sample.z, N);
 
 	// Apply rotation (before scaling)
 	rotate_3f(_rotation, x_filtered, y_filtered, z_filtered);
 
-	const Vector3f raw{x_filtered, y_filtered, z_filtered};
-
-	// Apply range scale and the calibrating offset/scale
-	const Vector3f val_calibrated{(raw * _scale) - _calibration_offset};
+	// Apply range scale and the calibration offset
+	const Vector3f val_calibrated{(Vector3f{x_filtered, y_filtered, z_filtered} * _scale) - _calibration_offset};
 
 
 	// publish control data (filtered) immediately
@@ -220,7 +240,7 @@ void PX4Gyroscope::updateFIFO(const FIFOSample &sample)
 		if (publish_control) {
 			sensor_gyro_s report{};
 
-			report.timestamp_sample = sample.timestamp_sample + ((sample.samples - 1) * sample.dt); // timestamp of last sample
+			report.timestamp_sample = sample.timestamp_sample;
 			report.device_id = _device_id;
 			report.temperature = _temperature;
 			report.x = val_calibrated(0);
@@ -236,88 +256,59 @@ void PX4Gyroscope::updateFIFO(const FIFOSample &sample)
 
 
 	// clipping
-	const int16_t clip_limit = (_range / _scale) * 0.95f;
+	unsigned clip_count_x = clipping(sample.x, _clip_limit, N);
+	unsigned clip_count_y = clipping(sample.y, _clip_limit, N);
+	unsigned clip_count_z = clipping(sample.z, _clip_limit, N);
 
-	// x clipping
-	for (int n = 0; n < sample.samples; n++) {
-		if (abs(sample.x[n]) > clip_limit) {
-			_clipping[0]++;
-			_integrator_clipping++;
-		}
-	}
-
-	// y clipping
-	for (int n = 0; n < sample.samples; n++) {
-		if (abs(sample.y[n]) > clip_limit) {
-			_clipping[1]++;
-			_integrator_clipping++;
-		}
-	}
-
-	// z clipping
-	for (int n = 0; n < sample.samples; n++) {
-		if (abs(sample.z[n]) > clip_limit) {
-			_clipping[2]++;
-			_integrator_clipping++;
-		}
-	}
+	_clipping[0] += clip_count_x;
+	_clipping[1] += clip_count_y;
+	_clipping[2] += clip_count_z;
 
+	_integrator_clipping += clip_count_x + clip_count_y + clip_count_z;
 
 	// integrated data (INS)
 	{
 		// reset integrator if previous sample was too long ago
 		if ((sample.timestamp_sample > _timestamp_sample_prev)
-		    && ((sample.timestamp_sample - _timestamp_sample_prev) > (sample.samples * sample.dt * 2))) {
+		    && ((sample.timestamp_sample - _timestamp_sample_prev) > (N * dt * 2.0f))) {
 
 			ResetIntegrator();
 		}
 
-		if (_integrator_samples == 0) {
-			_integrator_timestamp_sample = sample.timestamp_sample;
-		}
-
 		// integrate
 		_integrator_samples += 1;
-		_integrator_fifo_samples += sample.samples;
-
-		for (int n = 0; n < sample.samples; n++) {
-			_integrator_accum[0] += sample.x[n];
-		}
+		_integrator_fifo_samples += N;
 
-		for (int n = 0; n < sample.samples; n++) {
-			_integrator_accum[1] += sample.y[n];
-		}
+		// trapezoidal integration (equally spaced, scaled by dt later)
+		_integration_raw(0) += (0.5f * (_last_sample[0] + sample.x[N - 1]) + sum(sample.x, N - 1));
+		_integration_raw(1) += (0.5f * (_last_sample[1] + sample.y[N - 1]) + sum(sample.y, N - 1));
+		_integration_raw(2) += (0.5f * (_last_sample[2] + sample.z[N - 1]) + sum(sample.z, N - 1));
+		_last_sample[0] = sample.x[N - 1];
+		_last_sample[1] = sample.y[N - 1];
+		_last_sample[2] = sample.z[N - 1];
 
-		for (int n = 0; n < sample.samples; n++) {
-			_integrator_accum[2] += sample.z[n];
-		}
 
 		if (_integrator_fifo_samples > 0 && (_integrator_samples >= _integrator_reset_samples)) {
 
-			const uint32_t integrator_dt_us = _integrator_fifo_samples * sample.dt; // time span in microseconds
+			// Apply rotation and scale
+			// integrated in microseconds, convert to seconds
+			const Vector3f delta_angle_uncalibrated{_rotation_dcm *_integration_raw * _scale};
 
-			// average integrated values to apply calibration
-			float x_int_avg = _integrator_accum[0] / _integrator_fifo_samples;
-			float y_int_avg = _integrator_accum[1] / _integrator_fifo_samples;
-			float z_int_avg = _integrator_accum[2] / _integrator_fifo_samples;
+			// scale calibration offset to number of samples
+			const Vector3f offset{_calibration_offset * _integrator_fifo_samples};
 
-			// Apply rotation (before scaling)
-			rotate_3f(_rotation, x_int_avg, y_int_avg, z_int_avg);
-
-			const Vector3f raw_int{x_int_avg, y_int_avg, z_int_avg};
-
-			// Apply range scale and the calibrating offset/scale
-			Vector3f delta_angle{(raw_int * _scale) - _calibration_offset};
-			delta_angle *= (_integrator_fifo_samples * sample.dt * 1e-6f);	// restore
+			// Apply calibration and scale to seconds
+			Vector3f delta_angle{delta_angle_uncalibrated - offset};
+			delta_angle *= 1e-6f * dt;
 
 			// fill sensor_gyro_integrated and publish
 			sensor_gyro_integrated_s report{};
 
-			report.timestamp_sample = _integrator_timestamp_sample;
+			report.timestamp_sample = sample.timestamp_sample;
 			report.error_count = _error_count;
 			report.device_id = _device_id;
 			delta_angle.copyTo(report.delta_angle);
-			report.dt = integrator_dt_us;
+			report.dt = _integrator_fifo_samples * dt; // time span in microseconds
 			report.samples = _integrator_fifo_samples;
 			report.clip_count = _integrator_clipping;
 
@@ -339,13 +330,13 @@ void PX4Gyroscope::updateFIFO(const FIFOSample &sample)
 
 	fifo.device_id = _device_id;
 	fifo.timestamp_sample = sample.timestamp_sample;
-	fifo.dt = sample.dt;
+	fifo.dt = dt;
 	fifo.scale = _scale;
-	fifo.samples = sample.samples;
+	fifo.samples = N;
 
-	memcpy(fifo.x, sample.x, sizeof(sample.x[0]) * sample.samples);
-	memcpy(fifo.y, sample.y, sizeof(sample.y[0]) * sample.samples);
-	memcpy(fifo.z, sample.z, sizeof(sample.z[0]) * sample.samples);
+	memcpy(fifo.x, sample.x, sizeof(sample.x[0]) * N);
+	memcpy(fifo.y, sample.y, sizeof(sample.y[0]) * N);
+	memcpy(fifo.z, sample.z, sizeof(sample.z[0]) * N);
 
 	fifo.timestamp = hrt_absolute_time();
 	_sensor_fifo_pub.publish(fifo);
@@ -383,12 +374,9 @@ void PX4Gyroscope::ResetIntegrator()
 {
 	_integrator_samples = 0;
 	_integrator_fifo_samples = 0;
-	_integrator_accum[0] = 0;
-	_integrator_accum[1] = 0;
-	_integrator_accum[2] = 0;
+	_integration_raw.zero();
 	_integrator_clipping = 0;
 
-	_integrator_timestamp_sample = 0;
 	_timestamp_sample_prev = 0;
 }
 
@@ -406,6 +394,13 @@ void PX4Gyroscope::ConfigureNotchFilter(float notch_freq, float bandwidth)
 	_notch_filter.setParameters(_sample_rate, notch_freq, bandwidth);
 }
 
+void PX4Gyroscope::UpdateClipLimit()
+{
+	// 95% of potential max
+	_clip_limit = (_range / _scale) * 0.95f;
+}
+
+
 void PX4Gyroscope::UpdateVibrationMetrics(const Vector3f &delta_angle)
 {
 	// Gyro high frequency vibe = filtered length of (delta_angle - prev_delta_angle)

src/lib/drivers/gyroscope/PX4Gyroscope.hpp

@@ -61,9 +61,9 @@ public:
 	void set_device_id(uint32_t device_id) { _device_id = device_id; }
 	void set_device_type(uint8_t devtype);
 	void set_error_count(uint64_t error_count) { _error_count += error_count; }
-	void set_range(float range) { _range = range; }
+	void set_range(float range) { _range = range; UpdateClipLimit(); }
 	void set_sample_rate(uint16_t rate);
-	void set_scale(float scale) { _scale = scale; }
+	void set_scale(float scale) { _scale = scale; UpdateClipLimit(); }
 	void set_temperature(float temperature) { _temperature = temperature; }
 	void set_update_rate(uint16_t rate);
 
@@ -92,6 +92,7 @@ private:
 	void ConfigureNotchFilter(float notch_freq, float bandwidth);
 	void PublishStatus();
 	void ResetIntegrator();
+	void UpdateClipLimit();
 	void UpdateVibrationMetrics(const matrix::Vector3f &delta_angle);
 
 	uORB::PublicationMulti<sensor_gyro_s>            _sensor_pub;
@@ -111,20 +112,23 @@ private:
 
 	Integrator		_integrator{4000, true};
 
-	matrix::Vector3f	_calibration_offset{0.0f, 0.0f, 0.0f};
+	matrix::Vector3f	_calibration_offset{0.f, 0.f, 0.f};
 
-	matrix::Vector3f _delta_angle_prev{0.0f, 0.0f, 0.0f};	// delta angle from the previous IMU measurement
-	float _vibration_metric{0.0f};	// high frequency vibration level in the IMU delta angle data (rad)
-	float _coning_vibration{0.0f};	// Level of coning vibration in the IMU delta angles (rad^2)
+	matrix::Vector3f _delta_angle_prev{0.f, 0.f, 0.f};	// delta angle from the previous IMU measurement
+	float _vibration_metric{0.f};	// high frequency vibration level in the IMU delta angle data (rad)
+	float _coning_vibration{0.f};	// Level of coning vibration in the IMU delta angles (rad^2)
 
 	int			_class_device_instance{-1};
 
 	uint32_t		_device_id{0};
 	const enum Rotation	_rotation;
+	const matrix::Dcmf	_rotation_dcm;
 
-	float			_range{math::radians(2000.0f)};
-	float			_scale{1.0f};
-	float			_temperature{0.0f};
+	float			_range{math::radians(2000.f)};
+	float			_scale{1.f};
+	float			_temperature{0.f};
+
+	int16_t			_clip_limit{(int16_t)(_range / _scale)};
 
 	uint64_t		_error_count{0};
 
@@ -134,9 +138,9 @@ private:
 	uint16_t		_update_rate{1000};
 
 	// integrator
-	hrt_abstime		_integrator_timestamp_sample{0};
 	hrt_abstime		_timestamp_sample_prev{0};
-	float			_integrator_accum[3] {};
+	matrix::Vector3f	_integration_raw{};
+	int16_t			_last_sample[3] {};
 	uint8_t			_integrator_reset_samples{4};
 	uint8_t			_integrator_samples{0};
 	uint8_t			_integrator_fifo_samples{0};