Skip to content

Commit

Permalink
Merge remote-tracking branch 'refs/remotes/origin/master'
Browse files Browse the repository at this point in the history
  • Loading branch information
@hedgecrw
hedgecrw committed Nov 15, 2023
2 parents d5b9429 + 0f460e6 commit c7a57bf
Show file tree
Hide file tree
Showing 3 changed files with 51 additions and 37 deletions.
21 changes: 17 additions & 4 deletions software/firmware/src/peripherals/include/imu.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -223,6 +223,19 @@ typedef struct {
uint8_t y_remap_sign;
uint8_t z_remap_sign;
}bno055_axis_remap_t;

typedef struct {
int16_t w;
int16_t x;
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);
Expand All @@ -233,10 +246,10 @@ 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_quaternion_data(int16_t *w, 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);
void imu_read_fw_version(uint8_t *msb, uint8_t *lsb);
Expand Down
42 changes: 21 additions & 21 deletions software/firmware/src/peripherals/src/imu.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -234,38 +234,38 @@ 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(int16_t *w, int16_t *x, int16_t *y, int16_t *z){
void imu_read_quaternion_data(bno055_quaternion_t *quaternion){
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;
read_int16_vector(BNO055_QUATERNION_DATA_W_LSB_ADDR, quaternion_data, sizeof(quaternion_data));
quaternion->w = quaternion_data[0];
quaternion->x = quaternion_data[1];
quaternion->y = quaternion_data[2];
quaternion->z = quaternion_data[3];
}

void imu_read_gyro_data(int16_t *x, int16_t *y, int16_t *z){
Expand Down
25 changes: 13 additions & 12 deletions software/firmware/tests/peripherals/test_imu.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,12 +15,14 @@ int main(void)
system_enable_interrupts(true);

// Loop forever, waiting for IMU interrupts
int16_t w, x, y, z;
int16_t x, y, z;
int8_t temp;
uint8_t rev_msb, rev_lsb;
bno55_calib_status_t status;
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);

Expand All @@ -32,23 +34,22 @@ int main(void)

while (true)
{
am_hal_delay_us(1000000);
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);
print("Linear Accel 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("Gravity Accel 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("Quaternion W = %d, X = %d, Y = %d, Z = %d\n", (int32_t)w, (int32_t)x, (int32_t)y, (int32_t)z);
am_hal_delay_us(20000);
//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);
//print("temp:%d\n", (int32_t)temp);

//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_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);

Expand Down

0 comments on commit c7a57bf

Please sign in to comment.