Commit cb3b6b8e authored by Mario Tesi's avatar Mario Tesi Committed by Jonathan Cameron
Browse files

iio: imu: st_lsm6dsx: add odr calibration feature 



On LSM6DSO/LSM6DSR/LSM6DSOX/ASM330LHH and ISH330DHCX
devices it is possible to trim the hardware timestamp
resolution through the FREQ_FINE[7:0] bits of the
INTERNAL_FREQ_FINE register, which contains the difference
in percentage of the effective ODR (and timestamp rate)
with respect to the typical value.

The formula for calculating the effective ODR reported
in the application notes has been linearized to the first
order to simplify the calculation (pls. see note on source
code).

This change may be useful in the outcome of CTS
tests regarding the SingleSensorTests and the
SensorTest#testSensorTimeStamps for high ODRs

Signed-off-by: default avatarMario Tesi <mario.tesi@st.com>
Acked-by: default avatarLorenzo Bianconi <lorenzo@kernel.org>
Signed-off-by: default avatarJonathan Cameron <Jonathan.Cameron@huawei.com>
parent 960506ed

drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h

+4 −0
Original line number Diff line number Diff line
@@ -153,12 +153,14 @@ struct st_lsm6dsx_fifo_ops { 
 * @hr_timer: Hw timer resolution register info (addr + mask).

 * @fifo_en: Hw timer FIFO enable register info (addr + mask).

 * @decimator: Hw timer FIFO decimator register info (addr + mask).

 * @freq_fine: Difference in % of ODR with respect to the typical.

 */

struct st_lsm6dsx_hw_ts_settings {

	struct st_lsm6dsx_reg timer_en;

	struct st_lsm6dsx_reg hr_timer;

	struct st_lsm6dsx_reg fifo_en;

	struct st_lsm6dsx_reg decimator;

	u8 freq_fine;

};



/**
@@ -346,6 +348,7 @@ struct st_lsm6dsx_sensor { 
 * @fifo_mode: FIFO operating mode supported by the device.

 * @suspend_mask: Suspended sensor bitmask.

 * @enable_mask: Enabled sensor bitmask.

 * @ts_gain: Hw timestamp rate after internal calibration.

 * @ts_sip: Total number of timestamp samples in a given pattern.

 * @sip: Total number of samples (acc/gyro/ts) in a given pattern.

 * @buff: Device read buffer.
@@ -367,6 +370,7 @@ struct st_lsm6dsx_hw { 
	enum st_lsm6dsx_fifo_mode fifo_mode;

	u8 suspend_mask;

	u8 enable_mask;

	s64 ts_gain;

	u8 ts_sip;

	u8 sip;

drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c

+2 −3
Original line number Diff line number Diff line
@@ -50,7 +50,6 @@ 


#define ST_LSM6DSX_MAX_FIFO_ODR_VAL		0x08



#define ST_LSM6DSX_TS_SENSITIVITY		25000UL /* 25us */

#define ST_LSM6DSX_TS_RESET_VAL			0xaa



struct st_lsm6dsx_decimator_entry {
@@ -423,7 +422,7 @@ int st_lsm6dsx_read_fifo(struct st_lsm6dsx_hw *hw) 
				 */

				if (!reset_ts && ts >= 0xff0000)

					reset_ts = true;

				ts *= ST_LSM6DSX_TS_SENSITIVITY;

				ts *= hw->ts_gain;



				offset += ST_LSM6DSX_SAMPLE_SIZE;

			}
@@ -572,7 +571,7 @@ int st_lsm6dsx_read_tagged_fifo(struct st_lsm6dsx_hw *hw) 
				 */

				if (!reset_ts && ts >= 0xffff0000)

					reset_ts = true;

				ts *= ST_LSM6DSX_TS_SENSITIVITY;

				ts *= hw->ts_gain;

			} else {

				st_lsm6dsx_push_tagged_data(hw, tag, iio_buff,

							    ts);

drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c

+23 −0
Original line number Diff line number Diff line
@@ -63,6 +63,8 @@ 


#define ST_LSM6DSX_REG_WHOAMI_ADDR		0x0f



#define ST_LSM6DSX_TS_SENSITIVITY		25000UL /* 25us */



static const struct iio_chan_spec st_lsm6dsx_acc_channels[] = {

	ST_LSM6DSX_CHANNEL_ACC(IIO_ACCEL, 0x28, IIO_MOD_X, 0),

	ST_LSM6DSX_CHANNEL_ACC(IIO_ACCEL, 0x2a, IIO_MOD_Y, 1),
@@ -843,6 +845,7 @@ static const struct st_lsm6dsx_settings st_lsm6dsx_sensor_settings[] = { 
				.addr = 0x0a,

				.mask = GENMASK(7, 6),

			},

			.freq_fine = 0x63,

		},

		.shub_settings = {

			.page_mux = {
@@ -1037,6 +1040,7 @@ static const struct st_lsm6dsx_settings st_lsm6dsx_sensor_settings[] = { 
				.addr = 0x0a,

				.mask = GENMASK(7, 6),

			},

			.freq_fine = 0x63,

		},

		.event_settings = {

			.enable_reg = {
@@ -1208,6 +1212,7 @@ static const struct st_lsm6dsx_settings st_lsm6dsx_sensor_settings[] = { 
				.addr = 0x0a,

				.mask = GENMASK(7, 6),

			},

			.freq_fine = 0x63,

		},

		.shub_settings = {

			.page_mux = {
@@ -1893,6 +1898,24 @@ static int st_lsm6dsx_init_hw_timer(struct st_lsm6dsx_hw *hw) 
		if (err < 0)

			return err;

	}



	/* calibrate timestamp sensitivity */

	hw->ts_gain = ST_LSM6DSX_TS_SENSITIVITY;

	if (ts_settings->freq_fine) {

		err = regmap_read(hw->regmap, ts_settings->freq_fine, &val);

		if (err < 0)

			return err;



		/*

		 * linearize the AN5192 formula:

		 * 1 / (1 + x) ~= 1 - x (Taylor’s Series)

		 * ttrim[s] = 1 / (40000 * (1 + 0.0015 * val))

		 * ttrim[ns] ~= 25000 - 37.5 * val

		 * ttrim[ns] ~= 25000 - (37500 * val) / 1000

		 */

		hw->ts_gain -= ((s8)val * 37500) / 1000;

	}



	return 0;

}

CRA Git | Maintained and supported by SUSTech CRA and CCSE