Skip to content

Commit

Permalink
enable the use of data ready interrupt on BNO055
Browse files Browse the repository at this point in the history
  • Loading branch information
@corruptbear
corruptbear committed Jun 7, 2024
1 parent f2b6128 commit acd4fd4
Show file tree
Hide file tree
Showing 3 changed files with 68 additions and 48 deletions.
2 changes: 2 additions & 0 deletions software/firmware/src/peripherals/include/imu.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,6 +12,7 @@
#define BNO055_ID 0xA0

typedef void (*motion_change_callback_t)(bool in_motion);
typedef void (*data_ready_callback_t)(uint8_t interrupt_status);

typedef enum
{
Expand Down Expand Up @@ -272,6 +273,7 @@ typedef struct
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_register_data_ready_callback(data_ready_callback_t callback);
void imu_set_power_mode(bno055_powermode_t power_mode);
void imu_read_accel_data(bno055_acc_t *acc);
void imu_read_linear_accel_data(bno055_acc_t *acc);
Expand Down
37 changes: 31 additions & 6 deletions software/firmware/src/peripherals/src/imu.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,13 +3,15 @@
#include "imu.h"
#include "math.h"
#include "system.h"
#include "logging.h"


// Static Global Variables ---------------------------------------------------------------------------------------------

static void *i2c_handle;
static volatile bool previously_in_motion;
static motion_change_callback_t motion_change_callback;
static data_ready_callback_t data_ready_callback;


// Private Helper Functions --------------------------------------------------------------------------------------------
Expand Down Expand Up @@ -82,11 +84,18 @@ static void i2c_read(uint8_t reg_number, uint8_t *read_buffer, uint32_t buffer_l
static void imu_isr(void *args)
{
// Read the device motion status and trigger the registered callback
const bool in_motion = i2c_read8(BNO055_INTR_STAT_ADDR) & 0x40;
i2c_write8(BNO055_SYS_TRIGGER_ADDR, 0xC0);
const uint8_t interrupt_status = i2c_read8(BNO055_INTR_STAT_ADDR);
const bool in_motion = interrupt_status & 0x40;

if (in_motion != previously_in_motion)
motion_change_callback(in_motion);
previously_in_motion = in_motion;
if (interrupt_status && (0x01 | 0x02 | 0x10))
{
if (data_ready_callback!=NULL)
data_ready_callback(interrupt_status);
}
i2c_write8(BNO055_SYS_TRIGGER_ADDR, 0xC0);//reset all interrupt status bits, and INT output
}

static void read_int16_vector(uint8_t reg_number, int16_t *read_buffer, uint32_t byte_count){
Expand Down Expand Up @@ -141,16 +150,26 @@ static void enable_motion_interrupts(void)
{
// Set up interrupts for motion and non-motion events
i2c_write8(BNO055_PAGE_ID_ADDR, 1);
i2c_write8(ACC_CONFIG, 0b00000000);
i2c_write8(ACC_CONFIG, 0b00000000); //operation mode/bandwidth/g range
i2c_write8(ACC_AM_THRE, 0b00001010);
i2c_write8(ACC_NM_THRE, 0b00001010);
i2c_write8(ACC_NM_SET, 0b00001001);
i2c_write8(ACC_INT_SET, 0b00011111);
i2c_write8(INT_MSK, 0xC0);
i2c_write8(INT_EN, 0xC0);
i2c_write8(ACC_INT_SET, 0b00011111); //axis and duration for triggering motion interrupt
i2c_write8(INT_MSK, 0b11000000); //11010011
i2c_write8(INT_EN, 0b11000000);
i2c_write8(BNO055_PAGE_ID_ADDR, 0);
}

static void enable_data_ready_interrupts(void)
{
i2c_write8(BNO055_PAGE_ID_ADDR, 1);
uint8_t int_msk = i2c_read8(INT_MSK)|0b00010011;
uint8_t int_en = i2c_read8(INT_EN)|0b00010011;
i2c_write8(INT_MSK, int_msk);
i2c_write8(INT_EN, int_en);
i2c_write8(BNO055_PAGE_ID_ADDR, 0);
}

// Math helper functions -----------------------------------------------------------------------------------------------
void quaternion_to_euler(bno055_quaternion_t quaternion, bno055_euler_t *euler)
{
Expand All @@ -171,6 +190,7 @@ void imu_init(void)
// Initialize static variables
previously_in_motion = false;
motion_change_callback = NULL;
data_ready_callback = NULL;

// Create an I2C configuration structure
const am_hal_iom_config_t i2c_config =
Expand Down Expand Up @@ -249,6 +269,11 @@ void imu_register_motion_change_callback(motion_change_callback_t callback, bno0
set_mode(mode);
}

void imu_register_data_ready_callback(data_ready_callback_t callback){
data_ready_callback = callback;
enable_data_ready_interrupts();
}

void imu_set_power_mode(bno055_powermode_t power_mode)
{
bno055_opmode_t saved_mode = get_mode();
Expand Down
77 changes: 35 additions & 42 deletions software/firmware/tests/peripherals/test_imu.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,67 +7,60 @@ static void motion_interrupt(bool in_motion)
print("Device is %s\n", in_motion ? "IN MOTION" : "STATIONARY");
}

static void read_data(uint8_t interrupt_status)
{
const bool acc_fusion_data_ready = interrupt_status & 0x01;//ACC_BSX_DRDY
//const bool mag_data_ready = interrupt_status & 0x02;//MAG_DRDY
//const bool gyro_data_ready = interrupt_status & 0x10;//GYR_DRDY
if (acc_fusion_data_ready)
{
int8_t temp;
bno55_calib_status_t status = {0};
bno055_calib_offsets_t offsets = {0};
bno055_axis_remap_t remap = {0};
bno055_quaternion_t quaternion = {0};
bno055_euler_t euler = {0};
bno055_euler_t device_calculated_euler = {0};
bno055_acc_t acc = {0};
bno055_gyro_t gyro = {0};
imu_read_calibration_status(&status);
//imu_read_calibration_offsets(&offsets);
imu_read_linear_accel_data(&acc);
imu_read_quaternion_data(&quaternion);
quaternion_to_euler(quaternion, &euler);
imu_read_gyro_data(&gyro);
//imu_read_temp(&temp);
//print("temp:%d\n", (int32_t)temp);
print("Calibration status: sys %u, gyro %u, accel %u, mag %u\n",status.sys, status.gyro, status.accel, status.mag);
//print("Calibration offsets: %d, %d, %d \n", offsets.gyro_offset_x, offsets.gyro_offset_y, offsets.gyro_offset_z);
print("Linear Accel X = %d, Y = %d, Z = %d, qw = %d, qx = %d, qy = %d, qz = %d, gx = %d, gy = %d, gz = %d\n", acc.x, acc.y, acc.z, quaternion.w, quaternion.x, quaternion.y, quaternion.z, gyro.x, gyro.y,gyro.z);
}
}

int main(void)
{
// Set up system hardware
setup_hardware();
imu_init();
system_enable_interrupts(true);

// Loop forever, waiting for IMU interrupts
int16_t x, y, z;
int8_t temp;
uint8_t rev_msb, rev_lsb;
bno55_calib_status_t status = {0};
bno055_calib_offsets_t offsets = {0};
//bno055_axis_remap_t remap = {.x_remap_val = 1, .y_remap_val = 0, .z_remap_val = 2};
bno055_axis_remap_t remap = {0};
bno055_quaternion_t quaternion = {0};
bno055_euler_t euler = {0};
bno055_euler_t device_calculated_euler = {0};
bno055_acc_t acc = {0};
bno055_gyro_t gyro = {0};

imu_register_motion_change_callback(motion_interrupt, OPERATION_MODE_NDOF);
//imu_register_data_ready_callback(read_data);
imu_set_power_mode(POWER_MODE_NORMAL);

imu_read_fw_version(&rev_msb, &rev_lsb);
print("BNO055 firmware version:%u.%u\n",rev_msb, rev_lsb);
print("BNO055 firmware version:%x.%x\n",rev_msb, rev_lsb);

//imu_read_axis_remap(&remap);
if (imu_set_axis_remap(remap)){print("remap success!\n");}
//if (imu_set_axis_remap(remap)){print("remap success!\n");}
//print("BNO055 X mapping:%u, Y mapping:%u, Z mapping:%u, X sign:%u, Y sign:%u, Z sign:%u\n", remap.x_remap_val, remap.y_remap_val, remap.z_remap_val, remap.x_remap_sign, remap.y_remap_sign, remap.z_remap_sign);

while (true)
{
am_hal_delay_us(20000);
//0: not calibrated; 3: fully calibrated
imu_read_calibration_status(&status);
print("Calibration status: sys %u, gyro %u, accel %u, mag %u\n",status.sys, status.gyro, status.accel, status.mag);
//imu_read_calibration_offsets(&offsets);
//print("Calibration offsets: %d, %d, %d \n", offsets.gyro_offset_x, offsets.gyro_offset_y, offsets.gyro_offset_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);
memset(&acc, 0, sizeof(acc));
memset(&quaternion, 0, sizeof(quaternion));
memset(&euler, 0, sizeof(euler));
//memset(&device_calculated_euler, 0 , sizeof(device_calculated_euler));

imu_read_linear_accel_data(&acc);
imu_read_quaternion_data(&quaternion);
quaternion_to_euler(quaternion, &euler);
imu_read_gyro_data(&gyro);
//imu_read_euler_data(&device_calculated_euler);
print("Linear Accel X = %d, Y = %d, Z = %d, qw = %d, qx = %d, qy = %d, qz = %d, gx = %d, gy = %d, gz = %d\n", acc.x, acc.y, acc.z, quaternion.w, quaternion.x, quaternion.y, quaternion.z, gyro.x, gyro.y,gyro.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);
//print("temp:%d\n", (int32_t)temp);



while (true){
;
}

// Should never reach this point
Expand Down

0 comments on commit acd4fd4

Please sign in to comment.