Skip to content

Commit 0a2b42f

Browse files
sctanfsctanf
committed
lsm6dso/dsr implement sensor_ext
1 parent 1b003af commit 0a2b42f

File tree

2 files changed

+109
-1
lines changed

2 files changed

+109
-1
lines changed

‎src/sensor/imu/LSM6DSO.c‎

Lines changed: 101 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -320,6 +320,100 @@ uint8_t lsm6dso_setup_WOM(void)
320320
return NRF_GPIO_PIN_PULLUP << 4 | NRF_GPIO_PIN_SENSE_LOW; // active low
321321
}
322322

323+
int lsm6dso_ext_setup(void)
324+
{
325+
sensor_interface_ext_configure(&sensor_ext_lsm6dsv);
326+
return 0;
327+
}
328+
329+
int lsm6dso_ext_passthrough(bool passthrough)
330+
{
331+
int err = 0;
332+
if (passthrough)
333+
{
334+
err |= ssi_reg_write_byte(SENSOR_INTERFACE_DEV_IMU, LSM6DSV_FUNC_CFG_ACCESS, 0x40); // switch to sensor hub registers
335+
err |= ssi_reg_write_byte(SENSOR_INTERFACE_DEV_IMU, LSM6DSV_MASTER_CONFIG, 0x10); // passthrough on
336+
err |= ssi_reg_write_byte(SENSOR_INTERFACE_DEV_IMU, LSM6DSV_FUNC_CFG_ACCESS, 0x00); // switch to normal registers
337+
}
338+
else
339+
{
340+
err |= ssi_reg_write_byte(SENSOR_INTERFACE_DEV_IMU, LSM6DSV_FUNC_CFG_ACCESS, 0x40); // switch to sensor hub registers
341+
err |= ssi_reg_write_byte(SENSOR_INTERFACE_DEV_IMU, LSM6DSV_MASTER_CONFIG, 0x08); // passthrough off
342+
err |= ssi_reg_write_byte(SENSOR_INTERFACE_DEV_IMU, LSM6DSV_FUNC_CFG_ACCESS, 0x00); // switch to normal registers
343+
}
344+
if (err)
345+
LOG_ERR("Communication error");
346+
return 0;
347+
}
348+
349+
int lsm6dso_ext_write(const uint8_t addr, const uint8_t *buf, uint32_t num_bytes)
350+
{
351+
if (num_bytes != 2)
352+
{
353+
LOG_ERR("Unsupported write");
354+
return -1;
355+
}
356+
// Configure transaction and begin one-shot (AN5922, page 80, One-shot write routine)
357+
int err = ssi_reg_write_byte(SENSOR_INTERFACE_DEV_IMU, LSM6DSV_FUNC_CFG_ACCESS, 0x40); // switch to sensor hub registers
358+
uint8_t slv0[3] = {(addr << 1) | 0x00, buf[0], 0x00 | 0x00}; // write, SHUB_ODR = 104Hz, reading no bytes
359+
err |= ssi_burst_write(SENSOR_INTERFACE_DEV_IMU, LSM6DSV_SLV0_ADD, slv0, 3);
360+
// err |= ssi_reg_write_byte(SENSOR_INTERFACE_DEV_IMU, LSM6DSV_SLV0_ADD, (addr << 1) | 0x00); // write
361+
// err |= ssi_reg_write_byte(SENSOR_INTERFACE_DEV_IMU, LSM6DSV_SLV0_SUBADD, buf[0]);
362+
// err |= ssi_reg_write_byte(SENSOR_INTERFACE_DEV_IMU, LSM6DSV_SLV0_CONFIG, 0x00 | 0x00); // SHUB_ODR = 104Hz, reading no bytes
363+
err |= ssi_reg_write_byte(SENSOR_INTERFACE_DEV_IMU, LSM6DSV_DATAWRITE_SLV0, buf[1]);
364+
err |= ssi_reg_write_byte(SENSOR_INTERFACE_DEV_IMU, LSM6DSV_MASTER_CONFIG, 0x4C); // WRITE_ONCE, SHUB_PU_EN, enable I2C master
365+
// Wait for transaction
366+
uint8_t status = 0;
367+
int64_t timeout = k_uptime_get() + 10;
368+
while ((status & 0x80) && k_uptime_get() < timeout) // WR_ONCE_DONE
369+
err |= ssi_reg_read_byte(SENSOR_INTERFACE_DEV_IMU, LSM6DSV_STATUS_MASTER, &status);
370+
err |= ssi_reg_write_byte(SENSOR_INTERFACE_DEV_IMU, LSM6DSV_MASTER_CONFIG, 0x08); // SHUB_PU_EN, disable I2C master
371+
k_usleep(300);
372+
err |= ssi_reg_write_byte(SENSOR_INTERFACE_DEV_IMU, LSM6DSV_FUNC_CFG_ACCESS, 0x00); // switch to normal registers
373+
if (~status & 0x80)
374+
{
375+
LOG_ERR("Write timeout");
376+
return -1;
377+
}
378+
return err;
379+
}
380+
381+
int lsm6dso_ext_write_read(const uint8_t addr, const void *write_buf, size_t num_write, void *read_buf, size_t num_read)
382+
{
383+
if (num_write != 1 || num_read < 1 || num_read > 8)
384+
{
385+
LOG_ERR("Unsupported write_read");
386+
return -1;
387+
}
388+
// Configure transaction and begin one-shot (AN5922, page 79, One-shot read routine)
389+
int err = ssi_reg_write_byte(SENSOR_INTERFACE_DEV_IMU, LSM6DSV_FUNC_CFG_ACCESS, 0x40); // switch to sensor hub registers
390+
uint8_t slv0[3] = {(addr << 1) | 0x01, ((const uint8_t *)write_buf)[0], 0x00 | num_read}; // read, SHUB_ODR = 104Hz, reading num_read bytes
391+
err |= ssi_burst_write(SENSOR_INTERFACE_DEV_IMU, LSM6DSV_SLV0_ADD, slv0, 3);
392+
// err |= ssi_reg_write_byte(SENSOR_INTERFACE_DEV_IMU, LSM6DSV_SLV0_ADD, (addr << 1) | 0x01); // read
393+
// err |= ssi_reg_write_byte(SENSOR_INTERFACE_DEV_IMU, LSM6DSV_SLV0_SUBADD, ((const uint8_t *)write_buf)[0]);
394+
// err |= ssi_reg_write_byte(SENSOR_INTERFACE_DEV_IMU, LSM6DSV_SLV0_CONFIG, 0x00 | num_read); // SHUB_ODR = 104Hz, reading num_read bytes
395+
err |= ssi_reg_write_byte(SENSOR_INTERFACE_DEV_IMU, LSM6DSV_MASTER_CONFIG, 0x4C); // WRITE_ONCE mandatory for read, SHUB_PU_EN, enable I2C master
396+
// Wait for transaction
397+
err |= ssi_reg_write_byte(SENSOR_INTERFACE_DEV_IMU, LSM6DSV_FUNC_CFG_ACCESS, 0x00); // switch to normal registers
398+
uint8_t tmp;
399+
err |= ssi_reg_read_byte(SENSOR_INTERFACE_DEV_IMU, LSM6DSV_OUTX_H_A, &tmp); // clear XLDA
400+
uint8_t status = 0;
401+
int64_t timeout = k_uptime_get() + 10;
402+
while ((status & 0x01) && k_uptime_get() < timeout) // XLDA
403+
err |= ssi_reg_read_byte(SENSOR_INTERFACE_DEV_IMU, LSM6DSV_STATUS_REG, &status);
404+
status = 0;
405+
timeout = k_uptime_get() + 10;
406+
while ((status & 0x01) && k_uptime_get() < timeout) // SENS_HUB_ENDOP
407+
err |= ssi_reg_read_byte(SENSOR_INTERFACE_DEV_IMU, LSM6DSO_STATUS_MASTER_MAINPAGE, &status);
408+
// Read data
409+
err |= ssi_reg_write_byte(SENSOR_INTERFACE_DEV_IMU, LSM6DSV_FUNC_CFG_ACCESS, 0x40); // switch to sensor hub registers
410+
err |= ssi_reg_write_byte(SENSOR_INTERFACE_DEV_IMU, LSM6DSV_MASTER_CONFIG, 0x08); // SHUB_PU_EN, disable I2C master
411+
k_usleep(300);
412+
err |= ssi_burst_read(SENSOR_INTERFACE_DEV_IMU, LSM6DSV_SENSOR_HUB_1, read_buf, num_read);
413+
err |= ssi_reg_write_byte(SENSOR_INTERFACE_DEV_IMU, LSM6DSV_FUNC_CFG_ACCESS, 0x00); // switch to normal registers
414+
return err;
415+
}
416+
323417
const sensor_imu_t sensor_imu_lsm6dso = {
324418
*lsm6dso_init,
325419
*lsm_shutdown,
@@ -335,6 +429,12 @@ const sensor_imu_t sensor_imu_lsm6dso = {
335429

336430
*lsm6dso_setup_WOM,
337431

338-
*imu_none_ext_setup,
432+
*lsm6dso_ext_passthrough,
339433
*lsm_ext_passthrough
340434
};
435+
436+
const sensor_ext_ssi_t sensor_ext_lsm6dso = {
437+
*lsm6dso_ext_write,
438+
*lsm6dso_ext_write_read,
439+
8
440+
};

‎src/sensor/imu/LSM6DSO.h‎

Lines changed: 8 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -16,6 +16,7 @@
1616
#define LSM6DSO_CTRL7 0x16
1717
#define LSM6DSO_CTRL8 0x17
1818

19+
#define LSM6DSO_STATUS_MASTER_MAINPAGE 0x39
1920
#define LSM6DSO_FIFO_STATUS1 0x3A
2021

2122
#define LSM6DSO_TAP_CFG0 0x56
@@ -68,6 +69,13 @@ uint16_t lsm6dso_fifo_read(uint8_t *data, uint16_t len);
6869

6970
uint8_t lsm6dso_setup_WOM(void);
7071

72+
int lsm6dso_ext_setup(void);
73+
int lsm6dso_ext_passthrough(bool passthrough);
74+
75+
int lsm6dso_ext_write(const uint8_t addr, const uint8_t *buf, uint32_t num_bytes);
76+
int lsm6dso_ext_write_read(const uint8_t addr, const void *write_buf, size_t num_write, void *read_buf, size_t num_read);
77+
7178
extern const sensor_imu_t sensor_imu_lsm6dso;
79+
extern const sensor_ext_ssi_t sensor_ext_lsm6dso;
7280

7381
#endif

0 commit comments

Comments
 (0)