gyro: refactor update functions

BossHobby · Nov 1, 2023 · cb5e849 · cb5e849
1 parent 5d443c4
commit cb5e849
Show file tree

Hide file tree

Showing 6 changed files with 38 additions and 38 deletions.
diff --git a/src/driver/spi_bmi270.c b/src/driver/spi_bmi270.c
@@ -202,15 +202,12 @@ void bmi270_read_gyro_data(gyro_data_t *data) {
   spi_bus_device_reconfigure(&gyro_bus, SPI_MODE_TRAILING_EDGE, SPI_SPEED_FAST);
 
   uint8_t buf[12];
-
   const spi_txn_segment_t gyro_segs[] = {
       spi_make_seg_const(BMI270_REG_ACC_DATA_X_LSB | 0x80),
       spi_make_seg_const(0xFF),
       spi_make_seg_buffer(buf, NULL, 12),
   };
-  spi_seg_submit(&gyro_bus, NULL, gyro_segs);
-
-  spi_txn_wait(&gyro_bus);
+  spi_seg_submit_wait(&gyro_bus, gyro_segs);
 
   data->accel.axis[0] = -(int16_t)((buf[1] << 8) | buf[0]);
   data->accel.axis[1] = -(int16_t)((buf[3] << 8) | buf[2]);

diff --git a/src/driver/spi_gyro.c b/src/driver/spi_gyro.c
@@ -103,7 +103,7 @@ uint8_t gyro_spi_init() {
 }
 
 gyro_data_t gyro_spi_read() {
-  gyro_data_t data;
+  static gyro_data_t data;
 
   switch (gyro_type) {
   case GYRO_TYPE_MPU6000:
@@ -112,36 +112,13 @@ gyro_data_t gyro_spi_read() {
   case GYRO_TYPE_ICM20602:
   case GYRO_TYPE_ICM20608:
   case GYRO_TYPE_ICM20689: {
-    uint8_t buf[14];
-    mpu6xxx_read_data(MPU_RA_ACCEL_XOUT_H, buf, 14);
-
-    data.accel.axis[0] = -(int16_t)((buf[0] << 8) | buf[1]);
-    data.accel.axis[1] = -(int16_t)((buf[2] << 8) | buf[3]);
-    data.accel.axis[2] = (int16_t)((buf[4] << 8) | buf[5]);
-
-    data.temp = (float)((int16_t)((buf[6] << 8) | buf[7])) / 333.87f + 21.f;
-
-    data.gyro.axis[1] = (int16_t)((buf[8] << 8) | buf[9]);
-    data.gyro.axis[0] = (int16_t)((buf[10] << 8) | buf[11]);
-    data.gyro.axis[2] = (int16_t)((buf[12] << 8) | buf[13]);
+    mpu6xxx_read_gyro_data(&data);
     break;
   }
 
   case GYRO_TYPE_ICM42605:
   case GYRO_TYPE_ICM42688P: {
-    uint8_t buf[14];
-    icm42605_read_data(ICM42605_TEMP_DATA1, buf, 14);
-
-    data.temp = (float)((int16_t)((buf[0] << 8) | buf[1])) / 132.48f + 25.f;
-
-    data.accel.axis[0] = -(int16_t)((buf[2] << 8) | buf[3]);
-    data.accel.axis[1] = -(int16_t)((buf[4] << 8) | buf[5]);
-    data.accel.axis[2] = (int16_t)((buf[6] << 8) | buf[7]);
-
-    data.gyro.axis[1] = (int16_t)((buf[8] << 8) | buf[9]);
-    data.gyro.axis[0] = (int16_t)((buf[10] << 8) | buf[11]);
-    data.gyro.axis[2] = (int16_t)((buf[12] << 8) | buf[13]);
-
+    icm42605_read_gyro_data(&data);
     break;
   }
 

diff --git a/src/driver/spi_icm42605.c b/src/driver/spi_icm42605.c
@@ -107,13 +107,24 @@ void icm42605_write(uint8_t reg, uint8_t data) {
   spi_seg_submit_wait(&gyro_bus, segs);
 }
 
-void icm42605_read_data(uint8_t reg, uint8_t *data, uint32_t size) {
+void icm42605_read_gyro_data(gyro_data_t *data) {
   spi_bus_device_reconfigure(&gyro_bus, SPI_MODE_TRAILING_EDGE, SPI_SPEED_FAST);
 
+  uint8_t buf[14];
   const spi_txn_segment_t segs[] = {
-      spi_make_seg_const(reg | 0x80),
-      spi_make_seg_buffer(data, NULL, size),
+      spi_make_seg_const(ICM42605_TEMP_DATA1 | 0x80),
+      spi_make_seg_buffer(buf, NULL, 14),
   };
   spi_seg_submit_wait(&gyro_bus, segs);
+
+  data->temp = (float)((int16_t)((buf[0] << 8) | buf[1])) / 132.48f + 25.f;
+
+  data->accel.axis[0] = -(int16_t)((buf[2] << 8) | buf[3]);
+  data->accel.axis[1] = -(int16_t)((buf[4] << 8) | buf[5]);
+  data->accel.axis[2] = (int16_t)((buf[6] << 8) | buf[7]);
+
+  data->gyro.axis[1] = (int16_t)((buf[8] << 8) | buf[9]);
+  data->gyro.axis[0] = (int16_t)((buf[10] << 8) | buf[11]);
+  data->gyro.axis[2] = (int16_t)((buf[12] << 8) | buf[13]);
 }
 #endif
diff --git a/src/driver/spi_icm42605.h b/src/driver/spi_icm42605.h
@@ -2,6 +2,8 @@
 
 #include <stdint.h>
 
+#include "driver/spi_gyro.h"
+
 // Bank 0
 #define ICM42605_DEVICE_CONFIG 0x11
 #define ICM42605_DRIVE_CONFIG 0x13
@@ -196,4 +198,4 @@ void icm42605_configure();
 void icm42605_write(uint8_t reg, uint8_t data);
 
 uint8_t icm42605_read(uint8_t reg);
-void icm42605_read_data(uint8_t reg, uint8_t *data, uint32_t size);
+void icm42605_read_gyro_data(gyro_data_t *data);
diff --git a/src/driver/spi_mpu6xxx.c b/src/driver/spi_mpu6xxx.c
@@ -112,14 +112,25 @@ void mpu6xxx_write(uint8_t reg, uint8_t data) {
   spi_seg_submit_wait(&gyro_bus, segs);
 }
 
-void mpu6xxx_read_data(uint8_t reg, uint8_t *data, uint32_t size) {
+void mpu6xxx_read_gyro_data(gyro_data_t *data) {
   spi_bus_device_reconfigure(&gyro_bus, SPI_MODE_TRAILING_EDGE, mpu6xxx_fast_divider());
 
+  uint8_t buf[14];
   const spi_txn_segment_t segs[] = {
-      spi_make_seg_const(reg | 0x80),
-      spi_make_seg_buffer(data, NULL, size),
+      spi_make_seg_const(MPU_RA_ACCEL_XOUT_H | 0x80),
+      spi_make_seg_buffer(buf, NULL, 14),
   };
   spi_seg_submit_wait(&gyro_bus, segs);
+
+  data->accel.axis[0] = -(int16_t)((buf[0] << 8) | buf[1]);
+  data->accel.axis[1] = -(int16_t)((buf[2] << 8) | buf[3]);
+  data->accel.axis[2] = (int16_t)((buf[4] << 8) | buf[5]);
+
+  data->temp = (float)((int16_t)((buf[6] << 8) | buf[7])) / 333.87f + 21.f;
+
+  data->gyro.axis[1] = (int16_t)((buf[8] << 8) | buf[9]);
+  data->gyro.axis[0] = (int16_t)((buf[10] << 8) | buf[11]);
+  data->gyro.axis[2] = (int16_t)((buf[12] << 8) | buf[13]);
 }
 
 #endif
diff --git a/src/driver/spi_mpu6xxx.h b/src/driver/spi_mpu6xxx.h
@@ -2,6 +2,8 @@
 
 #include <stdint.h>
 
+#include "driver/spi_gyro.h"
+
 #define MPU_BIT_SLEEP 0x40
 #define MPU_BIT_H_RESET 0x80
 #define MPU_BITS_CLKSEL 0x07
@@ -131,4 +133,4 @@ void mpu6xxx_configure();
 void mpu6xxx_write(uint8_t reg, uint8_t data);
 
 uint8_t mpu6xxx_read(uint8_t reg);
-void mpu6xxx_read_data(uint8_t reg, uint8_t *data, uint32_t size);
+void mpu6xxx_read_gyro_data(gyro_data_t *data);