CreatBot/CreatBotKlipper
CreatBot/CreatBotKlipper
7
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

mpu: shutdown on i2c errors

Browse Source 
Signed-off-by: Timofey Titovets <nefelim4ag@gmail.com>
This commit is contained in:
Timofey Titovets 2024-10-23 02:22:07 +02:00 committed by KevinOConnor
parent 1c3b30b815
commit 08102a0bf9
1 changed files with 12 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
src/sensor_mpu9250.c
View File

@ -70,13 +70,21 @@ mp9250_reschedule_timer(struct mpu9250 *mp)
irq_enable(); 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. // Reads the fifo byte count from the device.
static uint16_t static uint16_t
get_fifo_status(struct mpu9250 *mp) get_fifo_status(struct mpu9250 *mp)
{ {
uint8_t reg[] = {AR_FIFO_COUNT_H}; uint8_t reg[] = {AR_FIFO_COUNT_H};
uint8_t msg[2]; 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]; uint16_t fifo_bytes = ((msg[0] & 0x1f) << 8) | msg[1];
if (fifo_bytes > mp->fifo_max) if (fifo_bytes > mp->fifo_max)
mp->fifo_max = fifo_bytes; 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 we have enough bytes to fill the buffer do it and send report
if (mp->fifo_pkts_bytes >= BYTES_PER_BLOCK) { if (mp->fifo_pkts_bytes >= BYTES_PER_BLOCK) {
uint8_t reg = AR_FIFO; uint8_t reg = AR_FIFO;
i2c_dev_read(mp->i2c, sizeof(reg), &reg read_mpu(mp->i2c, sizeof(reg), &reg, BYTES_PER_BLOCK, &mp->sb.data[0]);
, BYTES_PER_BLOCK, &mp->sb.data[0]);
mp->sb.data_count = BYTES_PER_BLOCK; mp->sb.data_count = BYTES_PER_BLOCK;
mp->fifo_pkts_bytes -= BYTES_PER_BLOCK; mp->fifo_pkts_bytes -= BYTES_PER_BLOCK;
sensor_bulk_report(&mp->sb, oid); sensor_bulk_report(&mp->sb, oid);
@ -143,8 +150,7 @@ command_query_mpu9250_status(uint32_t *args)
// Detect if a FIFO overrun occurred // Detect if a FIFO overrun occurred
uint8_t int_reg[] = {AR_INT_STATUS}; uint8_t int_reg[] = {AR_INT_STATUS};
uint8_t int_msg; uint8_t int_msg;
i2c_dev_read(mp->i2c, sizeof(int_reg), int_reg, sizeof(int_msg), read_mpu(mp->i2c, sizeof(int_reg), int_reg, sizeof(int_msg), &int_msg);
&int_msg);
if (int_msg & FIFO_OVERFLOW_INT) if (int_msg & FIFO_OVERFLOW_INT)
mp->sb.possible_overflows++; mp->sb.possible_overflows++;
@ -152,7 +158,7 @@ command_query_mpu9250_status(uint32_t *args)
uint8_t reg[] = {AR_FIFO_COUNT_H}; uint8_t reg[] = {AR_FIFO_COUNT_H};
uint8_t msg[2]; uint8_t msg[2];
uint32_t time1 = timer_read_time(); 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(); uint32_t time2 = timer_read_time();
uint16_t fifo_bytes = ((msg[0] & 0x1f) << 8) | msg[1]; uint16_t fifo_bytes = ((msg[0] & 0x1f) << 8) | msg[1];