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/conversion/rotation.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *   Copyright (C) 2013-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
@@ -56,202 +56,3 @@ get_rot_quaternion(enum Rotation rot)
 			math::radians((float)rot_lookup[rot].pitch),
 			math::radians((float)rot_lookup[rot].yaw)}};
 }
-
-__EXPORT void
-rotate_3f(enum Rotation rot, float &x, float &y, float &z)
-{
-	switch (rot) {
-	case ROTATION_NONE:
-		break;
-
-	case ROTATION_YAW_90: {
-			float tmp = x;
-			x = -y;
-			y = tmp;
-		}
-		break;
-
-	case ROTATION_YAW_180: {
-			x = -x;
-			y = -y;
-		}
-		break;
-
-	case ROTATION_YAW_270: {
-			float tmp = x;
-			x = y;
-			y = -tmp;
-		}
-		break;
-
-	case ROTATION_ROLL_180: {
-			y = -y;
-			z = -z;
-		}
-		break;
-
-	case ROTATION_ROLL_180_YAW_90:
-
-	// FALLTHROUGH
-	case ROTATION_PITCH_180_YAW_270: {
-			float tmp = x;
-			x = y;
-			y = tmp;
-			z = -z;
-		}
-		break;
-
-	case ROTATION_PITCH_180: {
-			x = -x;
-			z = -z;
-		}
-		break;
-
-	case ROTATION_ROLL_180_YAW_270:
-
-	// FALLTHROUGH
-	case ROTATION_PITCH_180_YAW_90: {
-			float tmp = x;
-			x = -y;
-			y = -tmp;
-			z = -z;
-		}
-		break;
-
-	case ROTATION_ROLL_90: {
-			float tmp = z;
-			z = y;
-			y = -tmp;
-		}
-		break;
-
-	case ROTATION_ROLL_90_YAW_90: {
-			float tmp = x;
-			x = z;
-			z = y;
-			y = tmp;
-		}
-		break;
-
-	case ROTATION_ROLL_270: {
-			float tmp = z;
-			z = -y;
-			y = tmp;
-		}
-		break;
-
-	case ROTATION_ROLL_270_YAW_90: {
-			float tmp = x;
-			x = -z;
-			z = -y;
-			y = tmp;
-		}
-		break;
-
-	case ROTATION_PITCH_90: {
-			float tmp = z;
-			z = -x;
-			x = tmp;
-		}
-		break;
-
-	case ROTATION_PITCH_270: {
-			float tmp = z;
-			z = x;
-			x = -tmp;
-		}
-		break;
-
-	case ROTATION_ROLL_180_PITCH_270: {
-			float tmp = z;
-			z = x;
-			x = tmp;
-			y = -y;
-		}
-		break;
-
-	case ROTATION_ROLL_90_YAW_270: {
-			float tmp = x;
-			x = -z;
-			z = y;
-			y = -tmp;
-		}
-		break;
-
-	case ROTATION_ROLL_90_PITCH_90: {
-			float tmp = x;
-			x = y;
-			y = -z;
-			z = -tmp;
-		}
-		break;
-
-	case ROTATION_ROLL_180_PITCH_90: {
-			float tmp = x;
-			x = -z;
-			y = -y;
-			z = -tmp;
-		}
-		break;
-
-	case ROTATION_ROLL_270_PITCH_90: {
-			float tmp = x;
-			x = -y;
-			y = z;
-			z = -tmp;
-		}
-		break;
-
-	case ROTATION_ROLL_90_PITCH_180: {
-			float tmp = y;
-			x = -x;
-			y = -z;
-			z = -tmp;
-		}
-		break;
-
-	case ROTATION_ROLL_270_PITCH_180: {
-			float tmp = y;
-			x = -x;
-			y = z;
-			z = tmp;
-		}
-		break;
-
-	case ROTATION_ROLL_90_PITCH_270: {
-			float tmp = x;
-			x = -y;
-			y = -z;
-			z = tmp;
-		}
-		break;
-
-	case ROTATION_ROLL_270_PITCH_270: {
-			float tmp = x;
-			x = y;
-			y = z;
-			z = tmp;
-		}
-		break;
-
-	case ROTATION_ROLL_90_PITCH_180_YAW_90: {
-			float tmp = x;
-			x = z;
-			z = -y;
-			y = -tmp;
-		}
-		break;
-
-	default:
-
-		// otherwise use full rotation matrix for valid rotations
-		if (rot < ROTATION_MAX) {
-			const matrix::Vector3f r{get_rot_matrix(rot) *matrix::Vector3f{x, y, z}};
-			x = r(0);
-			y = r(1);
-			z = r(2);
-		}
-
-		break;
-	}
-}

src/lib/conversion/rotation.h

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2013-2020 PX4 Development Team. All rights reserved.
+ *   Copyright (C) 2013-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
@@ -37,8 +37,7 @@
  * Vector rotation library
  */
 
-#ifndef ROTATION_H_
-#define ROTATION_H_
+#pragma once
 
 #include <stdint.h>
 
@@ -149,20 +148,256 @@ static constexpr rot_lookup_t rot_lookup[ROTATION_MAX] = {
 /**
  * Get the rotation matrix
  */
-__EXPORT matrix::Dcmf
-get_rot_matrix(enum Rotation rot);
+__EXPORT matrix::Dcmf get_rot_matrix(enum Rotation rot);
 
 /**
  * Get the rotation quaternion
  */
-__EXPORT matrix::Quatf
-get_rot_quaternion(enum Rotation rot);
+__EXPORT matrix::Quatf get_rot_quaternion(enum Rotation rot);
+
+template<typename T>
+static constexpr bool rotate_3(enum Rotation rot, T &x, T &y, T &z)
+{
+	switch (rot) {
+	case ROTATION_NONE:
+		return true;
+
+	case ROTATION_YAW_90: {
+			T tmp = x;
+			x = math::negate(y);
+			y = tmp;
+		}
+
+		return true;
+
+	case ROTATION_YAW_180: {
+			x = math::negate(x);
+			y = math::negate(y);
+		}
+
+		return true;
+
+	case ROTATION_YAW_270: {
+			T tmp = x;
+			x = y;
+			y = math::negate(tmp);
+		}
+
+		return true;
+
+	case ROTATION_ROLL_180: {
+			y = math::negate(y);
+			z = math::negate(z);
+		}
+
+		return true;
+
+	case ROTATION_ROLL_180_YAW_90:
+
+	// FALLTHROUGH
+	case ROTATION_PITCH_180_YAW_270: {
+			T tmp = x;
+			x = y;
+			y = tmp;
+			z = math::negate(z);
+		}
+
+		return true;
+
+	case ROTATION_PITCH_180: {
+			x = math::negate(x);
+			z = math::negate(z);
+		}
+
+		return true;
+
+	case ROTATION_ROLL_180_YAW_270:
+
+	// FALLTHROUGH
+	case ROTATION_PITCH_180_YAW_90: {
+			T tmp = x;
+			x = math::negate(y);
+			y = math::negate(tmp);
+			z = math::negate(z);
+		}
+
+		return true;
+
+	case ROTATION_ROLL_90: {
+			T tmp = z;
+			z = y;
+			y = math::negate(tmp);
+		}
+
+		return true;
+
+	case ROTATION_ROLL_90_YAW_90: {
+			T tmp = x;
+			x = z;
+			z = y;
+			y = tmp;
+		}
+
+		return true;
+
+	case ROTATION_ROLL_270: {
+			T tmp = z;
+			z = math::negate(y);
+			y = tmp;
+		}
+
+		return true;
+
+	case ROTATION_ROLL_270_YAW_90: {
+			T tmp = x;
+			x = math::negate(z);
+			z = math::negate(y);
+			y = tmp;
+		}
+
+		return true;
+
+	case ROTATION_PITCH_90: {
+			T tmp = z;
+			z = math::negate(x);
+			x = tmp;
+		}
+
+		return true;
+
+	case ROTATION_PITCH_270: {
+			T tmp = z;
+			z = x;
+			x = math::negate(tmp);
+		}
+
+		return true;
+
+	case ROTATION_ROLL_180_PITCH_270: {
+			T tmp = z;
+			z = x;
+			x = tmp;
+			y = math::negate(y);
+		}
+
+		return true;
+
+	case ROTATION_ROLL_90_YAW_270: {
+			T tmp = x;
+			x = math::negate(z);
+			z = y;
+			y = math::negate(tmp);
+		}
+
+		return true;
+
+	case ROTATION_ROLL_90_PITCH_90: {
+			T tmp = x;
+			x = y;
+			y = math::negate(z);
+			z = math::negate(tmp);
+		}
+
+		return true;
+
+	case ROTATION_ROLL_180_PITCH_90: {
+			T tmp = x;
+			x = math::negate(z);
+			y = math::negate(y);
+			z = math::negate(tmp);
+		}
+
+		return true;
+
+	case ROTATION_ROLL_270_PITCH_90: {
+			T tmp = x;
+			x = math::negate(y);
+			y = z;
+			z = math::negate(tmp);
+		}
+
+		return true;
+
+	case ROTATION_ROLL_90_PITCH_180: {
+			T tmp = y;
+			x = math::negate(x);
+			y = math::negate(z);
+			z = math::negate(tmp);
+		}
+
+		return true;
+
+	case ROTATION_ROLL_270_PITCH_180: {
+			T tmp = y;
+			x = math::negate(x);
+			y = z;
+			z = tmp;
+		}
+
+		return true;
+
+	case ROTATION_ROLL_90_PITCH_270: {
+			T tmp = x;
+			x = math::negate(y);
+			y = math::negate(z);
+			z = tmp;
+		}
+
+		return true;
+
+	case ROTATION_ROLL_270_PITCH_270: {
+			T tmp = x;
+			x = y;
+			y = z;
+			z = tmp;
+		}
+
+		return true;
+
+	case ROTATION_ROLL_90_PITCH_180_YAW_90: {
+			T tmp = x;
+			x = z;
+			z = math::negate(y);
+			y = math::negate(tmp);
+		}
+
+		return true;
+
+	default:
+		break;
+	}
+
+	return false;
+}
+
+/**
+ * rotate a 3 element int16_t vector in-place
+ */
+__EXPORT inline void rotate_3i(enum Rotation rot, int16_t &x, int16_t &y, int16_t &z)
+{
+	if (!rotate_3(rot, x, y, z)) {
+		// otherwise use full rotation matrix for valid rotations
+		if (rot < ROTATION_MAX) {
+			const matrix::Vector3f r{get_rot_matrix(rot) *matrix::Vector3f{(float)x, (float)y, (float)z}};
+			x = math::constrain(roundf(r(0)), (float)INT16_MIN, (float)INT16_MAX);
+			y = math::constrain(roundf(r(1)), (float)INT16_MIN, (float)INT16_MAX);
+			z = math::constrain(roundf(r(2)), (float)INT16_MIN, (float)INT16_MAX);
+		}
+	}
+}
 
 /**
  * rotate a 3 element float vector in-place
  */
-__EXPORT void
-rotate_3f(enum Rotation rot, float &x, float &y, float &z);
-
-
-#endif /* ROTATION_H_ */
+__EXPORT inline void rotate_3f(enum Rotation rot, float &x, float &y, float &z)
+{
+	if (!rotate_3(rot, x, y, z)) {
+		// otherwise use full rotation matrix for valid rotations
+		if (rot < ROTATION_MAX) {
+			const matrix::Vector3f r{get_rot_matrix(rot) *matrix::Vector3f{x, y, z}};
+			x = r(0);
+			y = r(1);
+			z = r(2);
+		}
+	}
+}

src/lib/drivers/accelerometer/PX4Accelerometer.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;
 
@@ -51,7 +51,7 @@ static constexpr int32_t sum(const int16_t samples[16], uint8_t len)
 	return sum;
 }
 
-static constexpr uint8_t clipping(const int16_t samples[16], int16_t clip_limit, uint8_t len)
+static constexpr uint8_t clipping(const int16_t samples[], int16_t clip_limit, uint8_t len)
 {
 	unsigned clip_count = 0;
 
@@ -94,67 +94,11 @@ void PX4Accelerometer::set_device_type(uint8_t devtype)
 }
 
 void PX4Accelerometer::update(const hrt_abstime &timestamp_sample, float x, float y, float z)
-{
-	// clipping
-	uint8_t clip_count[3];
-	clip_count[0] = (fabsf(x) >= _clip_limit);
-	clip_count[1] = (fabsf(y) >= _clip_limit);
-	clip_count[2] = (fabsf(z) >= _clip_limit);
-
-	// publish
-	Publish(timestamp_sample, x, y, z, clip_count);
-}
-
-void PX4Accelerometer::updateFIFO(sensor_accel_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];
-
-		// clipping
-		uint8_t clip_count[3] {
-			clipping(sample.x, _clip_limit, N),
-			clipping(sample.y, _clip_limit, N),
-			clipping(sample.z, _clip_limit, N),
-		};
-
-		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, clip_count, N);
-	}
-}
-
-void PX4Accelerometer::Publish(const hrt_abstime &timestamp_sample, float x, float y, float z, uint8_t clip_count[3],
-			       uint8_t samples)
 {
 	// Apply rotation (before scaling)
 	rotate_3f(_rotation, x, y, z);
 
-	float clipping_x = clip_count[0];
-	float clipping_y = clip_count[1];
-	float clipping_z = clip_count[2];
-	rotate_3f(_rotation, clipping_x, clipping_y, clipping_z);
-
+	// publish
 	sensor_accel_s report;
 
 	report.timestamp_sample = timestamp_sample;
@@ -164,10 +108,57 @@ void PX4Accelerometer::Publish(const hrt_abstime &timestamp_sample, float x, flo
 	report.x = x * _scale;
 	report.y = y * _scale;
 	report.z = z * _scale;
-	report.clip_counter[0] = fabsf(roundf(clipping_x));
-	report.clip_counter[1] = fabsf(roundf(clipping_y));
-	report.clip_counter[2] = fabsf(roundf(clipping_z));
-	report.samples = samples;
+	report.clip_counter[0] = (fabsf(x) >= _clip_limit);
+	report.clip_counter[1] = (fabsf(y) >= _clip_limit);
+	report.clip_counter[2] = (fabsf(z) >= _clip_limit);
+	report.samples = 1;
+	report.timestamp = hrt_absolute_time();
+
+	_sensor_pub.publish(report);
+}
+
+void PX4Accelerometer::updateFIFO(sensor_accel_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 / (float)N;
+
+	// publish
+	sensor_accel_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.clip_counter[0] = clipping(sample.x, _clip_limit, N);
+	report.clip_counter[1] = clipping(sample.y, _clip_limit, N);
+	report.clip_counter[2] = clipping(sample.z, _clip_limit, N);
+	report.samples = N;
 	report.timestamp = hrt_absolute_time();
 
 	_sensor_pub.publish(report);
@@ -176,5 +167,5 @@ void PX4Accelerometer::Publish(const hrt_abstime &timestamp_sample, float x, flo
 void PX4Accelerometer::UpdateClipLimit()
 {
 	// 99.9% of potential max
-	_clip_limit = fmaxf((_range / _scale) * 0.999f, INT16_MAX);
+	_clip_limit = math::constrain((_range / _scale) * 0.999f, 0.f, (float)INT16_MAX);
 }

src/lib/drivers/accelerometer/PX4Accelerometer.hpp

@@ -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
@@ -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 clip_count[3],
-		     uint8_t samples = 1);
 	void UpdateClipLimit();
 
 	uORB::PublicationMulti<sensor_accel_s> _sensor_pub{ORB_ID(sensor_accel)};

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)};
 

src/lib/mathlib/math/Functions.hpp

@@ -210,4 +210,17 @@ const T lerp(const T &a, const T &b, const T &s)
 	return (static_cast<T>(1) - s) * a + s * b;
 }
 
+template<typename T>
+constexpr T negate(T value)
+{
+	static_assert(sizeof(T) > 2, "implement for T");
+	return -value;
+}
+
+template<>
+constexpr int16_t negate<int16_t>(int16_t value)
+{
+	return (value == INT16_MIN) ? INT16_MAX : -value;
+}
+
 } /* namespace math */

src/lib/sensor_calibration/Gyroscope.hpp

@@ -82,6 +82,11 @@ public:
 		return _rotation * matrix::Vector3f{data - _thermal_offset - _offset};
 	}
 
+	inline matrix::Vector3f Uncorrect(const matrix::Vector3f &corrected_data) const
+	{
+		return (_rotation.I() * corrected_data) + _thermal_offset + _offset;
+	}
+
 	bool ParametersSave();
 	void ParametersUpdate();