From 0f460e60899e79bdb382d33f6f4f0b73f2c74cdf Mon Sep 17 00:00:00 2001 From: corruptbear Date: Tue, 14 Nov 2023 15:07:22 -0800 Subject: [PATCH] Use read_int16_vector to get accelerations; use structure to store accelerations --- .../firmware/src/peripherals/include/imu.h | 12 ++++++-- software/firmware/src/peripherals/src/imu.c | 30 +++++++++---------- .../firmware/tests/peripherals/test_imu.c | 16 +++++----- 3 files changed, 32 insertions(+), 26 deletions(-) diff --git a/software/firmware/src/peripherals/include/imu.h b/software/firmware/src/peripherals/include/imu.h index 82279c26..76c05a01 100644 --- a/software/firmware/src/peripherals/include/imu.h +++ b/software/firmware/src/peripherals/include/imu.h @@ -230,6 +230,12 @@ typedef struct { int16_t y; int16_t z; } bno055_quaternion_t; + +typedef struct { + int16_t x; + int16_t y; + int16_t z; +} bno055_acc_t; // Peripheral Type Definitions ----------------------------------------------------------------------------------------- typedef void (*motion_change_callback_t)(bool in_motion); @@ -240,9 +246,9 @@ typedef void (*motion_change_callback_t)(bool in_motion); void imu_init(void); void imu_deinit(void); void imu_register_motion_change_callback(motion_change_callback_t callback, bno055_opmode_t mode); -void imu_read_accel_data(int16_t *x, int16_t *y, int16_t *z); -void imu_read_linear_accel_data(int16_t *x, int16_t *y, int16_t *z); -void imu_read_gravity_accel_data(int16_t *x, int16_t *y, int16_t *z); +void imu_read_accel_data(bno055_acc_t *acc); +void imu_read_linear_accel_data(bno055_acc_t *acc); +void imu_read_gravity_accel_data(bno055_acc_t *acc); void imu_read_quaternion_data(bno055_quaternion_t *quaternion); void imu_read_gyro_data(int16_t *x, int16_t *y, int16_t *z); void imu_read_temp(int8_t *temp); diff --git a/software/firmware/src/peripherals/src/imu.c b/software/firmware/src/peripherals/src/imu.c index 028e2f5b..687496c6 100644 --- a/software/firmware/src/peripherals/src/imu.c +++ b/software/firmware/src/peripherals/src/imu.c @@ -234,29 +234,29 @@ void imu_register_motion_change_callback(motion_change_callback_t callback, bno0 set_mode(mode); } -void imu_read_accel_data(int16_t *x, int16_t *y, int16_t *z) +void imu_read_accel_data(bno055_acc_t *acc) { static int16_t accel_data[3]; - i2c_read(BNO055_ACCEL_DATA_X_LSB_ADDR, (uint8_t*)accel_data, sizeof(accel_data)); - *x = (int16_t)(accel_data[0] << 2) / 4; - *y = (int16_t)(accel_data[1] << 2) / 4; - *z = (int16_t)(accel_data[2] << 2) / 4; + read_int16_vector(BNO055_ACCEL_DATA_X_LSB_ADDR, accel_data, sizeof(accel_data)); + acc->x = accel_data[0]; + acc->y = accel_data[1]; + acc->z = accel_data[2]; } -void imu_read_linear_accel_data(int16_t *x, int16_t *y, int16_t *z){ +void imu_read_linear_accel_data(bno055_acc_t *acc){ static int16_t accel_data[3]; - i2c_read(BNO055_LINEAR_ACCEL_DATA_X_LSB_ADDR, (uint8_t*)accel_data, sizeof(accel_data)); - *x = (int16_t)(accel_data[0] << 2) / 4; - *y = (int16_t)(accel_data[1] << 2) / 4; - *z = (int16_t)(accel_data[2] << 2) / 4; + read_int16_vector(BNO055_LINEAR_ACCEL_DATA_X_LSB_ADDR, accel_data, sizeof(accel_data)); + acc->x = accel_data[0]; + acc->y = accel_data[1]; + acc->z = accel_data[2]; } -void imu_read_gravity_accel_data(int16_t *x, int16_t *y, int16_t *z){ +void imu_read_gravity_accel_data(bno055_acc_t *acc){ static int16_t accel_data[3]; - i2c_read(BNO055_GRAVITY_DATA_X_LSB_ADDR, (uint8_t*)accel_data, sizeof(accel_data)); - *x = (int16_t)(accel_data[0] << 2) / 4; - *y = (int16_t)(accel_data[1] << 2) / 4; - *z = (int16_t)(accel_data[2] << 2) / 4; + read_int16_vector(BNO055_GRAVITY_DATA_X_LSB_ADDR, accel_data, sizeof(accel_data)); + acc->x = accel_data[0]; + acc->y = accel_data[1]; + acc->z = accel_data[2]; } void imu_read_quaternion_data(bno055_quaternion_t *quaternion){ diff --git a/software/firmware/tests/peripherals/test_imu.c b/software/firmware/tests/peripherals/test_imu.c index 3e017f5c..b1ba820d 100644 --- a/software/firmware/tests/peripherals/test_imu.c +++ b/software/firmware/tests/peripherals/test_imu.c @@ -22,6 +22,7 @@ int main(void) bno055_calib_offsets_t offsets; bno055_axis_remap_t remap; bno055_quaternion_t quaternion; + bno055_acc_t acc; imu_register_motion_change_callback(motion_interrupt, OPERATION_MODE_NDOF); @@ -34,14 +35,13 @@ int main(void) while (true) { am_hal_delay_us(20000); - //imu_read_accel_data(&x, &y, &z); - //print("Accel X = %d, Y = %d, Z = %d\n", (int32_t)x, (int32_t)y, (int32_t)z); - imu_read_linear_accel_data(&x, &y, &z); - imu_read_quaternion_data(&quaternion); - print("Linear Accel X = %d, Y = %d, Z = %d, qw = %d, qx = %d, qy = %d, qz = %d\n", (int32_t)x, (int32_t)y, (int32_t)z, (int32_t)quaternion.w, (int32_t)quaternion.x, (int32_t)quaternion.y, (int32_t)quaternion.z); - //imu_read_gravity_accel_data(&x, &y, &z); - //print("Gravity Accel X = %d, Y = %d, Z = %d\n", (int32_t)x, (int32_t)y, (int32_t)z); - //print("Quaternion W = %d, X = %d, Y = %d, Z = %d\n", (int32_t)w, (int32_t)x, (int32_t)y, (int32_t)z); + //imu_read_accel_data(&acc); + //print("Accel X = %d, Y = %d, Z = %d\n", (int32_t)acc.x, (int32_t)acc.y, (int32_t)acc.z); + imu_read_linear_accel_data(&acc); + imu_read_quaternion_data(&quaternion); + print("Linear Accel X = %d, Y = %d, Z = %d, qw = %d, qx = %d, qy = %d, qz = %d\n", (int32_t)acc.x, (int32_t)acc.y, (int32_t)acc.z, (int32_t)quaternion.w, (int32_t)quaternion.x, (int32_t)quaternion.y, (int32_t)quaternion.z); + //imu_read_gravity_accel_data(&acc); + //print("Gravity Accel X = %d, Y = %d, Z = %d\n", (int32_t)acc.x, (int32_t)acc.y, (int32_t)acc.z); //imu_read_gyro_data(&x, &y, &z); //print("gyro 1 = %d, gyro 2 = %d, gyro 3 = %d\n", (int32_t)x, (int32_t)y, (int32_t)z); //imu_read_temp(&temp);