diff --git a/software/firmware/src/peripherals/include/imu.h b/software/firmware/src/peripherals/include/imu.h index 63ed55ab..b37cc23c 100644 --- a/software/firmware/src/peripherals/include/imu.h +++ b/software/firmware/src/peripherals/include/imu.h @@ -158,7 +158,13 @@ typedef enum { ACC_AM_THRE = 0x11, ACC_INT_SET = 0x12, ACC_NM_THRE = 0x15, - ACC_NM_SET = 0x16 + ACC_NM_SET = 0x16, + + //configs + BNO055_MAG_CONFIG_ADDR = 0X09, + BNO055_GYRO_CONFIG_ADDR =0X0A, + BNO055_GYRO_MODE_CONFIG_ADDR = 0X0B, + } bno055_reg_t; typedef enum { @@ -194,5 +200,10 @@ 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_quaternion_data(int16_t *w, int16_t *x, int16_t *y, int16_t *z); +void imu_read_gyro_data(int16_t *x, int16_t *y, int16_t *z); +void imu_read_temp(int8_t *temp); #endif // #ifndef __IMU_HEADER_H__ diff --git a/software/firmware/src/peripherals/src/imu.c b/software/firmware/src/peripherals/src/imu.c index 9f44f273..4703bc57 100644 --- a/software/firmware/src/peripherals/src/imu.c +++ b/software/firmware/src/peripherals/src/imu.c @@ -138,7 +138,6 @@ static void enable_motion_interrupts(void) i2c_write8(BNO055_PAGE_ID_ADDR, 0); } - // Public API Functions ------------------------------------------------------------------------------------------------ void imu_init(void) @@ -230,4 +229,43 @@ void imu_read_accel_data(int16_t *x, int16_t *y, int16_t *z) *x = (int16_t)(accel_data[0] << 2) / 4; *y = (int16_t)(accel_data[1] << 2) / 4; *z = (int16_t)(accel_data[2] << 2) / 4; +} + +void imu_read_linear_accel_data(int16_t *x, int16_t *y, int16_t *z){ + 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; +} + +void imu_read_gravity_accel_data(int16_t *x, int16_t *y, int16_t *z){ + 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; +} + +void imu_read_quaternion_data(int16_t *w, int16_t *x, int16_t *y, int16_t *z){ + static int16_t quaternion_data[4]; + i2c_read(BNO055_QUATERNION_DATA_W_LSB_ADDR, (uint8_t*)quaternion_data, sizeof(quaternion_data)); + *w = (int16_t)(quaternion_data[0] << 2) / 4; + *x = (int16_t)(quaternion_data[1] << 2) / 4; + *y = (int16_t)(quaternion_data[2] << 2) / 4; + *z = (int16_t)(quaternion_data[3] << 2) / 4; +} + +void imu_read_gyro_data(int16_t *x, int16_t *y, int16_t *z){ + static int16_t gyro_data[3]; + i2c_read(BNO055_GYRO_DATA_X_LSB_ADDR, (uint8_t*)gyro_data, sizeof(gyro_data)); + *x = (int16_t)(gyro_data[0] << 2) / 4; + *y = (int16_t)(gyro_data[1] << 2) / 4; + *z = (int16_t)(gyro_data[2] << 2) / 4; +} + +void imu_read_temp(int8_t *temp){ + static int8_t temp_data; + i2c_read(BNO055_TEMP_ADDR, (uint8_t*)&temp_data, 1); + *temp = (int8_t)temp_data; } \ No newline at end of file diff --git a/software/firmware/tests/peripherals/test_imu.c b/software/firmware/tests/peripherals/test_imu.c index 18329eb4..a0f1889a 100644 --- a/software/firmware/tests/peripherals/test_imu.c +++ b/software/firmware/tests/peripherals/test_imu.c @@ -15,13 +15,24 @@ int main(void) system_enable_interrupts(true); // Loop forever, waiting for IMU interrupts - int16_t x, y, z; + int16_t w, x, y, z; + int16_t temp; imu_register_motion_change_callback(motion_interrupt, OPERATION_MODE_NDOF); while (true) { - am_hal_delay_us(5000000); + am_hal_delay_us(1000000); imu_read_accel_data(&x, &y, &z); print("X = %d, Y = %d, Z = %d\n", (int32_t)x, (int32_t)y, (int32_t)z); + imu_read_linear_accel_data(&x, &y, &z); + print("X = %d, Y = %d, Z = %d\n", (int32_t)x, (int32_t)y, (int32_t)z); + imu_read_gravity_accel_data(&x, &y, &z); + print("X = %d, Y = %d, Z = %d\n", (int32_t)x, (int32_t)y, (int32_t)z); + imu_read_quaternion_data(&w, &x, &y, &z); + print("W = %d, X = %d, Y = %d, Z = %d\n", (int32_t)w, (int32_t)x, (int32_t)y, (int32_t)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); + print("temp:%d\n", (int32_t)temp); } // Should never reach this point