rotate accel/gyro FIFO before publish and fix angular velocity filter resets

- rotates accel & gyro FIFO data before publication both to simplify downstream usage (including log review) and fix other issues
     - to best handle int16_t data rotations are now either performed with swaps if possible, otherwise promoted to float, rotated using the full rotation matrix, then rounded back to int16_t
 - fix sensors/vehicle_angular_velocity filter reset both with proper rotation and new calibration uncorrect helper
      - in FIFO case filtering is done before calibration is applied, but we need to handle a possible reset from a completely different sensor (vehicle body angular velocity -> sensor frame uncorrected data)

src/lib/drivers/gyroscope/PX4Gyroscope.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2018-2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2018-2021 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
@@ -40,7 +40,7 @@
 using namespace time_literals;
 using matrix::Vector3f;
 
-static constexpr int32_t sum(const int16_t samples[16], uint8_t len)
+static constexpr int32_t sum(const int16_t samples[], uint8_t len)
 {
 	int32_t sum = 0;
 
@@ -81,44 +81,6 @@ void PX4Gyroscope::set_device_type(uint8_t devtype)
 }
 
 void PX4Gyroscope::update(const hrt_abstime &timestamp_sample, float x, float y, float z)
-{
-	// publish
-	Publish(timestamp_sample, x, y, z);
-}
-
-void PX4Gyroscope::updateFIFO(sensor_gyro_fifo_s &sample)
-{
-	// publish fifo
-	sample.device_id = _device_id;
-	sample.scale = _scale;
-	sample.rotation = _rotation;
-
-	sample.timestamp = hrt_absolute_time();
-	_sensor_fifo_pub.publish(sample);
-
-	{
-		// trapezoidal integration (equally spaced, scaled by dt later)
-		const uint8_t N = sample.samples;
-		const Vector3f integral{
-			(0.5f * (_last_sample[0] + sample.x[N - 1]) + sum(sample.x, N - 1)),
-			(0.5f * (_last_sample[1] + sample.y[N - 1]) + sum(sample.y, N - 1)),
-			(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];
-
-		const float x = integral(0) / (float)N;
-		const float y = integral(1) / (float)N;
-		const float z = integral(2) / (float)N;
-
-		// publish
-		Publish(sample.timestamp_sample, x, y, z, N);
-	}
-}
-
-void PX4Gyroscope::Publish(const hrt_abstime &timestamp_sample, float x, float y, float z, uint8_t samples)
 {
 	// Apply rotation (before scaling)
 	rotate_3f(_rotation, x, y, z);
@@ -132,7 +94,51 @@ void PX4Gyroscope::Publish(const hrt_abstime &timestamp_sample, float x, float y
 	report.x = x * _scale;
 	report.y = y * _scale;
 	report.z = z * _scale;
-	report.samples = samples;
+	report.samples = 1;
+	report.timestamp = hrt_absolute_time();
+
+	_sensor_pub.publish(report);
+}
+
+void PX4Gyroscope::updateFIFO(sensor_gyro_fifo_s &sample)
+{
+	// rotate all raw samples and publish fifo
+	const uint8_t N = sample.samples;
+
+	for (int n = 0; n < N; n++) {
+		rotate_3i(_rotation, sample.x[n], sample.y[n], sample.z[n]);
+	}
+
+	sample.device_id = _device_id;
+	sample.scale = _scale;
+	sample.timestamp = hrt_absolute_time();
+	_sensor_fifo_pub.publish(sample);
+
+
+	// trapezoidal integration (equally spaced, scaled by dt later)
+	const Vector3f integral{
+		(0.5f * (_last_sample[0] + sample.x[N - 1]) + sum(sample.x, N - 1)),
+		(0.5f * (_last_sample[1] + sample.y[N - 1]) + sum(sample.y, N - 1)),
+		(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];
+
+
+	const float scale = _scale / N;
+
+	sensor_gyro_s report;
+
+	report.timestamp_sample = sample.timestamp_sample;
+	report.device_id = _device_id;
+	report.temperature = _temperature;
+	report.error_count = _error_count;
+	report.x = integral(0) * scale;
+	report.y = integral(1) * scale;
+	report.z = integral(2) * scale;
+	report.samples = N;
 	report.timestamp = hrt_absolute_time();
 
 	_sensor_pub.publish(report);

src/lib/drivers/gyroscope/PX4Gyroscope.hpp

@@ -1,7 +1,7 @@
 
 /****************************************************************************
  *
- *   Copyright (c) 2018-2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2018-2021 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
@@ -65,8 +65,6 @@ public:
 	int get_instance() { return _sensor_pub.get_instance(); };
 
 private:
-	void Publish(const hrt_abstime &timestamp_sample, float x, float y, float z, uint8_t samples = 1);
-
 	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)};