Add gyro clipping to mirror accel clipping monitoring instances.

msg/sensor_combined.msg

@@ -2,22 +2,24 @@
 # These fields are scaled and offset-compensated where possible and do not
 # change with board revisions and sensor updates.
 
-uint64 timestamp				# time since system start (microseconds)
+uint64 timestamp                                # time since system start (microseconds)
 
-int32 RELATIVE_TIMESTAMP_INVALID = 2147483647	# (0x7fffffff) If one of the relative timestamps is set to this value, it means the associated sensor values are invalid
+int32 RELATIVE_TIMESTAMP_INVALID = 2147483647   # (0x7fffffff) If one of the relative timestamps is set to this value, it means the associated sensor values are invalid
 
 # gyro timstamp is equal to the timestamp of the message
-float32[3] gyro_rad			# average angular rate measured in the FRD body frame XYZ-axis in rad/s over the last gyro sampling period
-uint32 gyro_integral_dt		# gyro measurement sampling period in microseconds
+float32[3] gyro_rad                     # average angular rate measured in the FRD body frame XYZ-axis in rad/s over the last gyro sampling period
+uint32 gyro_integral_dt                 # gyro measurement sampling period in microseconds
 
-int32 accelerometer_timestamp_relative	# timestamp + accelerometer_timestamp_relative = Accelerometer timestamp
-float32[3] accelerometer_m_s2		# average value acceleration measured in the FRD body frame XYZ-axis in m/s^2 over the last accelerometer sampling period
-uint32 accelerometer_integral_dt	# accelerometer measurement sampling period in microseconds
+int32 accelerometer_timestamp_relative  # timestamp + accelerometer_timestamp_relative = Accelerometer timestamp
+float32[3] accelerometer_m_s2           # average value acceleration measured in the FRD body frame XYZ-axis in m/s^2 over the last accelerometer sampling period
+uint32 accelerometer_integral_dt        # accelerometer measurement sampling period in microseconds
 
 uint8 CLIPPING_X = 1
 uint8 CLIPPING_Y = 2
 uint8 CLIPPING_Z = 4
-uint8 accelerometer_clipping            # bitfield indicating if there was any accelerometer clipping (per axis) during the sampling period
 
-uint8 accel_calibration_count  	# Calibration changed counter. Monotonically increases whenever accelermeter calibration changes.
-uint8 gyro_calibration_count   	# Calibration changed counter. Monotonically increases whenever rate gyro calibration changes.
+uint8 accelerometer_clipping    # bitfield indicating if there was any accelerometer clipping (per axis) during the integration time frame
+uint8 gyro_clipping             # bitfield indicating if there was any gyro clipping (per axis) during the integration time frame
+
+uint8 accel_calibration_count   # Calibration changed counter. Monotonically increases whenever accelermeter calibration changes.
+uint8 gyro_calibration_count    # Calibration changed counter. Monotonically increases whenever rate gyro calibration changes.

msg/sensor_gyro.msg

@@ -11,6 +11,8 @@ float32 temperature       # temperature in degrees Celsius
 
 uint32 error_count
 
+uint8[3] clip_counter     # clip count per axis in the sample period
+
 uint8 samples             # number of raw samples that went into this message
 
 uint8 ORB_QUEUE_LENGTH = 8

msg/vehicle_attitude.msg

@@ -1,12 +1,12 @@
 # This is similar to the mavlink message ATTITUDE_QUATERNION, but for onboard use
 
-uint64 timestamp		    # time since system start (microseconds)
+uint64 timestamp                # time since system start (microseconds)
 
-uint64 timestamp_sample     # the timestamp of the raw data (microseconds)
+uint64 timestamp_sample         # the timestamp of the raw data (microseconds)
 
-float32[4] q			    # Quaternion rotation from the FRD body frame to the NED earth frame
-float32[4] delta_q_reset 	# Amount by which quaternion has changed during last reset
-uint8 quat_reset_counter	# Quaternion reset counter
+float32[4] q                    # Quaternion rotation from the FRD body frame to the NED earth frame
+float32[4] delta_q_reset        # Amount by which quaternion has changed during last reset
+uint8 quat_reset_counter        # Quaternion reset counter
 
 # TOPICS vehicle_attitude vehicle_attitude_groundtruth vehicle_vision_attitude
 # TOPICS estimator_attitude

msg/vehicle_imu.msg

@@ -8,12 +8,15 @@ uint32 gyro_device_id           # Gyroscope unique device ID for the sensor that
 
 float32[3] delta_angle          # delta angle about the FRD body frame XYZ-axis in rad over the integration time frame (delta_angle_dt)
 float32[3] delta_velocity       # delta velocity in the FRD body frame XYZ-axis in m/s over the integration time frame (delta_velocity_dt)
+
 uint16 delta_angle_dt           # integration period in microseconds
 uint16 delta_velocity_dt        # integration period in microseconds
 
 uint8 CLIPPING_X = 1
 uint8 CLIPPING_Y = 2
 uint8 CLIPPING_Z = 4
+
+uint8 delta_angle_clipping     # bitfield indicating if there was any gyro clipping (per axis) during the integration time frame
 uint8 delta_velocity_clipping   # bitfield indicating if there was any accelerometer clipping (per axis) during the integration time frame
 
 uint8 accel_calibration_count  	# Calibration changed counter. Monotonically increases whenever accelermeter calibration changes.

msg/vehicle_imu_status.msg

@@ -4,6 +4,7 @@ uint32 accel_device_id          # unique device ID for the sensor that does not
 uint32 gyro_device_id           # unique device ID for the sensor that does not change between power cycles
 
 uint32[3] accel_clipping        # total clipping per axis
+uint32[3] gyro_clipping         # total clipping per axis
 
 uint32 accel_error_count
 uint32 gyro_error_count

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] {};

src/modules/sensors/vehicle_imu/VehicleIMU.cpp

@@ -407,11 +407,11 @@ bool VehicleIMU::UpdateAccel()
 
 			_publish_status = true;
 
-			if (_accel_calibration.enabled() && (hrt_elapsed_time(&_last_clipping_notify_time) > 3_s)) {
+			if (_accel_calibration.enabled() && (hrt_elapsed_time(&_last_accel_clipping_notify_time) > 3_s)) {
 				// start notifying the user periodically if there's significant continuous clipping
 				const uint64_t clipping_total = _status.accel_clipping[0] + _status.accel_clipping[1] + _status.accel_clipping[2];
 
-				if (clipping_total > _last_clipping_notify_total_count + 1000) {
+				if (clipping_total > _last_accel_clipping_notify_total_count + 1000) {
 					mavlink_log_critical(&_mavlink_log_pub, "Accel %" PRIu8 " clipping, not safe to fly!\t", _instance);
 					/* EVENT
 					 * @description Land now, and check the vehicle setup.
@@ -419,8 +419,8 @@ bool VehicleIMU::UpdateAccel()
 					 */
 					events::send<uint8_t>(events::ID("vehicle_imu_accel_clipping"), events::Log::Critical,
 							      "Accel {1} clipping, not safe to fly!", _instance);
-					_last_clipping_notify_time = accel.timestamp_sample;
-					_last_clipping_notify_total_count = clipping_total;
+					_last_accel_clipping_notify_time = accel.timestamp_sample;
+					_last_accel_clipping_notify_total_count = clipping_total;
 				}
 			}
 		}
@@ -534,6 +534,52 @@ bool VehicleIMU::UpdateGyro()
 		_gyro_integrator.put(gyro_raw, dt);
 
 		updated = true;
+
+		if (gyro.clip_counter[0] > 0 || gyro.clip_counter[1] > 0 || gyro.clip_counter[2] > 0) {
+			// rotate sensor clip counts into vehicle body frame
+			const Vector3f clipping{_gyro_calibration.rotation() *
+						Vector3f{(float)gyro.clip_counter[0], (float)gyro.clip_counter[1], (float)gyro.clip_counter[2]}};
+
+			// round to get reasonble clip counts per axis (after board rotation)
+			const uint8_t clip_x = roundf(fabsf(clipping(0)));
+			const uint8_t clip_y = roundf(fabsf(clipping(1)));
+			const uint8_t clip_z = roundf(fabsf(clipping(2)));
+
+			_status.gyro_clipping[0] += clip_x;
+			_status.gyro_clipping[1] += clip_y;
+			_status.gyro_clipping[2] += clip_z;
+
+			if (clip_x > 0) {
+				_delta_angle_clipping |= vehicle_imu_s::CLIPPING_X;
+			}
+
+			if (clip_y > 0) {
+				_delta_angle_clipping |= vehicle_imu_s::CLIPPING_Y;
+			}
+
+			if (clip_z > 0) {
+				_delta_angle_clipping |= vehicle_imu_s::CLIPPING_Z;
+			}
+
+			_publish_status = true;
+
+			if (_gyro_calibration.enabled() && (hrt_elapsed_time(&_last_gyro_clipping_notify_time) > 3_s)) {
+				// start notifying the user periodically if there's significant continuous clipping
+				const uint64_t clipping_total = _status.gyro_clipping[0] + _status.gyro_clipping[1] + _status.gyro_clipping[2];
+
+				if (clipping_total > _last_gyro_clipping_notify_total_count + 1000) {
+					mavlink_log_critical(&_mavlink_log_pub, "Gyro %" PRIu8 " clipping, not safe to fly!\t", _instance);
+					/* EVENT
+					 * @description Land now, and check the vehicle setup.
+					 * Clipping can lead to fly-aways.
+					 */
+					events::send<uint8_t>(events::ID("vehicle_imu_gyro_clipping"), events::Log::Critical,
+							      "Gyro {1} clipping, not safe to fly!", _instance);
+					_last_gyro_clipping_notify_time = gyro.timestamp_sample;
+					_last_gyro_clipping_notify_total_count = clipping_total;
+				}
+			}
+		}
 	}
 
 	return updated;

src/modules/sensors/vehicle_imu/VehicleIMU.hpp

@@ -148,10 +148,15 @@ private:
 	float _coning_norm_accum{0};
 	float _coning_norm_accum_total_time_s{0};
 
-	uint8_t _delta_velocity_clipping{0};
+	uint8_t     _delta_angle_clipping{0};
+	uint8_t     _delta_velocity_clipping{0};
+
+	hrt_abstime _last_accel_clipping_notify_time{0};
+	hrt_abstime _last_gyro_clipping_notify_time{0};
+
+	uint64_t    _last_accel_clipping_notify_total_count{0};
+	uint64_t    _last_gyro_clipping_notify_total_count{0};
 
-	hrt_abstime _last_clipping_notify_time{0};
-	uint64_t _last_clipping_notify_total_count{0};
 	orb_advert_t _mavlink_log_pub{nullptr};
 
 	uint32_t _backup_schedule_timeout_us{20000};

src/modules/sensors/voted_sensors_update.cpp

@@ -172,6 +172,7 @@ void VotedSensorsUpdate::imuPoll(struct sensor_combined_s &raw)
 			_last_sensor_data[uorb_index].gyro_rad[1] = gyro_rate(1);
 			_last_sensor_data[uorb_index].gyro_rad[2] = gyro_rate(2);
 			_last_sensor_data[uorb_index].gyro_integral_dt = imu_report.delta_angle_dt;
+			_last_sensor_data[uorb_index].gyro_clipping = imu_report.delta_angle_clipping;
 			_last_sensor_data[uorb_index].accel_calibration_count = imu_report.accel_calibration_count;
 			_last_sensor_data[uorb_index].gyro_calibration_count = imu_report.gyro_calibration_count;
 
@@ -229,11 +230,14 @@ void VotedSensorsUpdate::imuPoll(struct sensor_combined_s &raw)
 		memcpy(&raw.accelerometer_m_s2, &_last_sensor_data[accel_best_index].accelerometer_m_s2,
 		       sizeof(raw.accelerometer_m_s2));
 		memcpy(&raw.gyro_rad, &_last_sensor_data[gyro_best_index].gyro_rad, sizeof(raw.gyro_rad));
+
 		raw.accelerometer_integral_dt = _last_sensor_data[accel_best_index].accelerometer_integral_dt;
-		raw.gyro_integral_dt = _last_sensor_data[gyro_best_index].gyro_integral_dt;
-		raw.accelerometer_clipping = _last_sensor_data[accel_best_index].accelerometer_clipping;
-		raw.accel_calibration_count = _last_sensor_data[accel_best_index].accel_calibration_count;
-		raw.gyro_calibration_count = _last_sensor_data[gyro_best_index].gyro_calibration_count;
+		raw.accelerometer_clipping    = _last_sensor_data[accel_best_index].accelerometer_clipping;
+		raw.accel_calibration_count   = _last_sensor_data[accel_best_index].accel_calibration_count;
+
+		raw.gyro_integral_dt          = _last_sensor_data[gyro_best_index].gyro_integral_dt;
+		raw.gyro_clipping             = _last_sensor_data[gyro_best_index].gyro_clipping;
+		raw.gyro_calibration_count    = _last_sensor_data[gyro_best_index].gyro_calibration_count;
 
 		if ((accel_best_index != _accel.last_best_vote) || (_selection.accel_device_id != _accel_device_id[accel_best_index])) {
 			_accel.last_best_vote = (uint8_t)accel_best_index;