sensor_mpu9250: Check for overflows on each query_mpu9250_status command

Move overflow detection from mp9250_stop() to
command_query_mpu9250_status().  Currently the host ignores any
contents returned from a stop request, so overflow reporting at that
point has limited utility.

In practice, this change will result in one additional i2c transaction
to the mpu9250 device every 100ms.

Signed-off-by: Kevin O'Connor <kevin@koconnor.net>

1 files changed, 12 insertions, 10 deletions

diff --git a/src/sensor_mpu9250.c b/src/sensor_mpu9250.c
index 68dd502f..c535d097 100644
--- a/src/sensor_mpu9250.c
+++ b/src/sensor_mpu9250.c
@@ -196,14 +196,6 @@ mp9250_stop(struct mpu9250 *mp, uint8_t oid)
     i2c_write(mp->i2c->i2c_config, sizeof(msg), msg);
     uint32_t end2_time = timer_read_time();
 
-    // Detect if a FIFO overrun occured
-    uint8_t int_reg[] = {AR_INT_STATUS};
-    uint8_t int_msg;
-    i2c_read(mp->i2c->i2c_config, sizeof(int_reg), int_reg, sizeof(int_msg),
-                &int_msg);
-    if (int_msg & FIFO_OVERFLOW_INT)
-        mp->limit_count++;
-
     // Report final data
     if (mp->data_count > 0)
         mp9250_report(mp, oid);
@@ -245,14 +237,24 @@ void
 command_query_mpu9250_status(uint32_t *args)
 {
     struct mpu9250 *mp = oid_lookup(args[0], command_config_mpu9250);
+
+    // Detect if a FIFO overrun occurred
+    uint8_t int_reg[] = {AR_INT_STATUS};
+    uint8_t int_msg;
+    i2c_read(mp->i2c->i2c_config, sizeof(int_reg), int_reg, sizeof(int_msg),
+             &int_msg);
+    if (int_msg & FIFO_OVERFLOW_INT)
+        mp->limit_count++;
+
+    // Read latest FIFO count (with precise timing)
     uint8_t reg[] = {AR_FIFO_COUNT_H};
     uint8_t msg[2];
-
     uint32_t time1 = timer_read_time();
     i2c_read(mp->i2c->i2c_config, sizeof(reg), reg, sizeof(msg), msg);
     uint32_t time2 = timer_read_time();
-
     uint16_t fifo_bytes = ((msg[0] & 0x1f) << 8) | msg[1];
+
+    // Report status
     mp9250_status(mp, args[0], time1, time2, fifo_bytes / BYTES_PER_FIFO_ENTRY);
 }
 DECL_COMMAND(command_query_mpu9250_status, "query_mpu9250_status oid=%c");