gyro: use common buffer

BossHobby · Aug 11, 2024 · f06162e · f06162e
1 parent cdd9181
commit f06162e
Show file tree

Hide file tree

Showing 5 changed files with 31 additions and 36 deletions.
diff --git a/src/driver/gyro/bmi270.c b/src/driver/gyro/bmi270.c
@@ -15,6 +15,7 @@
 #define SPI_SPEED_FAST MHZ_TO_HZ(10.5)
 
 extern spi_bus_device_t gyro_bus;
+extern uint8_t gyro_buf[32];
 extern const uint8_t bmi270_config_file[8192];
 
 static int8_t gyro_cas = 0;
@@ -198,8 +199,6 @@ void bmi270_read_gyro_data(gyro_data_t *data) {
   spi_bus_device_reconfigure(&gyro_bus, SPI_MODE_TRAILING_EDGE, SPI_SPEED_FAST);
   spi_txn_wait(&gyro_bus);
 
-  static uint8_t gyro_buf[12];
-
   data->accel.pitch = -(int16_t)((gyro_buf[1] << 8) | gyro_buf[0]);
   data->accel.roll = -(int16_t)((gyro_buf[3] << 8) | gyro_buf[2]);
   data->accel.yaw = (int16_t)((gyro_buf[5] << 8) | gyro_buf[4]);
@@ -223,21 +222,19 @@ void bmi270_read_gyro_data(gyro_data_t *data) {
   data->gyro.roll = gyro_data[1];
   data->gyro.yaw = gyro_data[2];
 
+  data->temp = (float)((int16_t)((gyro_buf[13] << 8) | gyro_buf[12])) / 512.0 + 23.0;
+
   {
     const spi_txn_segment_t segs[] = {
         spi_make_seg_const(BMI270_REG_ACC_DATA_X_LSB | 0x80, 0xFF),
         spi_make_seg_buffer(gyro_buf, NULL, 12),
     };
     spi_seg_submit(&gyro_bus, segs);
   }
-
-  static uint8_t temp_buf[2];
-  data->temp = (float)((int16_t)((temp_buf[1] << 8) | temp_buf[0])) / 512.0 + 23.0;
-
   {
     const spi_txn_segment_t segs[] = {
         spi_make_seg_const(BMI270_REG_TEMPERATURE_LSB | 0x80, 0xFF),
-        spi_make_seg_buffer(temp_buf, NULL, 2),
+        spi_make_seg_buffer(gyro_buf + 12, NULL, 2),
     };
     spi_seg_submit(&gyro_bus, segs);
   }

diff --git a/src/driver/gyro/bmi323.c b/src/driver/gyro/bmi323.c
@@ -13,6 +13,7 @@
 #define SPI_SPEED_FAST MHZ_TO_HZ(10)
 
 extern spi_bus_device_t gyro_bus;
+extern uint8_t gyro_buf[32];
 
 static int8_t gyro_cas = 0; // not ready
 
@@ -179,16 +180,14 @@ void bmi323_read_gyro_data(gyro_data_t *data) {
   spi_bus_device_reconfigure(&gyro_bus, SPI_MODE_TRAILING_EDGE, SPI_SPEED_FAST);
   spi_txn_wait(&gyro_bus);
 
-  static uint8_t buf[12];
-
-  data->accel.pitch = -(int16_t)((buf[1] << 8) | buf[0]);
-  data->accel.roll = -(int16_t)((buf[3] << 8) | buf[2]);
-  data->accel.yaw = (int16_t)((buf[5] << 8) | buf[4]);
+  data->accel.pitch = -(int16_t)((gyro_buf[1] << 8) | gyro_buf[0]);
+  data->accel.roll = -(int16_t)((gyro_buf[3] << 8) | gyro_buf[2]);
+  data->accel.yaw = (int16_t)((gyro_buf[5] << 8) | gyro_buf[4]);
 
   int16_t gyro_data[3] = {
-      (int16_t)((buf[7] << 8) | buf[6]),
-      (int16_t)((buf[9] << 8) | buf[8]),
-      (int16_t)((buf[11] << 8) | buf[10]),
+      (int16_t)((gyro_buf[7] << 8) | gyro_buf[6]),
+      (int16_t)((gyro_buf[9] << 8) | gyro_buf[8]),
+      (int16_t)((gyro_buf[11] << 8) | gyro_buf[10]),
   };
 
   const int32_t tempx = gyro_data[0] - (int16_t)(gyro_cas * (int16_t)(gyro_data[2]) / 512);
@@ -208,7 +207,7 @@ void bmi323_read_gyro_data(gyro_data_t *data) {
 
   const spi_txn_segment_t segs[] = {
       spi_make_seg_const(BMI323_REG_ACC_DATA_X_LSB | 0x80, 0xFF),
-      spi_make_seg_buffer(buf, NULL, 12),
+      spi_make_seg_buffer(gyro_buf, NULL, 12),
   };
   spi_seg_submit(&gyro_bus, segs);
   while (!spi_txn_continue(&gyro_bus))

diff --git a/src/driver/gyro/gyro.c b/src/driver/gyro/gyro.c
@@ -14,6 +14,7 @@
 gyro_types_t gyro_type = GYRO_TYPE_INVALID;
 
 spi_bus_device_t gyro_bus = {};
+uint8_t gyro_buf[32];
 
 static gyro_types_t gyro_spi_detect() {
   gyro_types_t type = GYRO_TYPE_INVALID;

diff --git a/src/driver/gyro/icm42605.c b/src/driver/gyro/icm42605.c
@@ -21,6 +21,7 @@
 #define AAF_BITSHIFT 10
 
 extern spi_bus_device_t gyro_bus;
+extern uint8_t gyro_buf[32];
 
 uint8_t icm42605_detect() {
   const uint8_t id = icm42605_read(ICM42605_WHO_AM_I);
@@ -110,21 +111,19 @@ void icm42605_read_gyro_data(gyro_data_t *data) {
   spi_bus_device_reconfigure(&gyro_bus, SPI_MODE_TRAILING_EDGE, SPI_SPEED_FAST);
   spi_txn_wait(&gyro_bus);
 
-  static uint8_t buf[14];
+  data->temp = (float)((int16_t)((gyro_buf[0] << 8) | gyro_buf[1])) / 132.48f + 25.f;
 
-  data->temp = (float)((int16_t)((buf[0] << 8) | buf[1])) / 132.48f + 25.f;
+  data->accel.pitch = -(int16_t)((gyro_buf[2] << 8) | gyro_buf[3]);
+  data->accel.roll = -(int16_t)((gyro_buf[4] << 8) | gyro_buf[5]);
+  data->accel.yaw = (int16_t)((gyro_buf[6] << 8) | gyro_buf[7]);
 
-  data->accel.pitch = -(int16_t)((buf[2] << 8) | buf[3]);
-  data->accel.roll = -(int16_t)((buf[4] << 8) | buf[5]);
-  data->accel.yaw = (int16_t)((buf[6] << 8) | buf[7]);
-
-  data->gyro.pitch = (int16_t)((buf[8] << 8) | buf[9]);
-  data->gyro.roll = (int16_t)((buf[10] << 8) | buf[11]);
-  data->gyro.yaw = (int16_t)((buf[12] << 8) | buf[13]);
+  data->gyro.pitch = (int16_t)((gyro_buf[8] << 8) | gyro_buf[9]);
+  data->gyro.roll = (int16_t)((gyro_buf[10] << 8) | gyro_buf[11]);
+  data->gyro.yaw = (int16_t)((gyro_buf[12] << 8) | gyro_buf[13]);
 
   const spi_txn_segment_t segs[] = {
       spi_make_seg_const(ICM42605_TEMP_DATA1 | 0x80),
-      spi_make_seg_buffer(buf, NULL, 14),
+      spi_make_seg_buffer(gyro_buf, NULL, 14),
   };
   spi_seg_submit(&gyro_bus, segs);
   while (!spi_txn_continue(&gyro_bus))

diff --git a/src/driver/gyro/mpu6xxx.c b/src/driver/gyro/mpu6xxx.c
@@ -22,6 +22,7 @@
 #define SPI_SPEED_INIT MHZ_TO_HZ(0.5)
 
 extern spi_bus_device_t gyro_bus;
+extern uint8_t gyro_buf[32];
 
 static uint32_t mpu6xxx_fast_divider() {
   switch (gyro_type) {
@@ -115,21 +116,19 @@ void mpu6xxx_read_gyro_data(gyro_data_t *data) {
   spi_bus_device_reconfigure(&gyro_bus, SPI_MODE_TRAILING_EDGE, mpu6xxx_fast_divider());
   spi_txn_wait(&gyro_bus);
 
-  static uint8_t buf[14];
+  data->accel.pitch = -(int16_t)((gyro_buf[0] << 8) | gyro_buf[1]);
+  data->accel.roll = -(int16_t)((gyro_buf[2] << 8) | gyro_buf[3]);
+  data->accel.yaw = (int16_t)((gyro_buf[4] << 8) | gyro_buf[5]);
 
-  data->accel.pitch = -(int16_t)((buf[0] << 8) | buf[1]);
-  data->accel.roll = -(int16_t)((buf[2] << 8) | buf[3]);
-  data->accel.yaw = (int16_t)((buf[4] << 8) | buf[5]);
+  data->temp = (float)((int16_t)((gyro_buf[6] << 8) | gyro_buf[7])) / 333.87f + 21.f;
 
-  data->temp = (float)((int16_t)((buf[6] << 8) | buf[7])) / 333.87f + 21.f;
-
-  data->gyro.pitch = (int16_t)((buf[8] << 8) | buf[9]);
-  data->gyro.roll = (int16_t)((buf[10] << 8) | buf[11]);
-  data->gyro.yaw = (int16_t)((buf[12] << 8) | buf[13]);
+  data->gyro.pitch = (int16_t)((gyro_buf[8] << 8) | gyro_buf[9]);
+  data->gyro.roll = (int16_t)((gyro_buf[10] << 8) | gyro_buf[11]);
+  data->gyro.yaw = (int16_t)((gyro_buf[12] << 8) | gyro_buf[13]);
 
   const spi_txn_segment_t segs[] = {
       spi_make_seg_const(MPU_RA_ACCEL_XOUT_H | 0x80),
-      spi_make_seg_buffer(buf, NULL, 14),
+      spi_make_seg_buffer(gyro_buf, NULL, 14),
   };
   spi_seg_submit(&gyro_bus, segs);
   while (!spi_txn_continue(&gyro_bus))