sensor_mpu9250: Fix timing in command_query_mpu9250_status()

Commit 80a7744b optimized the fifo tracking code.  However, it
introduced an error in the time tracking in
command_query_mpu9250_status().  The purpose of that function is to
provide a precise timestamp of the total number of messages produced
at the time of that call.  Thus, the returned fifo value needs to be
the fifo level in the chip at the time of the call (not the value read
during previous checks).

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

1 files changed, 9 insertions, 8 deletions

diff --git a/src/sensor_mpu9250.c b/src/sensor_mpu9250.c
index 51df5a71..86508a6e 100644
--- a/src/sensor_mpu9250.c
+++ b/src/sensor_mpu9250.c
@@ -65,10 +65,10 @@ get_fifo_status (struct mpu9250 *mp)
     uint8_t reg[] = {AR_FIFO_COUNT_H};
     uint8_t msg[2];
     i2c_read(mp->i2c->i2c_config, sizeof(reg), reg, sizeof(msg), msg);
-    msg[0] = 0x1F & msg[0]; // discard 3 MSB per datasheet
-    uint16_t bytes_to_read = ((uint16_t)msg[0]) << 8 | msg[1];
-    if (bytes_to_read > mp->fifo_max) mp->fifo_max = bytes_to_read;
-    return bytes_to_read;
+    uint16_t fifo_bytes = ((msg[0] & 0x1f) << 8) | msg[1];
+    if (fifo_bytes > mp->fifo_max)
+        mp->fifo_max = fifo_bytes;
+    return fifo_bytes;
 }
 
 // Event handler that wakes mpu9250_task() periodically
@@ -249,14 +249,15 @@ void
 command_query_mpu9250_status(uint32_t *args)
 {
     struct mpu9250 *mp = oid_lookup(args[0], command_config_mpu9250);
+    uint8_t reg[] = {AR_FIFO_COUNT_H};
     uint8_t msg[2];
+
     uint32_t time1 = timer_read_time();
-    uint8_t reg[] = {AR_FIFO_COUNT_H};
     i2c_read(mp->i2c->i2c_config, sizeof(reg), reg, sizeof(msg), msg);
     uint32_t time2 = timer_read_time();
-    msg[0] = 0x1F & msg[0]; // discard 3 MSB
-    mp9250_status(mp, args[0], time1, time2, mp->fifo_pkts_bytes
-                  / BYTES_PER_FIFO_ENTRY);
+
+    uint16_t fifo_bytes = ((msg[0] & 0x1f) << 8) | msg[1];
+    mp9250_status(mp, args[0], time1, time2, fifo_bytes / BYTES_PER_FIFO_ENTRY);
 }
 DECL_COMMAND(command_query_mpu9250_status, "query_mpu9250_status oid=%c");