Add gyro clipping to mirror accel clipping monitoring instances.

src/lib/drivers/gyroscope/PX4Gyroscope.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2018-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2018-2022 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
@@ -50,6 +50,22 @@ static constexpr int32_t sum(const int16_t samples[], uint8_t len)
 	return sum;
 }
 
+static constexpr uint8_t clipping(const int16_t samples[], uint8_t len)
+{
+	unsigned clip_count = 0;
+
+	for (int n = 0; n < len; n++) {
+		// - consider data clipped/saturated if it's INT16_MIN/INT16_MAX or within 1
+		// - this accommodates rotated data (|INT16_MIN| = INT16_MAX + 1)
+		//   and sensors that may re-use the lowest bit for other purposes (sync indicator, etc)
+		if ((samples[n] <= INT16_MIN + 1) || (samples[n] >= INT16_MAX - 1)) {
+			clip_count++;
+		}
+	}
+
+	return clip_count;
+}
+
 PX4Gyroscope::PX4Gyroscope(uint32_t device_id, enum Rotation rotation) :
 	_device_id{device_id},
 	_rotation{rotation}
@@ -90,6 +106,8 @@ void PX4Gyroscope::set_scale(float scale)
 		}
 
 		_scale = scale;
+
+		UpdateClipLimit();
 	}
 }
 
@@ -107,6 +125,9 @@ void PX4Gyroscope::update(const hrt_abstime &timestamp_sample, float x, float y,
 	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();
 
@@ -145,8 +166,17 @@ void PX4Gyroscope::updateFIFO(sensor_gyro_fifo_s &sample)
 	_last_sample[1] = sample.y[N - 1];
 	_last_sample[2] = sample.z[N - 1];
 
+	report.clip_counter[0] = clipping(sample.x, N);
+	report.clip_counter[1] = clipping(sample.y, N);
+	report.clip_counter[2] = clipping(sample.z, N);
 	report.samples = N;
 	report.timestamp = hrt_absolute_time();
 
 	_sensor_pub.publish(report);
 }
+
+void PX4Gyroscope::UpdateClipLimit()
+{
+	// 99.9% of potential max
+	_clip_limit = math::constrain((_range / _scale) * 0.999f, 0.f, (float)INT16_MAX);
+}

src/lib/drivers/gyroscope/PX4Gyroscope.hpp

@@ -1,7 +1,6 @@
-
 /****************************************************************************
  *
- *   Copyright (c) 2018-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2018-2022 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
@@ -53,7 +52,7 @@ public:
 	void set_device_id(uint32_t device_id) { _device_id = device_id; }
 	void set_device_type(uint8_t devtype);
 	void set_error_count(uint32_t error_count) { _error_count = error_count; }
-	void set_range(float range) { _range = range; }
+	void set_range(float range) { _range = range; UpdateClipLimit(); }
 	void set_scale(float scale);
 	void set_temperature(float temperature) { _temperature = temperature; }
 
@@ -64,6 +63,8 @@ public:
 	int get_instance() { return _sensor_pub.get_instance(); };
 
 private:
+	void UpdateClipLimit();
+
 	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)};
 
@@ -76,6 +77,8 @@ private:
 	float			_scale{1.f};
 	float			_temperature{NAN};
 
+	float			_clip_limit{_range / _scale};
+
 	uint32_t		_error_count{0};
 
 	int16_t			_last_sample[3] {};