ekf2 handle accelerometer clipping

- track clipping per IMU axis and pass through to ecl/EKF
 - update ecl/EKF to include delta velocity clipping changes (PX4/ecl#663)

msg/sensor_accel_integrated.msg

@@ -8,4 +8,5 @@ uint64 error_count
 float32[3] delta_velocity # delta velocity in the NED board axis in m/s over the integration time frame (dt)
 uint16 dt                 # integration time (microseconds)
 uint8 samples             # number of samples integrated
-uint8 clip_count          # total clip count per integration period on any axis
+
+uint8[3] clip_counter     # clip count per axis over the integration period

msg/sensor_combined.msg

@@ -13,3 +13,8 @@ uint32 gyro_integral_dt		# gyro measurement sampling period in us
 int32 accelerometer_timestamp_relative	# timestamp + accelerometer_timestamp_relative = Accelerometer timestamp
 float32[3] accelerometer_m_s2		# average value acceleration measured in the XYZ body frame in m/s/s over the last accelerometer sampling period
 uint32 accelerometer_integral_dt	# accelerometer measurement sampling period in us
+
+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

msg/sensor_gyro_integrated.msg

@@ -8,4 +8,5 @@ uint64 error_count
 float32[3] delta_angle    # delta angle in the NED board axis in rad/s over the integration time frame (dt)
 uint16 dt                 # integration time (microseconds)
 uint8 samples             # number of samples integrated
-uint8 clip_count          # total clip count per integration period on any axis
+
+uint8[3] clip_counter     # clip count per axis over the integration period

msg/vehicle_imu.msg

@@ -11,5 +11,7 @@ float32[3] delta_velocity       # delta velocity in the NED board axis in m/s ov
 
 uint16 dt                       # integration period in us
 
-uint8 integrated_samples        # number of samples integrated
+uint8 CLIPPING_X = 1
-uint8 clip_count                # total clip count per integration period on any axis
+uint8 CLIPPING_Y = 2
+uint8 CLIPPING_Z = 4
+uint8 delta_velocity_clipping   # bitfield indicating if there was any accelerometer clipping (per axis) during the integration time frame

src/lib/drivers/accelerometer/PX4Accelerometer.cpp

@@ -137,8 +137,8 @@ void PX4Accelerometer::update(hrt_abstime timestamp_sample, float x, float y, fl
 	// Clipping (check unscaled raw values)
 	for (int i = 0; i < 3; i++) {
 		if (fabsf(raw(i)) > _clip_limit) {
-			_clipping[i]++;
+			_clipping_total[i]++;
-			_integrator_clipping++;
+			_integrator_clipping(i)++;
 		}
 	}
 
@@ -177,7 +177,11 @@ void PX4Accelerometer::update(hrt_abstime timestamp_sample, float x, float y, fl
 		delta_velocity.copyTo(report.delta_velocity);
 		report.dt = integral_dt;
 		report.samples = _integrator_samples;
-		report.clip_count = _integrator_clipping;
+
+		for (int i = 0; i < 3; i++) {
+			report.clip_counter[i] = fabsf(roundf(_integrator_clipping(i)));
+		}
+
 		report.timestamp = hrt_absolute_time();
 
 		_sensor_integrated_pub.publish(report);
@@ -230,11 +234,13 @@ void PX4Accelerometer::updateFIFO(const FIFOSample &sample)
 	unsigned clip_count_y = clipping(sample.y, _clip_limit, N);
 	unsigned clip_count_z = clipping(sample.z, _clip_limit, N);
 
-	_clipping[0] += clip_count_x;
+	_clipping_total[0] += clip_count_x;
-	_clipping[1] += clip_count_y;
+	_clipping_total[1] += clip_count_y;
-	_clipping[2] += clip_count_z;
+	_clipping_total[2] += clip_count_z;
 
-	_integrator_clipping += clip_count_x + clip_count_y + clip_count_z;
+	_integrator_clipping(0) += clip_count_x;
+	_integrator_clipping(1) += clip_count_y;
+	_integrator_clipping(2) += clip_count_z;
 
 	// integrated data (INS)
 	{
@@ -280,7 +286,12 @@ void PX4Accelerometer::updateFIFO(const FIFOSample &sample)
 			delta_velocity.copyTo(report.delta_velocity);
 			report.dt = _integrator_fifo_samples * dt; // time span in microseconds
 			report.samples = _integrator_fifo_samples;
-			report.clip_count = _integrator_clipping;
+
+			const Vector3f clipping{_rotation_dcm * _integrator_clipping};
+
+			for (int i = 0; i < 3; i++) {
+				report.clip_counter[i] = fabsf(roundf(clipping(i)));
+			}
 
 			report.timestamp = hrt_absolute_time();
 			_sensor_integrated_pub.publish(report);
@@ -328,9 +339,9 @@ void PX4Accelerometer::PublishStatus()
 		status.measure_rate_hz = _update_rate;
 		status.temperature = _temperature;
 		status.vibration_metric = _vibration_metric;
-		status.clipping[0] = _clipping[0];
+		status.clipping[0] = _clipping_total[0];
-		status.clipping[1] = _clipping[1];
+		status.clipping[1] = _clipping_total[1];
-		status.clipping[2] = _clipping[2];
+		status.clipping[2] = _clipping_total[2];
 		status.timestamp = hrt_absolute_time();
 		_sensor_status_pub.publish(status);
 
@@ -343,7 +354,7 @@ void PX4Accelerometer::ResetIntegrator()
 	_integrator_samples = 0;
 	_integrator_fifo_samples = 0;
 	_integration_raw.zero();
-	_integrator_clipping = 0;
+	_integrator_clipping.zero();
 
 	_timestamp_sample_prev = 0;
 }

src/lib/drivers/accelerometer/PX4Accelerometer.hpp

@@ -120,17 +120,16 @@ private:
 
 	uint64_t		_error_count{0};
 
-	uint32_t		_clipping[3] {};
+	uint32_t		_clipping_total[3] {};
 
 	uint16_t		_update_rate{1000};
 
 	// integrator
 	hrt_abstime		_timestamp_sample_prev{0};
 	matrix::Vector3f	_integration_raw{};
+	matrix::Vector3f	_integrator_clipping{};
 	int16_t			_last_sample[3] {};
 	uint8_t			_integrator_reset_samples{4};
 	uint8_t			_integrator_samples{0};
 	uint8_t			_integrator_fifo_samples{0};
-	uint8_t			_integrator_clipping{0};
-
 };

src/lib/drivers/gyroscope/PX4Gyroscope.cpp

@@ -139,8 +139,8 @@ void PX4Gyroscope::update(hrt_abstime timestamp_sample, float x, float y, float
 	// Clipping (check unscaled raw values)
 	for (int i = 0; i < 3; i++) {
 		if (fabsf(raw(i)) > _clip_limit) {
-			_clipping[i]++;
+			_clipping_total[i]++;
-			_integrator_clipping++;
+			_integrator_clipping(i)++;
 		}
 	}
 
@@ -179,7 +179,11 @@ void PX4Gyroscope::update(hrt_abstime timestamp_sample, float x, float y, float
 		delta_angle.copyTo(report.delta_angle);
 		report.dt = integral_dt;
 		report.samples = _integrator_samples;
-		report.clip_count = _integrator_clipping;
+
+		for (int i = 0; i < 3; i++) {
+			report.clip_counter[i] = fabsf(roundf(_integrator_clipping(i)));
+		}
+
 		report.timestamp = hrt_absolute_time();
 
 		_sensor_integrated_pub.publish(report);
@@ -232,11 +236,13 @@ void PX4Gyroscope::updateFIFO(const FIFOSample &sample)
 	unsigned clip_count_y = clipping(sample.y, _clip_limit, N);
 	unsigned clip_count_z = clipping(sample.z, _clip_limit, N);
 
-	_clipping[0] += clip_count_x;
+	_clipping_total[0] += clip_count_x;
-	_clipping[1] += clip_count_y;
+	_clipping_total[1] += clip_count_y;
-	_clipping[2] += clip_count_z;
+	_clipping_total[2] += clip_count_z;
 
-	_integrator_clipping += clip_count_x + clip_count_y + clip_count_z;
+	_integrator_clipping(0) += clip_count_x;
+	_integrator_clipping(1) += clip_count_y;
+	_integrator_clipping(2) += clip_count_z;
 
 	// integrated data (INS)
 	{
@@ -282,7 +288,12 @@ void PX4Gyroscope::updateFIFO(const FIFOSample &sample)
 			delta_angle.copyTo(report.delta_angle);
 			report.dt = _integrator_fifo_samples * dt; // time span in microseconds
 			report.samples = _integrator_fifo_samples;
-			report.clip_count = _integrator_clipping;
+
+			const Vector3f clipping{_rotation_dcm * _integrator_clipping};
+
+			for (int i = 0; i < 3; i++) {
+				report.clip_counter[i] = fabsf(roundf(clipping(i)));
+			}
 
 			report.timestamp = hrt_absolute_time();
 			_sensor_integrated_pub.publish(report);
@@ -331,9 +342,9 @@ void PX4Gyroscope::PublishStatus()
 		status.temperature = _temperature;
 		status.vibration_metric = _vibration_metric;
 		status.coning_vibration = _coning_vibration;
-		status.clipping[0] = _clipping[0];
+		status.clipping[0] = _clipping_total[0];
-		status.clipping[1] = _clipping[1];
+		status.clipping[1] = _clipping_total[1];
-		status.clipping[2] = _clipping[2];
+		status.clipping[2] = _clipping_total[2];
 		status.timestamp = hrt_absolute_time();
 		_sensor_status_pub.publish(status);
 
@@ -346,7 +357,7 @@ void PX4Gyroscope::ResetIntegrator()
 	_integrator_samples = 0;
 	_integrator_fifo_samples = 0;
 	_integration_raw.zero();
-	_integrator_clipping = 0;
+	_integrator_clipping.zero();
 
 	_timestamp_sample_prev = 0;
 }

src/lib/drivers/gyroscope/PX4Gyroscope.hpp

@@ -122,18 +122,18 @@ private:
 
 	uint64_t		_error_count{0};
 
-	uint32_t		_clipping[3] {};
+	uint32_t		_clipping_total[3] {};
 
 	uint16_t		_update_rate{1000};
 
 	// integrator
 	hrt_abstime		_timestamp_sample_prev{0};
 	matrix::Vector3f	_integration_raw{};
+	matrix::Vector3f	_integrator_clipping{};
 	int16_t			_last_sample[3] {};
 	uint8_t			_integrator_reset_samples{4};
 	uint8_t			_integrator_samples{0};
 	uint8_t			_integrator_fifo_samples{0};
-	uint8_t			_integrator_clipping{0};
 
 	DEFINE_PARAMETERS(
 		(ParamInt<px4::params::IMU_GYRO_RATEMAX>) _param_imu_gyro_rate_max

src/modules/ekf2/ekf2_main.cpp

@@ -777,6 +777,12 @@ void Ekf2::Run()
 		imu_sample_new.delta_vel_dt = imu.dt * 1.e-6f;
 		imu_sample_new.delta_vel = Vector3f{imu.delta_velocity};
 
+		if (imu.delta_velocity_clipping > 0) {
+			imu_sample_new.delta_vel_clipping[0] = imu.delta_velocity_clipping & vehicle_imu_s::CLIPPING_X;
+			imu_sample_new.delta_vel_clipping[1] = imu.delta_velocity_clipping & vehicle_imu_s::CLIPPING_Y;
+			imu_sample_new.delta_vel_clipping[2] = imu.delta_velocity_clipping & vehicle_imu_s::CLIPPING_Z;
+		}
+
 		imu_dt = imu.dt;
 
 		bias.accel_device_id = imu.accel_device_id;
@@ -792,6 +798,12 @@ void Ekf2::Run()
 		imu_sample_new.delta_vel_dt = sensor_combined.accelerometer_integral_dt * 1.e-6f;
 		imu_sample_new.delta_vel = Vector3f{sensor_combined.accelerometer_m_s2} * imu_sample_new.delta_vel_dt;
 
+		if (sensor_combined.accelerometer_clipping > 0) {
+			imu_sample_new.delta_vel_clipping[0] = sensor_combined.accelerometer_clipping & sensor_combined_s::CLIPPING_X;
+			imu_sample_new.delta_vel_clipping[1] = sensor_combined.accelerometer_clipping & sensor_combined_s::CLIPPING_Y;
+			imu_sample_new.delta_vel_clipping[2] = sensor_combined.accelerometer_clipping & sensor_combined_s::CLIPPING_Z;
+		}
+
 		imu_dt = sensor_combined.gyro_integral_dt;
 	}
 

src/modules/sensors/vehicle_imu/VehicleIMU.cpp

@@ -121,8 +121,7 @@ void VehicleIMU::Run()
 		delta_velocity.copyTo(imu.delta_velocity);
 
 		imu.dt = accel.dt;
-		imu.integrated_samples = accel.samples;
+		//imu.clip_count = accel.clip_count;
-		imu.clip_count = accel.clip_count;
 		imu.timestamp = hrt_absolute_time();
 
 		_vehicle_imu_pub.publish(imu);

src/modules/sensors/voted_sensors_update.cpp

@@ -505,6 +505,28 @@ void VotedSensorsUpdate::accelPoll(struct sensor_combined_s &raw)
 			_last_sensor_data[uorb_index].accelerometer_m_s2[1] = accel_data(1);
 			_last_sensor_data[uorb_index].accelerometer_m_s2[2] = accel_data(2);
 
+			// record if there's any clipping per axis
+			_last_sensor_data[uorb_index].accelerometer_clipping = 0;
+
+			if (accel_report.clip_counter[0] > 0 || accel_report.clip_counter[1] > 0 || accel_report.clip_counter[2] > 0) {
+
+				const Vector3f sensor_clip_count{(float)accel_report.clip_counter[0], (float)accel_report.clip_counter[1], (float)accel_report.clip_counter[2]};
+				const Vector3f clipping{_board_rotation * sensor_clip_count};
+				static constexpr float CLIP_COUNT_THRESHOLD = 1.f;
+
+				if (fabsf(clipping(0)) >= CLIP_COUNT_THRESHOLD) {
+					_last_sensor_data[uorb_index].accelerometer_clipping |= sensor_combined_s::CLIPPING_X;
+				}
+
+				if (fabsf(clipping(1)) >= CLIP_COUNT_THRESHOLD) {
+					_last_sensor_data[uorb_index].accelerometer_clipping |= sensor_combined_s::CLIPPING_Y;
+				}
+
+				if (fabsf(clipping(2)) >= CLIP_COUNT_THRESHOLD) {
+					_last_sensor_data[uorb_index].accelerometer_clipping |= sensor_combined_s::CLIPPING_Z;
+				}
+			}
+
 			_last_accel_timestamp[uorb_index] = accel_report.timestamp;
 			_accel.voter.put(uorb_index, accel_report.timestamp, _last_sensor_data[uorb_index].accelerometer_m_s2,
 					 accel_report.error_count, _accel.priority[uorb_index]);
@@ -520,6 +542,8 @@ void VotedSensorsUpdate::accelPoll(struct sensor_combined_s &raw)
 		raw.accelerometer_integral_dt = _last_sensor_data[best_index].accelerometer_integral_dt;
 		memcpy(&raw.accelerometer_m_s2, &_last_sensor_data[best_index].accelerometer_m_s2, sizeof(raw.accelerometer_m_s2));
 
+		raw.accelerometer_clipping = _last_sensor_data[best_index].accelerometer_clipping;
+
 		if (best_index != _accel.last_best_vote) {
 			_accel.last_best_vote = (uint8_t)best_index;
 		}