3diy
/
klipper
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

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>
This commit is contained in:
Kevin O'Connor 2023-12-17 16:19:21 -05:00
parent 1a1568c38b
commit 49315b3cc4
1 changed files with 10 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
src/sensor_mpu9250.c
View File

@ -65,10 +65,10 @@ 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_read(mp->i2c->i2c_config, sizeof(reg), reg, sizeof(msg), msg); i2c_read(mp->i2c->i2c_config, sizeof(reg), reg, sizeof(msg), msg);
msg[0] = 0x1F & msg[0]; // discard 3 MSB per datasheet uint16_t fifo_bytes = ((msg[0] & 0x1f) << 8) | msg[1];
uint16_t bytes_to_read = ((uint16_t)msg[0]) << 8 | msg[1]; if (fifo_bytes > mp->fifo_max)
if (bytes_to_read > mp->fifo_max) mp->fifo_max = bytes_to_read; mp->fifo_max = fifo_bytes;
return bytes_to_read; return fifo_bytes;
} }
// Event handler that wakes mpu9250_task() periodically // Event handler that wakes mpu9250_task() periodically
@ -249,14 +249,15 @@ void
command_query_mpu9250_status(uint32_t *args) command_query_mpu9250_status(uint32_t *args)
{ {
struct mpu9250 *mp = oid_lookup(args[0], command_config_mpu9250); struct mpu9250 *mp = oid_lookup(args[0], command_config_mpu9250);
uint8_t msg[2];
uint32_t time1 = timer_read_time();
uint8_t reg[] = {AR_FIFO_COUNT_H}; 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); i2c_read(mp->i2c->i2c_config, sizeof(reg), reg, sizeof(msg), msg);
uint32_t time2 = timer_read_time(); 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 uint16_t fifo_bytes = ((msg[0] & 0x1f) << 8) | msg[1];
/ BYTES_PER_FIFO_ENTRY); mp9250_status(mp, args[0], time1, time2, fifo_bytes / BYTES_PER_FIFO_ENTRY);
} }
DECL_COMMAND(command_query_mpu9250_status, "query_mpu9250_status oid=%c"); DECL_COMMAND(command_query_mpu9250_status, "query_mpu9250_status oid=%c");