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)

boards/nxp/fmurt1062-v1/default.cmake

@@ -71,7 +71,7 @@ px4_add_board(
 		navigator
 		rc_update
 		sensors
-		sih
+		#sih
 		temperature_compensation
 		#vmount
 	SYSTEMCMDS
@@ -94,6 +94,7 @@ px4_add_board(
 		reboot
 		reflect
 		sd_bench
+		#serial_test
 		system_time
 		top
 		topic_listener
@@ -102,7 +103,6 @@ px4_add_board(
 		usb_connected
 		ver
 		work_queue
-		serial_test
 	EXAMPLES
 ##		fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control
 #		hello

msg/sensor_accel_fifo.msg

@@ -11,5 +11,3 @@ uint8 samples             # number of valid samples
 int16[32] x               # acceleration in the FRD board frame X-axis in m/s^2
 int16[32] y               # acceleration in the FRD board frame Y-axis in m/s^2
 int16[32] z               # acceleration in the FRD board frame Z-axis in m/s^2
-
-uint8 rotation            # Direction the sensor faces (see Rotation enum)

msg/sensor_gyro_fifo.msg

@@ -12,6 +12,4 @@ int16[32] x               # angular velocity in the FRD board frame X-axis in ra
 int16[32] y               # angular velocity in the FRD board frame Y-axis in rad/s
 int16[32] z               # angular velocity in the FRD board frame Z-axis in rad/s
 
-uint8 rotation            # Direction the sensor faces (see Rotation enum)
-
 uint8 ORB_QUEUE_LENGTH = 4

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 float vector in-place
+ * rotate a 3 element int16_t vector in-place
  */
-__EXPORT void
-rotate_3f(enum Rotation rot, float &x, float &y, float &z);
-
+__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);
+		}
+	}
+}
 
-#endif /* ROTATION_H_ */
+/**
+ * rotate a 3 element float vector in-place
+ */
+__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;
 
@@ -95,29 +95,44 @@ 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);
+	// Apply rotation (before scaling)
+	rotate_3f(_rotation, x, y, z);
 
 	// publish
-	Publish(timestamp_sample, x, y, z, clip_count);
+	sensor_accel_s report;
+
+	report.timestamp_sample = timestamp_sample;
+	report.device_id = _device_id;
+	report.temperature = _temperature;
+	report.error_count = _error_count;
+	report.x = x * _scale;
+	report.y = y * _scale;
+	report.z = z * _scale;
+	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)
 {
-	// publish fifo
+	// 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.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)),
@@ -128,46 +143,22 @@ void PX4Accelerometer::updateFIFO(sensor_accel_fifo_s &sample)
 	_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;
+	const float scale = _scale / (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);
-
 	sensor_accel_s report;
-
-	report.timestamp_sample = timestamp_sample;
+	report.timestamp_sample = sample.timestamp_sample;
 	report.device_id = _device_id;
 	report.temperature = _temperature;
 	report.error_count = _error_count;
-	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.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;
 
@@ -82,23 +82,40 @@ 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);
+	// Apply rotation (before scaling)
+	rotate_3f(_rotation, x, y, z);
+
+	sensor_gyro_s report;
+
+	report.timestamp_sample = timestamp_sample;
+	report.device_id = _device_id;
+	report.temperature = _temperature;
+	report.error_count = _error_count;
+	report.x = x * _scale;
+	report.y = y * _scale;
+	report.z = z * _scale;
+	report.samples = 1;
+	report.timestamp = hrt_absolute_time();
+
+	_sensor_pub.publish(report);
 }
 
 void PX4Gyroscope::updateFIFO(sensor_gyro_fifo_s &sample)
 {
-	// publish fifo
+	// 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.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)),
@@ -109,30 +126,19 @@ void PX4Gyroscope::updateFIFO(sensor_gyro_fifo_s &sample)
 	_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);
+	const float scale = _scale / N;
 
 	sensor_gyro_s report;
 
-	report.timestamp_sample = timestamp_sample;
+	report.timestamp_sample = sample.timestamp_sample;
 	report.device_id = _device_id;
 	report.temperature = _temperature;
 	report.error_count = _error_count;
-	report.x = x * _scale;
-	report.y = y * _scale;
-	report.z = z * _scale;
-	report.samples = samples;
+	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();
 

src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp

@@ -330,6 +330,18 @@ void VehicleAngularVelocity::ParametersUpdate(bool force)
 	}
 }
 
+Vector3f VehicleAngularVelocity::GetResetAngularVelocity() const
+{
+	if (_fifo_available && (_last_scale > 0.f)) {
+		return _calibration.Uncorrect(_angular_velocity + _bias) / _last_scale;
+
+	} else if (!_fifo_available) {
+		return _angular_velocity;
+	}
+
+	return Vector3f{0.f, 0.f, 0.f};
+}
+
 void VehicleAngularVelocity::DisableDynamicNotchEscRpm()
 {
 #if !defined(CONSTRAINED_FLASH)
@@ -395,10 +407,12 @@ void VehicleAngularVelocity::UpdateDynamicNotchEscRpm(bool force)
 							}
 
 							// only reset if there's sufficient change (> 1%)
-							if ((change_percent > 0.01f) && (_last_scale > 0.f)) {
+							if (change_percent > 0.01f) {
+								const Vector3f reset_angular_velocity{GetResetAngularVelocity()};
+
 								for (int axis = 0; axis < 3; axis++) {
 									auto &dnf = _dynamic_notch_filter_esc_rpm[i][harmonic][axis];
-									dnf.reset(_angular_velocity(axis) / _last_scale);
+									dnf.reset(reset_angular_velocity(axis));
 								}
 							}
 
@@ -451,9 +465,9 @@ void VehicleAngularVelocity::UpdateDynamicNotchFFT(bool force)
 							// peak frequency changed by at least 0.1%
 							dnf.setParameters(_filter_sample_rate_hz, peak_freq, sensor_gyro_fft.resolution_hz);
 
-							// only reset if there's sufficient change (> 1%)
-							if ((peak_diff_abs > sensor_gyro_fft.resolution_hz) && (_last_scale > 0.f)) {
-								dnf.reset(_angular_velocity(axis) / _last_scale);
+							// only reset if there's sufficient change
+							if (peak_diff_abs > sensor_gyro_fft.resolution_hz) {
+								dnf.reset(GetResetAngularVelocity()(axis));
 							}
 
 							perf_count(_dynamic_notch_filter_fft_update_perf);
@@ -500,15 +514,16 @@ void VehicleAngularVelocity::Run()
 
 				const int N = sensor_fifo_data.samples;
 				const float dt_s = sensor_fifo_data.dt * 1e-6f;
-				const enum Rotation fifo_rotation = static_cast<enum Rotation>(sensor_fifo_data.rotation);
 
 				if (_reset_filters || (fabsf(sensor_fifo_data.scale - _last_scale) > FLT_EPSILON)) {
 					if (UpdateSampleRate()) {
 						// in FIFO mode the unscaled raw data is filtered
-						_angular_velocity_prev = _angular_velocity / sensor_fifo_data.scale;
-						ResetFilters(_angular_velocity_prev, _angular_acceleration / sensor_fifo_data.scale);
-
 						_last_scale = sensor_fifo_data.scale;
+
+						_angular_velocity_prev = GetResetAngularVelocity();
+						Vector3f angular_acceleration{_calibration.rotation().I() *_angular_acceleration / _last_scale};
+
+						ResetFilters(_angular_velocity_prev, angular_acceleration);
 					}
 
 					if (_reset_filters) {
@@ -592,16 +607,13 @@ void VehicleAngularVelocity::Run()
 				}
 
 				// Angular velocity: rotate sensor frame to board, scale raw data to SI, apply calibration, and remove in-run estimated bias
-				rotate_3f(fifo_rotation, angular_velocity_unscaled(0), angular_velocity_unscaled(1), angular_velocity_unscaled(2));
 				_angular_velocity = _calibration.Correct(angular_velocity_unscaled * sensor_fifo_data.scale) - _bias;
 
 				// Angular acceleration: rotate sensor frame to board, scale raw data to SI, apply any additional configured rotation
-				rotate_3f(fifo_rotation, angular_acceleration_unscaled(0), angular_acceleration_unscaled(1),
-					  angular_acceleration_unscaled(2));
 				_angular_acceleration = _calibration.rotation() * angular_acceleration_unscaled * sensor_fifo_data.scale;
 
 				// Publish
-				if (!_sensor_fifo_sub.updated() && (sensor_fifo_data.timestamp_sample - _last_publish >= _publish_interval_min_us)) {
+				if (!_sensor_fifo_sub.updated()) {
 					Publish(sensor_fifo_data.timestamp_sample);
 				}
 			}
@@ -683,7 +695,7 @@ void VehicleAngularVelocity::Run()
 			_angular_velocity = angular_velocity;
 
 			// Publish
-			if (!_sensor_sub.updated() && (sensor_data.timestamp_sample - _last_publish >= _publish_interval_min_us)) {
+			if (!_sensor_sub.updated()) {
 				Publish(sensor_data.timestamp_sample);
 			}
 		}
@@ -692,6 +704,7 @@ void VehicleAngularVelocity::Run()
 
 void VehicleAngularVelocity::Publish(const hrt_abstime &timestamp_sample)
 {
+	if (timestamp_sample >= _last_publish + _publish_interval_min_us) {
 		// Publish vehicle_angular_acceleration
 		vehicle_angular_acceleration_s v_angular_acceleration;
 		v_angular_acceleration.timestamp_sample = timestamp_sample;
@@ -710,6 +723,7 @@ void VehicleAngularVelocity::Publish(const hrt_abstime &timestamp_sample)
 		_last_publish = math::constrain(_last_publish + _publish_interval_min_us,
 						timestamp_sample - _publish_interval_min_us, timestamp_sample);
 	}
+}
 
 void VehicleAngularVelocity::PrintStatus()
 {

src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp

@@ -86,6 +86,9 @@ private:
 	void UpdateDynamicNotchFFT(bool force = false);
 	bool UpdateSampleRate();
 
+	// scaled appropriately for current FIFO mode
+	matrix::Vector3f GetResetAngularVelocity() const;
+
 	static constexpr int MAX_SENSOR_COUNT = 4;
 
 	uORB::Publication<vehicle_angular_acceleration_s> _vehicle_angular_acceleration_pub{ORB_ID(vehicle_angular_acceleration)};