ALPAKA/TeleStern
1
0
Fork 0
Code Issues 1 Pull Requests Actions Packages Projects Releases Wiki Activity

Add rotation of value vector for IMU

Browse Source
This commit is contained in:
Jannis Bergmann 2024-07-03 00:09:31 +02:00
parent 8d6ff4dd0a
commit 499e91b6ec
1 changed files with 18 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
src/drivers/ao_bmi088.c
View File

@ -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