diff options
| author | Timofey Titovets <nefelim4ag@gmail.com> | 2024-10-23 02:22:07 +0200 |
|---|---|---|
| committer | KevinOConnor <kevin@koconnor.net> | 2024-10-26 22:06:30 -0400 |
| commit | 08102a0bf9e56a0cd61badf640903a3e0332215d (patch) | |
| tree | 8855a3c79321fa5baa9cd8d199272529f4f912f9 /src/sensor_mpu9250.c | |
| parent | 1c3b30b8159717a416e2dcc9bf92038151f1fcd5 (diff) | |
| download | kutter-08102a0bf9e56a0cd61badf640903a3e0332215d.tar.gz kutter-08102a0bf9e56a0cd61badf640903a3e0332215d.tar.xz kutter-08102a0bf9e56a0cd61badf640903a3e0332215d.zip | |
mpu: shutdown on i2c errors
Signed-off-by: Timofey Titovets <nefelim4ag@gmail.com>
Diffstat (limited to 'src/sensor_mpu9250.c')
| -rw-r--r-- | src/sensor_mpu9250.c | 18 |
1 files changed, 12 insertions, 6 deletions
diff --git a/src/sensor_mpu9250.c b/src/sensor_mpu9250.c index ee04e922..7476734c 100644 --- a/src/sensor_mpu9250.c +++ b/src/sensor_mpu9250.c @@ -70,13 +70,21 @@ mp9250_reschedule_timer(struct mpu9250 *mp) irq_enable(); } +static void +read_mpu(struct i2cdev_s *i2c, uint8_t reg_len, uint8_t *reg + , uint8_t read_len, uint8_t *read) +{ + int ret = i2c_dev_read(i2c, reg_len, reg, read_len, read); + i2c_shutdown_on_err(ret); +} + // Reads the fifo byte count from the device. static uint16_t get_fifo_status(struct mpu9250 *mp) { uint8_t reg[] = {AR_FIFO_COUNT_H}; uint8_t msg[2]; - i2c_dev_read(mp->i2c, sizeof(reg), reg, sizeof(msg), msg); + read_mpu(mp->i2c, sizeof(reg), reg, sizeof(msg), msg); uint16_t fifo_bytes = ((msg[0] & 0x1f) << 8) | msg[1]; if (fifo_bytes > mp->fifo_max) mp->fifo_max = fifo_bytes; @@ -94,8 +102,7 @@ mp9250_query(struct mpu9250 *mp, uint8_t oid) // If we have enough bytes to fill the buffer do it and send report if (mp->fifo_pkts_bytes >= BYTES_PER_BLOCK) { uint8_t reg = AR_FIFO; - i2c_dev_read(mp->i2c, sizeof(reg), ® - , BYTES_PER_BLOCK, &mp->sb.data[0]); + read_mpu(mp->i2c, sizeof(reg), ®, BYTES_PER_BLOCK, &mp->sb.data[0]); mp->sb.data_count = BYTES_PER_BLOCK; mp->fifo_pkts_bytes -= BYTES_PER_BLOCK; sensor_bulk_report(&mp->sb, oid); @@ -143,8 +150,7 @@ command_query_mpu9250_status(uint32_t *args) // Detect if a FIFO overrun occurred uint8_t int_reg[] = {AR_INT_STATUS}; uint8_t int_msg; - i2c_dev_read(mp->i2c, sizeof(int_reg), int_reg, sizeof(int_msg), - &int_msg); + read_mpu(mp->i2c, sizeof(int_reg), int_reg, sizeof(int_msg), &int_msg); if (int_msg & FIFO_OVERFLOW_INT) mp->sb.possible_overflows++; @@ -152,7 +158,7 @@ command_query_mpu9250_status(uint32_t *args) uint8_t reg[] = {AR_FIFO_COUNT_H}; uint8_t msg[2]; uint32_t time1 = timer_read_time(); - i2c_dev_read(mp->i2c, sizeof(reg), reg, sizeof(msg), msg); + read_mpu(mp->i2c, sizeof(reg), reg, sizeof(msg), msg); uint32_t time2 = timer_read_time(); uint16_t fifo_bytes = ((msg[0] & 0x1f) << 8) | msg[1]; |