From 499e91b6ec2e5e9207de688742fa1b54651cbf60 Mon Sep 17 00:00:00 2001 From: Jannis Bergmann Date: Wed, 3 Jul 2024 00:09:31 +0200 Subject: [PATCH] Add rotation of value vector for IMU --- src/drivers/ao_bmi088.c | 20 ++++++++++++++++++-- 1 file changed, 18 insertions(+), 2 deletions(-) diff --git a/src/drivers/ao_bmi088.c b/src/drivers/ao_bmi088.c index 96e995f..977651f 100644 --- a/src/drivers/ao_bmi088.c +++ b/src/drivers/ao_bmi088.c @@ -60,12 +60,20 @@ _ao_bmi088_acc_sample_read(struct ao_bmi088_acc_sample *sample) { uint8_t dummy; uint8_t addr = BMI088_ACC_DATA | 0x80; + struct ao_bmi088_acc_sample raw; + float magic_number = (float)0.70710678118654752440084436210484; // 1/sqrt(2); could use double for higher percision + ao_bmi088_acc_start(); ao_spi_send(&addr, 1, AO_BMI088_SPI_BUS); ao_spi_recv(&dummy, 1, AO_BMI088_SPI_BUS); /* part sends garbage for first byte */ - ao_spi_recv(sample, sizeof(struct ao_bmi088_acc_sample), AO_BMI088_SPI_BUS); + ao_spi_recv(&raw, sizeof(struct ao_bmi088_acc_sample), AO_BMI088_SPI_BUS); ao_bmi088_acc_end(); + + // rotate value vector by 45 degrees mathimatical positice around z-axis + sample->x = (int16_t)((raw.x + raw.y) * magic_number); + sample->y = (int16_t)((raw.x - raw.y) * magic_number); + sample->z = raw.z; } static void @@ -98,7 +106,15 @@ _ao_bmi088_gyr_reg_write(uint8_t addr, uint8_t value) static void _ao_bmi088_gyr_sample_read(struct ao_bmi088_gyr_sample *sample) { - _ao_bmi088_gyr_read(BMI088_GYRO_DATA, sample, sizeof (struct ao_bmi088_gyr_sample)); + struct ao_bmi088_gyr_sample raw; + float magic_number = (float)0.70710678118654752440084436210484; // 1/sqrt(2); could use double for higher percision + + _ao_bmi088_gyr_read(BMI088_GYRO_DATA, &raw, sizeof (struct ao_bmi088_gyr_sample)); + + // rotate value vector by 45 degrees mathimatical negative around z-axis + sample->x = (int16_t)((raw.x + raw.y) * magic_number); + sample->y = (int16_t)((raw.x - raw.y) * magic_number); + sample->z = raw.z; } static void