From f06162e12c9b949eb4fd0b7544ec491965f52473 Mon Sep 17 00:00:00 2001 From: bkleiner Date: Thu, 8 Aug 2024 19:10:19 +0200 Subject: [PATCH] gyro: use common buffer --- src/driver/gyro/bmi270.c | 11 ++++------- src/driver/gyro/bmi323.c | 17 ++++++++--------- src/driver/gyro/gyro.c | 1 + src/driver/gyro/icm42605.c | 19 +++++++++---------- src/driver/gyro/mpu6xxx.c | 19 +++++++++---------- 5 files changed, 31 insertions(+), 36 deletions(-) diff --git a/src/driver/gyro/bmi270.c b/src/driver/gyro/bmi270.c index ade3bd400..a396c6e6c 100644 --- 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,6 +222,8 @@ 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), @@ -230,14 +231,10 @@ void bmi270_read_gyro_data(gyro_data_t *data) { }; 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 index 17a69aaee..71b43c574 100644 --- 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 index 97d4de3d6..8cd822d74 100644 --- 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 index 10e1b3f9f..94f480823 100644 --- 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 index 45fcb9583..b1c2e72e2 100644 --- 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))