Re: st_lsm6dsx : first two values of ism330dlc_gyro are wrong

[Lorenzo Bianconi <lorenzo@kernel.org>]

[-- Attachment #1: Type: text/plain, Size: 9804 bytes --]

> On Mon, 6 Feb 2023 10:58:08 +0100
> Lorenzo Bianconi <lorenzo@kernel.org> wrote:
> 
> > > On Sun, 5 Feb 2023 11:12:23 +0100
> > > Philippe De Muyter <phdm@macq.eu> wrote:
> > >   
> > > > Hello Lorenzo and list,
> > > > 
> > > > I use the imu/st_lsm6dsx (i2c) driver to drive a ism330dlc imu.
> > > > 
> > > > Every time I start a new acquisition of gyro values, the first two
> > > > values read are wrong, as can be see here :
> > > > 
> > > >  $ sudo ./iio_generic_buffer -n ism330dlc_gyro -g -c 10 -a
> > > >  iio device number being used is 1
> > > >  trigger-less mode selected
> > > >  No channels are enabled, enabling all channels
> > > >  Enabling: in_anglvel_z_en
> > > >  Enabling: in_timestamp_en
> > > >  Enabling: in_anglvel_y_en
> > > >  Enabling: in_anglvel_x_en
> > > >  -0.138924 -0.915246 0.470628 1675591514696125669
> > > >  -0.012699 -0.362151 0.143208 1675591514772675669
> > > >  0.001989 -0.076500 0.035190 1675591514849250669
> > > >  0.002295 -0.076194 0.035343 1675591514925825669
> > > >  0.002142 -0.076041 0.035343 1675591515002400669
> > > >  0.001989 -0.076041 0.035343 1675591515078975669
> > > >  0.001836 -0.076347 0.035649 1675591515155525669
> > > >  0.001836 -0.076500 0.035649 1675591515232075669
> > > >  0.001989 -0.076500 0.035649 1675591515308625669
> > > >  0.001989 -0.076347 0.035649 1675591515385200669
> > > >  Disabling: in_anglvel_z_en
> > > >  Disabling: in_timestamp_en
> > > >  Disabling: in_anglvel_y_en
> > > >  Disabling: in_anglvel_x_en
> > > >  $
> > > > 
> > > > Is that a normal behaviour for a gyro in that family or is it be caused
> > > > by a software or hardware bug ?  
> > > 
> > > So, some random thoughts on what might be going on...
> > > 1) Stale data in the fifo.  Could you run this experiment twice whilst being
> > >    careful not to move the device between the runs.  If we still see the wrong
> > >    values at the start then it's not that...  
> > 
> > When the device is powered-down we set the FIFO in bypass mode and in-flight
> > samples are discarded.
> > 
> > > 
> > > 2) Device takes a little whilst to stabilize. Possibly this is down to the
> > >    low pass filters requiring a few samples before they deliver stable output.
> > >    From a quick glance I don't think we provide any userspace control of those
> > >    filters and I think LPR1 is left in default state of disabled.  
> > 
> > I would say this issue is related to the "Accelerometer and gyroscope
> > turn-on/off time" (section 3.9 in the sensor application note).
> > 
> > https://www.st.com/resource/en/application_note/an5125-ism330dlc-3d-accelerometer-and-3d-gyroscope-with-digital-output-for-industrial-applications-stmicroelectronics.pdf
> > 
> > @Jonathan: do you think we should discard these sample in the driver or in the
> > user-space app? I would say this can be a general issue. What do you think?
> 
> In driver.  This isn't an uncommon problem for sensors and userspace would in 
> general have no idea how many samples to drop.  Also dependent on the sampling
> rates etc so if we support control of those, we'll want to have the driver
> drop the right number of samples.  Though yikes at the top end. You can have
> to drop 540 samples... Ah well. That is at 6kHz sampling so still not very long.
> 
> The only alternative would be to expose the current number to drop to userspace
> but then existing userspace code would not drop them.  Hence I think it needs
> to be in driver.
> 
> Jonathan
> 
> 
> > 
> > Regards,
> > Lorenzo
> > 
> > > 
> > > You could try messing with the sampling frequency as that may affect the number
> > > of bad samples you see and give us more of a clue (it affects lpf2 directly).
> > > 
> > > Jonathan
> > >   
> > > > 
> > > > Best regards
> > > > 
> > > > Philippe

Hi Philippe,

can you please test the patch below? Please note it is just compiled tested.

Regards,
Lorenzo

diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h
index 499fcf8875b4..2fb4d75ad096 100644
--- a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h
+++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h
@@ -137,6 +137,13 @@ struct st_lsm6dsx_odr_table_entry {
 	int odr_len;
 };
 
+struct st_lsm6dsx_samples_to_discard {
+	struct {
+		u32 milli_hz;
+		u16 samples;
+	} map[ST_LSM6DSX_ODR_LIST_SIZE];
+};
+
 struct st_lsm6dsx_fs {
 	u32 gain;
 	u8 val;
@@ -323,6 +330,7 @@ struct st_lsm6dsx_settings {
 	} irq_config;
 	struct st_lsm6dsx_reg drdy_mask;
 	struct st_lsm6dsx_odr_table_entry odr_table[2];
+	struct st_lsm6dsx_samples_to_discard samples_to_discard[2];
 	struct st_lsm6dsx_fs_table_entry fs_table[2];
 	struct st_lsm6dsx_reg decimator[ST_LSM6DSX_MAX_ID];
 	struct st_lsm6dsx_reg batch[ST_LSM6DSX_MAX_ID];
@@ -353,6 +361,7 @@ enum st_lsm6dsx_fifo_mode {
  * @hw: Pointer to instance of struct st_lsm6dsx_hw.
  * @gain: Configured sensor sensitivity.
  * @odr: Output data rate of the sensor [Hz].
+ * @samples_to_discard: Number of samples to discard for filters transitory.
  * @watermark: Sensor watermark level.
  * @decimator: Sensor decimation factor.
  * @sip: Number of samples in a given pattern.
@@ -367,6 +376,7 @@ struct st_lsm6dsx_sensor {
 	u32 gain;
 	u32 odr;
 
+	u16 samples_to_discard;
 	u16 watermark;
 	u8 decimator;
 	u8 sip;
diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c
index 7dd5205aea5b..1b78238db388 100644
--- a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c
+++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c
@@ -418,12 +418,22 @@ int st_lsm6dsx_read_fifo(struct st_lsm6dsx_hw *hw)
 				       &hw->buff[offset],
 				       sizeof(hw->scan[ST_LSM6DSX_ID_GYRO].channels));
 				offset += sizeof(hw->scan[ST_LSM6DSX_ID_GYRO].channels);
+				/* We need to discards gyro samples during
+				 * filters settiling time
+				 */
+				if (gyro_sensor->samples_to_discard)
+					gyro_sensor->samples_to_discard--;
 			}
 			if (acc_sip > 0 && !(sip % acc_sensor->decimator)) {
 				memcpy(hw->scan[ST_LSM6DSX_ID_ACC].channels,
 				       &hw->buff[offset],
 				       sizeof(hw->scan[ST_LSM6DSX_ID_ACC].channels));
 				offset += sizeof(hw->scan[ST_LSM6DSX_ID_ACC].channels);
+				/* We need to discards accel samples during
+				 * filters settiling time
+				 */
+				if (acc_sensor->samples_to_discard)
+					acc_sensor->samples_to_discard--;
 			}
 			if (ext_sip > 0 && !(sip % ext_sensor->decimator)) {
 				memcpy(hw->scan[ST_LSM6DSX_ID_EXT0].channels,
@@ -456,14 +466,16 @@ int st_lsm6dsx_read_fifo(struct st_lsm6dsx_hw *hw)
 				offset += ST_LSM6DSX_SAMPLE_SIZE;
 			}
 
-			if (gyro_sip > 0 && !(sip % gyro_sensor->decimator)) {
+			if (gyro_sip > 0 && !(sip % gyro_sensor->decimator) &&
+			    !gyro_sensor->samples_to_discard) {
 				iio_push_to_buffers_with_timestamp(
 					hw->iio_devs[ST_LSM6DSX_ID_GYRO],
 					&hw->scan[ST_LSM6DSX_ID_GYRO],
 					gyro_sensor->ts_ref + ts);
 				gyro_sip--;
 			}
-			if (acc_sip > 0 && !(sip % acc_sensor->decimator)) {
+			if (acc_sip > 0 && !(sip % acc_sensor->decimator) &&
+			    !acc_sensor->samples_to_discard) {
 				iio_push_to_buffers_with_timestamp(
 					hw->iio_devs[ST_LSM6DSX_ID_ACC],
 					&hw->scan[ST_LSM6DSX_ID_ACC],
@@ -541,8 +553,12 @@ st_lsm6dsx_push_tagged_data(struct st_lsm6dsx_hw *hw, u8 tag,
 	}
 
 	sensor = iio_priv(iio_dev);
-	iio_push_to_buffers_with_timestamp(iio_dev, data,
-					   ts + sensor->ts_ref);
+	/* We need to discards gyro samples during filters settiling time */
+	if (!sensor->samples_to_discard)
+		iio_push_to_buffers_with_timestamp(iio_dev, data,
+						   ts + sensor->ts_ref);
+	else
+		sensor->samples_to_discard--;
 
 	return 0;
 }
@@ -654,6 +670,25 @@ int st_lsm6dsx_flush_fifo(struct st_lsm6dsx_hw *hw)
 	return err;
 }
 
+static void
+st_lsm6dsx_update_samples_to_discard(struct st_lsm6dsx_sensor *sensor)
+{
+	const struct st_lsm6dsx_samples_to_discard *data;
+	int i;
+
+	if (sensor->id != ST_LSM6DSX_ID_GYRO &&
+	    sensor->id != ST_LSM6DSX_ID_ACC)
+		return;
+
+	data = &sensor->hw->settings->samples_to_discard[sensor->id];
+	for (i = 0; i < ST_LSM6DSX_ODR_LIST_SIZE; i++) {
+		if (data->map[i].milli_hz == sensor->odr) {
+			sensor->samples_to_discard = data->map[i].samples;
+			return;
+		}
+	}
+}
+
 int st_lsm6dsx_update_fifo(struct st_lsm6dsx_sensor *sensor, bool enable)
 {
 	struct st_lsm6dsx_hw *hw = sensor->hw;
@@ -673,6 +708,9 @@ int st_lsm6dsx_update_fifo(struct st_lsm6dsx_sensor *sensor, bool enable)
 			goto out;
 	}
 
+	if (enable)
+		st_lsm6dsx_update_samples_to_discard(sensor);
+
 	err = st_lsm6dsx_device_set_enable(sensor, enable);
 	if (err < 0)
 		goto out;
diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c
index 3f6060c64f32..a74a702205e8 100644
--- a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c
+++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c
@@ -610,6 +610,24 @@ static const struct st_lsm6dsx_settings st_lsm6dsx_sensor_settings[] = {
 				.odr_len = 6,
 			},
 		},
+		.samples_to_discard = {
+			[ST_LSM6DSX_ID_ACC] = {
+				.map[0] = {  12500, 1 },
+				.map[1] = {  26000, 1 },
+				.map[2] = {  52000, 1 },
+				.map[3] = { 104000, 2 },
+				.map[4] = { 208000, 2 },
+				.map[5] = { 416000, 2 },
+			},
+			[ST_LSM6DSX_ID_GYRO] = {
+				.map[0] = {  12500, 2 },
+				.map[1] = {  26000, 3 },
+				.map[2] = {  52000, 3 },
+				.map[3] = { 104000, 4 },
+				.map[4] = { 208000, 5 },
+				.map[5] = { 416000, 6 },
+			},
+		},
 		.fs_table = {
 			[ST_LSM6DSX_ID_ACC] = {
 				.reg = {

> > > >   
> > >   
> > 
> 

[-- Attachment #2: signature.asc --]
[-- Type: application/pgp-signature, Size: 228 bytes --]