From a326c126adbe6ed20962597e2566b944f819a72e Mon Sep 17 00:00:00 2001 From: muiseac <61459612+muiseac@users.noreply.github.com> Date: Thu, 20 Nov 2025 15:17:23 +0100 Subject: [PATCH 1/4] Update driver_mpu6050.c fifo fix. --- src/driver_mpu6050.c | 81 ++++++++++++++++++++++++++++++++++++-------- 1 file changed, 67 insertions(+), 14 deletions(-) diff --git a/src/driver_mpu6050.c b/src/driver_mpu6050.c index 22bc3cd..8e0921f 100644 --- a/src/driver_mpu6050.c +++ b/src/driver_mpu6050.c @@ -3954,13 +3954,24 @@ uint8_t mpu6050_read(mpu6050_handle_t *handle, int16_t (*accel_raw)[3], float (* return 1; /* return error */ } - if (conf != 0x78) /* check the conf */ + + // assume no slave writes to fifo FIFO_EN = 0bXXXXX000 + if (conf&0x07) /* check the conf */ { handle->debug_print("mpu6050: fifo conf is error.\n"); /* fifo conf is error */ return 6; /* return error */ } + uint8_t accel_fifo_en = conf & 0x08; + uint8_t zg_fifo_en = conf & 0x10; + uint8_t yg_fifo_en = conf & 0x20; + uint8_t xg_fifo_en = conf & 0x40; + uint8_t temp_fifo_en = conf & 0x80; + + uint8_t packet_size = accel_fifo_en*4 + (zg_fifo_en + yg_fifo_en + xg_fifo_en + temp_fifo_en)*2; // packet size to be read from fifo + + res = a_mpu6050_iic_read(handle, MPU6050_REG_FIFO_COUNTH, (uint8_t *)buf, 2); /* read fifo count */ if (res != 0) /* check result */ { @@ -3970,9 +3981,9 @@ uint8_t mpu6050_read(mpu6050_handle_t *handle, int16_t (*accel_raw)[3], float (* } count = (uint16_t)(((uint16_t)buf[0] << 8) | buf[1]); /* set count */ count = (count < 1024) ? count : 1024; /* just the counter */ - count = (count < ((*len) * 12)) ? count : ((*len) * 12); /* just outer buffer size */ - count = (count / 12) * 12; /* 12 times */ - *len = count / 12; /* set the output length */ + count = (count < ((*len) * packet_size)) ? count : ((*len) * packet_size); /* just outer buffer size */ + count -= count%packet_size; /* 12 times */ + *len = count / packet_size; /* set the output length */ res = a_mpu6050_iic_read(handle, MPU6050_REG_R_W, handle->buf, count); /* read data */ if (res != 0) /* check result */ { @@ -3982,19 +3993,61 @@ uint8_t mpu6050_read(mpu6050_handle_t *handle, int16_t (*accel_raw)[3], float (* } for (i = 0; i < (*len); i++) /* *len times */ { + uint8_t offset = 0; + + if(accel_fifo_en) { + accel_raw[i][0] = (int16_t)((uint16_t)handle->buf[i * 12 + 0] << 8) | - handle->buf[i * 12 + 1]; /* set raw accel x */ + handle->buf[i * packet_size + 1]; /* set raw accel x */ accel_raw[i][1] = (int16_t)((uint16_t)handle->buf[i * 12 + 2] << 8) | - handle->buf[i * 12 + 3]; /* set raw accel y */ + handle->buf[i * packet_size + 3]; /* set raw accel y */ accel_raw[i][2] = (int16_t)((uint16_t)handle->buf[i * 12 + 4] << 8) | - handle->buf[i * 12 + 5]; /* set raw accel z */ - gyro_raw[i][0] = (int16_t)((uint16_t)handle->buf[i * 12 + 6] << 8) | - handle->buf[i * 12 + 7]; /* set raw gyro x */ - gyro_raw[i][1] = (int16_t)((uint16_t)handle->buf[i * 12 + 8] << 8) | - handle->buf[i * 12 + 9]; /* set raw gyro y */ - gyro_raw[i][2] = (int16_t)((uint16_t)handle->buf[i * 12 + 10] << 8) | - handle->buf[i * 12 + 11]; /* set raw gyro z */ - + handle->buf[i * packet_size + 5]; /* set raw accel z */ + offset += 4; + + }else { + accel_raw[i][0] = 0; /* set raw accel x */ + accel_raw[i][1] = 0; /* set raw accel y */ + accel_raw[i][2] = 0; /* set raw accel z */ + } + if(temp_fifo_en){ + // temperature read from fifo + offset +=2; + }else { + + + } + if(xg_fifo_en){ + gyro_raw[i][0] = (int16_t)((uint16_t)handle->buf[i * 12 + offset] << 8) | + handle->buf[i * packet_size + offset+1]; /* set raw gyro x */ + offset +=2; + + }else{ + gyro_raw[i][0] = 0; + } + if(yg_fifo_en){ + gyro_raw[i][1] = (int16_t)((uint16_t)handle->buf[i * 12 + offset] << 8) | + handle->buf[i * packet_size + offset+1]; /* set raw gyro y */ + offset +=2; + + }else{ + gyro_raw[i][1] = 0; + } + if(zg_fifo_en){ + gyro_raw[i][2] = (int16_t)((uint16_t)handle->buf[i * 12 + offset] << 8) | + handle->buf[i * packet_size + offset+1]; /* set raw gyro z */ + offset +=2; + + + }else{ + gyro_raw[i][2] = 0; + } + + + + + + if (accel_conf == 0) /* ±2g */ { accel_g[i][0] = (float)(accel_raw[i][0]) / 16384.0f; /* set accel x */ From ca6ae33e4e16878ad341f3c84ba1fd0bff892529 Mon Sep 17 00:00:00 2001 From: muiseac <61459612+muiseac@users.noreply.github.com> Date: Thu, 20 Nov 2025 15:33:35 +0100 Subject: [PATCH 2/4] Update driver_mpu6050.c fifo fix --- src/driver_mpu6050.c | 37 +++++++++++++++++++------------------ 1 file changed, 19 insertions(+), 18 deletions(-) diff --git a/src/driver_mpu6050.c b/src/driver_mpu6050.c index 8e0921f..3fbab80 100644 --- a/src/driver_mpu6050.c +++ b/src/driver_mpu6050.c @@ -3963,11 +3963,11 @@ uint8_t mpu6050_read(mpu6050_handle_t *handle, int16_t (*accel_raw)[3], float (* return 6; /* return error */ } - uint8_t accel_fifo_en = conf & 0x08; - uint8_t zg_fifo_en = conf & 0x10; - uint8_t yg_fifo_en = conf & 0x20; - uint8_t xg_fifo_en = conf & 0x40; - uint8_t temp_fifo_en = conf & 0x80; + uint8_t accel_fifo_en = conf & 0x08 >> 3; + uint8_t zg_fifo_en = conf & 0x10 >>4; + uint8_t yg_fifo_en = conf & 0x20 >> 5; + uint8_t xg_fifo_en = conf & 0x40 >> 6; + uint8_t temp_fifo_en = conf & 0x80 >> 7; uint8_t packet_size = accel_fifo_en*4 + (zg_fifo_en + yg_fifo_en + xg_fifo_en + temp_fifo_en)*2; // packet size to be read from fifo @@ -3994,16 +3994,17 @@ uint8_t mpu6050_read(mpu6050_handle_t *handle, int16_t (*accel_raw)[3], float (* for (i = 0; i < (*len); i++) /* *len times */ { uint8_t offset = 0; + uint16_t base_idx = i * packet_size; if(accel_fifo_en) { - accel_raw[i][0] = (int16_t)((uint16_t)handle->buf[i * 12 + 0] << 8) | - handle->buf[i * packet_size + 1]; /* set raw accel x */ - accel_raw[i][1] = (int16_t)((uint16_t)handle->buf[i * 12 + 2] << 8) | - handle->buf[i * packet_size + 3]; /* set raw accel y */ - accel_raw[i][2] = (int16_t)((uint16_t)handle->buf[i * 12 + 4] << 8) | - handle->buf[i * packet_size + 5]; /* set raw accel z */ - offset += 4; + accel_raw[i][0] = (int16_t)((uint16_t)handle->buf[base_idx ] << 8) | + handle->buf[base_idx + 1]; /* set raw accel x */ + accel_raw[i][1] = (int16_t)((uint16_t)handle->buf[base_idx+ 2] << 8) | + handle->buf[base_idx + 3]; /* set raw accel y */ + accel_raw[i][2] = (int16_t)((uint16_t)handle->buf[base_idx + 4] << 8) | + handle->buf[base_idx + 5]; /* set raw accel z */ + offset += 6; }else { accel_raw[i][0] = 0; /* set raw accel x */ @@ -4018,24 +4019,24 @@ uint8_t mpu6050_read(mpu6050_handle_t *handle, int16_t (*accel_raw)[3], float (* } if(xg_fifo_en){ - gyro_raw[i][0] = (int16_t)((uint16_t)handle->buf[i * 12 + offset] << 8) | - handle->buf[i * packet_size + offset+1]; /* set raw gyro x */ + gyro_raw[i][0] = (int16_t)((uint16_t)handle->buf[base_idx + offset] << 8) | + handle->buf[base_idx + offset+1]; /* set raw gyro x */ offset +=2; }else{ gyro_raw[i][0] = 0; } if(yg_fifo_en){ - gyro_raw[i][1] = (int16_t)((uint16_t)handle->buf[i * 12 + offset] << 8) | - handle->buf[i * packet_size + offset+1]; /* set raw gyro y */ + gyro_raw[i][1] = (int16_t)((uint16_t)handle->buf[base_idx + offset] << 8) | + handle->buf[base_idx + offset+1]; /* set raw gyro y */ offset +=2; }else{ gyro_raw[i][1] = 0; } if(zg_fifo_en){ - gyro_raw[i][2] = (int16_t)((uint16_t)handle->buf[i * 12 + offset] << 8) | - handle->buf[i * packet_size + offset+1]; /* set raw gyro z */ + gyro_raw[i][2] = (int16_t)((uint16_t)handle->buf[base_idx + offset] << 8) | + handle->buf[base_idx + offset+1]; /* set raw gyro z */ offset +=2; From 6d75cc530562e8b2052b1b6e8abbc11bb40f77eb Mon Sep 17 00:00:00 2001 From: muiseac <61459612+muiseac@users.noreply.github.com> Date: Thu, 20 Nov 2025 15:40:25 +0100 Subject: [PATCH 3/4] Update driver_mpu6050.c fifo fix --- src/driver_mpu6050.c | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/driver_mpu6050.c b/src/driver_mpu6050.c index 3fbab80..7035478 100644 --- a/src/driver_mpu6050.c +++ b/src/driver_mpu6050.c @@ -3963,13 +3963,13 @@ uint8_t mpu6050_read(mpu6050_handle_t *handle, int16_t (*accel_raw)[3], float (* return 6; /* return error */ } - uint8_t accel_fifo_en = conf & 0x08 >> 3; - uint8_t zg_fifo_en = conf & 0x10 >>4; - uint8_t yg_fifo_en = conf & 0x20 >> 5; - uint8_t xg_fifo_en = conf & 0x40 >> 6; - uint8_t temp_fifo_en = conf & 0x80 >> 7; + uint8_t accel_fifo_en = (conf & 0x08) >> 3; // accel fifo enabled? + uint8_t zg_fifo_en = (conf & 0x10) >>4; // gyro z axis enabled? + uint8_t yg_fifo_en = (conf & 0x20) >> 5; // gyro y axis enabled? + uint8_t xg_fifo_en = (conf & 0x40) >> 6; // gyro x axis enabled? + uint8_t temp_fifo_en = (conf & 0x80) >> 7; // temp enabled? - uint8_t packet_size = accel_fifo_en*4 + (zg_fifo_en + yg_fifo_en + xg_fifo_en + temp_fifo_en)*2; // packet size to be read from fifo + uint8_t packet_size = accel_fifo_en*6 + (zg_fifo_en + yg_fifo_en + xg_fifo_en + temp_fifo_en)*2; // packet size to be read from fifo res = a_mpu6050_iic_read(handle, MPU6050_REG_FIFO_COUNTH, (uint8_t *)buf, 2); /* read fifo count */ From 5b74b793290b7308eeebb52b7c444e4c554fbd4a Mon Sep 17 00:00:00 2001 From: muiseac <61459612+muiseac@users.noreply.github.com> Date: Thu, 20 Nov 2025 15:54:11 +0100 Subject: [PATCH 4/4] Update driver_mpu6050.c add guard against divison by zero --- src/driver_mpu6050.c | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/driver_mpu6050.c b/src/driver_mpu6050.c index 7035478..11704dd 100644 --- a/src/driver_mpu6050.c +++ b/src/driver_mpu6050.c @@ -3971,7 +3971,11 @@ uint8_t mpu6050_read(mpu6050_handle_t *handle, int16_t (*accel_raw)[3], float (* uint8_t packet_size = accel_fifo_en*6 + (zg_fifo_en + yg_fifo_en + xg_fifo_en + temp_fifo_en)*2; // packet size to be read from fifo - + if(packet_size ==0){ + handle->debug_print("mpu6050: divison by zero error, packet_size=0, check fifo configuration or disable fifo.\n"); + return 1; + + } res = a_mpu6050_iic_read(handle, MPU6050_REG_FIFO_COUNTH, (uint8_t *)buf, 2); /* read fifo count */ if (res != 0) /* check result */ {