TWI for bno055 implementation on nrf52

[Joseph]

Hi,

I'm stuck in the loop of calibration. This is the error that I get from Putty:

It says that I get a TX error.

This is the code that I have for bno055.h:

#ifndef __BNO055_H__
#define __BNO055_H__

#include <stdbool.h>
#include <stdint.h>

/** BNO055 Address A **/
#define BNO055_ADDRESS_A (0x28)
/** BNO055 ID **/
#define BNO055_ID (0xA0)

/** Offsets registers **/
#define NUM_BNO055_OFFSET_REGISTERS (22)

/** A structure to represent offsets **/
typedef struct {
  int16_t accel_offset_x;
  int16_t accel_offset_y;
  int16_t accel_offset_z;
  int16_t mag_offset_x;
  int16_t mag_offset_y;
  int16_t mag_offset_z;
  int16_t gyro_offset_x;
  int16_t gyro_offset_y;
  int16_t gyro_offset_z;
  int16_t accel_radius;
  int16_t mag_radius;
} bno055_offsets_t;

/** Operation mode settings **/
typedef enum {
  OPERATION_MODE_CONFIG = 0X00,
  OPERATION_MODE_ACCONLY = 0X01,
  OPERATION_MODE_MAGONLY = 0X02,
  OPERATION_MODE_GYRONLY = 0X03,
  OPERATION_MODE_ACCMAG = 0X04,
  OPERATION_MODE_ACCGYRO = 0X05,
  OPERATION_MODE_MAGGYRO = 0X06,
  OPERATION_MODE_AMG = 0X07,
  OPERATION_MODE_IMUPLUS = 0X08,
  OPERATION_MODE_COMPASS = 0X09,
  OPERATION_MODE_M4G = 0X0A,
  OPERATION_MODE_NDOF_FMC_OFF = 0X0B,
  OPERATION_MODE_NDOF = 0X0C
} bno055_opmode_t;

/** BNO055 power settings **/
typedef enum {
  POWER_MODE_NORMAL = 0X00,
  POWER_MODE_LOWPOWER = 0X01,
  POWER_MODE_SUSPEND = 0X02
} bno055_powermode_t;

/** A structure to represent revisions **/
typedef struct {
  uint8_t accel_rev;
  uint8_t mag_rev;
  uint8_t gyro_rev;
  uint16_t sw_rev;
  uint8_t bl_rev;
} bno055_rev_info_t;

/** Vector Mappings **/
typedef enum {
  VECTOR_ACCELEROMETER = 0x08,
  VECTOR_MAGNETOMETER = 0x0E,
  VECTOR_GYROSCOPE = 0x14,
  VECTOR_EULER = 0x1A,
  VECTOR_LINEARACCEL = 0x28,
  VECTOR_GRAVITY = 0x2E
} bno055_vector_type_t;

/** Register addresses **/
typedef enum {
    BNO055_PAGE_ID_ADDR = 0X07,

    BNO055_CHIP_ID_ADDR = 0x00,
    BNO055_ACCEL_REV_ID_ADDR = 0x01,
    BNO055_MAG_REV_ID_ADDR = 0x02,
    BNO055_GYRO_REV_ID_ADDR = 0x03,
    BNO055_SW_REV_ID_LSB_ADDR = 0x04,
    BNO055_SW_REV_ID_MSB_ADDR = 0x05,
    BNO055_BL_REV_ID_ADDR = 0X06,

    /* Accel data register */
    BNO055_ACCEL_DATA_X_LSB_ADDR = 0X08,
    BNO055_ACCEL_DATA_X_MSB_ADDR = 0X09,
    BNO055_ACCEL_DATA_Y_LSB_ADDR = 0X0A,
    BNO055_ACCEL_DATA_Y_MSB_ADDR = 0X0B,
    BNO055_ACCEL_DATA_Z_LSB_ADDR = 0X0C,
    BNO055_ACCEL_DATA_Z_MSB_ADDR = 0X0D,

    /* Mag data register */
    BNO055_MAG_DATA_X_LSB_ADDR = 0X0E,
    BNO055_MAG_DATA_X_MSB_ADDR = 0X0F,
    BNO055_MAG_DATA_Y_LSB_ADDR = 0X10,
    BNO055_MAG_DATA_Y_MSB_ADDR = 0X11,
    BNO055_MAG_DATA_Z_LSB_ADDR = 0X12,
    BNO055_MAG_DATA_Z_MSB_ADDR = 0X13,

    /* Gyro data registers */
    BNO055_GYRO_DATA_X_LSB_ADDR = 0X14,
    BNO055_GYRO_DATA_X_MSB_ADDR = 0X15,
    BNO055_GYRO_DATA_Y_LSB_ADDR = 0X16,
    BNO055_GYRO_DATA_Y_MSB_ADDR = 0X17,
    BNO055_GYRO_DATA_Z_LSB_ADDR = 0X18,
    BNO055_GYRO_DATA_Z_MSB_ADDR = 0X19,

    /* Euler data registers */
    BNO055_EULER_H_LSB_ADDR = 0X1A,
    BNO055_EULER_H_MSB_ADDR = 0X1B,
    BNO055_EULER_R_LSB_ADDR = 0X1C,
    BNO055_EULER_R_MSB_ADDR = 0X1D,
    BNO055_EULER_P_LSB_ADDR = 0X1E,
    BNO055_EULER_P_MSB_ADDR = 0X1F,

    /* Quaternion data registers */
    BNO055_QUATERNION_DATA_W_LSB_ADDR = 0X20,
    BNO055_QUATERNION_DATA_W_MSB_ADDR = 0X21,
    BNO055_QUATERNION_DATA_X_LSB_ADDR = 0X22,
    BNO055_QUATERNION_DATA_X_MSB_ADDR = 0X23,
    BNO055_QUATERNION_DATA_Y_LSB_ADDR = 0X24,
    BNO055_QUATERNION_DATA_Y_MSB_ADDR = 0X25,
    BNO055_QUATERNION_DATA_Z_LSB_ADDR = 0X26,
    BNO055_QUATERNION_DATA_Z_MSB_ADDR = 0X27,

    /* Linear acceleration data registers */
    BNO055_LINEAR_ACCEL_DATA_X_LSB_ADDR = 0X28,
    BNO055_LINEAR_ACCEL_DATA_X_MSB_ADDR = 0X29,
    BNO055_LINEAR_ACCEL_DATA_Y_LSB_ADDR = 0X2A,
    BNO055_LINEAR_ACCEL_DATA_Y_MSB_ADDR = 0X2B,
    BNO055_LINEAR_ACCEL_DATA_Z_LSB_ADDR = 0X2C,
    BNO055_LINEAR_ACCEL_DATA_Z_MSB_ADDR = 0X2D,

    /* Gravity data registers */
    BNO055_GRAVITY_DATA_X_LSB_ADDR = 0X2E,
    BNO055_GRAVITY_DATA_X_MSB_ADDR = 0X2F,
    BNO055_GRAVITY_DATA_Y_LSB_ADDR = 0X30,
    BNO055_GRAVITY_DATA_Y_MSB_ADDR = 0X31,
    BNO055_GRAVITY_DATA_Z_LSB_ADDR = 0X32,
    BNO055_GRAVITY_DATA_Z_MSB_ADDR = 0X33,

    /* Temperature data register */
    BNO055_TEMP_ADDR = 0X34,

    /* Status registers */
    BNO055_CALIB_STAT_ADDR = 0X35,
    BNO055_SELFTEST_RESULT_ADDR = 0X36,
    BNO055_INTR_STAT_ADDR = 0X37,

    BNO055_SYS_CLK_STAT_ADDR = 0X38,
    BNO055_SYS_STAT_ADDR = 0X39,
    BNO055_SYS_ERR_ADDR = 0X3A,

    /* Unit selection register */
    BNO055_UNIT_SEL_ADDR = 0X3B,

    /* Mode registers */
    BNO055_OPR_MODE_ADDR = 0X3D,
    BNO055_PWR_MODE_ADDR = 0X3E,

    BNO055_SYS_TRIGGER_ADDR = 0X3F,
    BNO055_TEMP_SOURCE_ADDR = 0X40,

    /* Axis remap registers */
    BNO055_AXIS_MAP_CONFIG_ADDR = 0X41,
    BNO055_AXIS_MAP_SIGN_ADDR = 0X42,

    /* SIC registers */
    BNO055_SIC_MATRIX_0_LSB_ADDR = 0X43,
    BNO055_SIC_MATRIX_0_MSB_ADDR = 0X44,
    BNO055_SIC_MATRIX_1_LSB_ADDR = 0X45,
    BNO055_SIC_MATRIX_1_MSB_ADDR = 0X46,
    BNO055_SIC_MATRIX_2_LSB_ADDR = 0X47,
    BNO055_SIC_MATRIX_2_MSB_ADDR = 0X48,
    BNO055_SIC_MATRIX_3_LSB_ADDR = 0X49,
    BNO055_SIC_MATRIX_3_MSB_ADDR = 0X4A,
    BNO055_SIC_MATRIX_4_LSB_ADDR = 0X4B,
    BNO055_SIC_MATRIX_4_MSB_ADDR = 0X4C,
    BNO055_SIC_MATRIX_5_LSB_ADDR = 0X4D,
    BNO055_SIC_MATRIX_5_MSB_ADDR = 0X4E,
    BNO055_SIC_MATRIX_6_LSB_ADDR = 0X4F,
    BNO055_SIC_MATRIX_6_MSB_ADDR = 0X50,
    BNO055_SIC_MATRIX_7_LSB_ADDR = 0X51,
    BNO055_SIC_MATRIX_7_MSB_ADDR = 0X52,
    BNO055_SIC_MATRIX_8_LSB_ADDR = 0X53,
    BNO055_SIC_MATRIX_8_MSB_ADDR = 0X54,

    /* Accelerometer Offset registers */
    ACCEL_OFFSET_X_LSB_ADDR = 0X55,
    ACCEL_OFFSET_X_MSB_ADDR = 0X56,
    ACCEL_OFFSET_Y_LSB_ADDR = 0X57,
    ACCEL_OFFSET_Y_MSB_ADDR = 0X58,
    ACCEL_OFFSET_Z_LSB_ADDR = 0X59,
    ACCEL_OFFSET_Z_MSB_ADDR = 0X5A,

    /* Magnetometer Offset registers */
    MAG_OFFSET_X_LSB_ADDR = 0X5B,
    MAG_OFFSET_X_MSB_ADDR = 0X5C,
    MAG_OFFSET_Y_LSB_ADDR = 0X5D,
    MAG_OFFSET_Y_MSB_ADDR = 0X5E,
    MAG_OFFSET_Z_LSB_ADDR = 0X5F,
    MAG_OFFSET_Z_MSB_ADDR = 0X60,

    /* Gyroscope Offset registers */
    GYRO_OFFSET_X_LSB_ADDR = 0X61,
    GYRO_OFFSET_X_MSB_ADDR = 0X62,
    GYRO_OFFSET_Y_LSB_ADDR = 0X63,
    GYRO_OFFSET_Y_MSB_ADDR = 0X64,
    GYRO_OFFSET_Z_LSB_ADDR = 0X65,
    GYRO_OFFSET_Z_MSB_ADDR = 0X66,

    /* Radius registers */
    ACCEL_RADIUS_LSB_ADDR = 0X67,
    ACCEL_RADIUS_MSB_ADDR = 0X68,
    MAG_RADIUS_LSB_ADDR = 0X69,
    MAG_RADIUS_MSB_ADDR = 0X6A
} adafruit_bno055_reg_t;

bool bno055_init(void);
void bno055_set_mode(bno055_opmode_t mode);
void bno055_set_power_mode(bno055_powermode_t mode);
void bno055_get_calibration(uint8_t *system, uint8_t *gyro, uint8_t *accel, uint8_t *mag);
void bno055_get_rev_info(bno055_rev_info_t *info);
int8_t bno055_get_temp(void);
bool bno055_read_vector(bno055_vector_type_t vector_type, int16_t *x, int16_t *y, int16_t *z);
void bno055_set_axis_remap(uint8_t remapcode);
void bno055_set_axis_sign(uint8_t remapsign);
void bno055_enter_suspend_mode(void);
void bno055_enter_normal_mode(void);

#endif // __BNO055_H__

This is the code that I have for bno055.c:

#include "bno055.h"
#include "nrf_drv_twi.h"
#include <stdbool.h>
#include <stdint.h>
#include "nrf_delay.h"
#include "nrf_log.h"

static const nrf_drv_twi_t m_twi = NRF_DRV_TWI_INSTANCE(0);

static ret_code_t read8(uint8_t address, uint8_t reg, uint8_t *value) {
    ret_code_t err_code;

    err_code = nrf_drv_twi_tx(&m_twi, address, &reg, 1, true);
    if (err_code != NRF_SUCCESS) {
        NRF_LOG_ERROR("I2C TX Error: %d", err_code);
        return err_code;
    }

    err_code = nrf_drv_twi_rx(&m_twi, address, value, 1);
    if (err_code != NRF_SUCCESS) {
        NRF_LOG_ERROR("I2C RX Error: %d", err_code);
    }

    return err_code;
}

static ret_code_t write8(uint8_t address, uint8_t reg, uint8_t value) {
    ret_code_t err_code;
    uint8_t buffer[2] = {reg, value};

    err_code = nrf_drv_twi_tx(&m_twi, address, buffer, 2, false);
    if (err_code != NRF_SUCCESS) {
        NRF_LOG_ERROR("I2C Write Error: %d", err_code);
    }

    return err_code;
}

bool bno055_init(void) {
    uint8_t id;
    ret_code_t err_code;

    err_code = read8(BNO055_ADDRESS_A, BNO055_CHIP_ID_ADDR, &id);
    if (err_code != NRF_SUCCESS || id != BNO055_ID) {
        NRF_LOG_INFO("BNO055 ID mismatch: expected 0xA0, got 0x%02x", id);
        return false;
    }

    bno055_set_mode(OPERATION_MODE_CONFIG);
    nrf_delay_ms(25);
    write8(BNO055_ADDRESS_A, BNO055_SYS_TRIGGER_ADDR, 0x20); // Reset all interrupts
    nrf_delay_ms(10);
    bno055_set_mode(OPERATION_MODE_NDOF);
    nrf_delay_ms(20);
    return true;
}

void bno055_set_mode(bno055_opmode_t mode) {
    write8(BNO055_ADDRESS_A, BNO055_OPR_MODE_ADDR, mode);
    nrf_delay_ms(30);
}

void bno055_set_power_mode(bno055_powermode_t mode) {
    write8(BNO055_ADDRESS_A, BNO055_PWR_MODE_ADDR, mode);
    nrf_delay_ms(30);
}

void bno055_get_calibration(uint8_t *system, uint8_t *gyro, uint8_t *accel, uint8_t *mag) {
    uint8_t calData;
    read8(BNO055_ADDRESS_A, BNO055_CALIB_STAT_ADDR, &calData);
    *system = (calData >> 6) & 0x03;
    *gyro = (calData >> 4) & 0x03;
    *accel = (calData >> 2) & 0x03;
    *mag = calData & 0x03;
}

void bno055_get_rev_info(bno055_rev_info_t *info) {
    read8(BNO055_ADDRESS_A, BNO055_ACCEL_REV_ID_ADDR, &info->accel_rev);
    read8(BNO055_ADDRESS_A, BNO055_MAG_REV_ID_ADDR, &info->mag_rev);
    read8(BNO055_ADDRESS_A, BNO055_GYRO_REV_ID_ADDR, &info->gyro_rev);
    uint8_t sw_rev_lsb, sw_rev_msb;
    read8(BNO055_ADDRESS_A, BNO055_SW_REV_ID_LSB_ADDR, &sw_rev_lsb);
    read8(BNO055_ADDRESS_A, BNO055_SW_REV_ID_MSB_ADDR, &sw_rev_msb);
    info->sw_rev = (uint16_t)((sw_rev_msb << 8) | sw_rev_lsb);
    read8(BNO055_ADDRESS_A, BNO055_BL_REV_ID_ADDR, &info->bl_rev);
}

int8_t bno055_get_temp(void) {
    int8_t temp;
    read8(BNO055_ADDRESS_A, BNO055_TEMP_ADDR, (uint8_t*)&temp);
    return temp;
}

bool bno055_read_vector(bno055_vector_type_t vector_type, int16_t *x, int16_t *y, int16_t *z) {
    uint8_t buffer[6];
    uint8_t reg = (uint8_t)vector_type;
    ret_code_t err_code;

    err_code = nrf_drv_twi_tx(&m_twi, BNO055_ADDRESS_A, &reg, 1, true);
    if (err_code != NRF_SUCCESS) {
        NRF_LOG_ERROR("I2C TX Error: %d", err_code);
        return false;
    }

    err_code = nrf_drv_twi_rx(&m_twi, BNO055_ADDRESS_A, buffer, 6);
    if (err_code != NRF_SUCCESS) {
        NRF_LOG_ERROR("I2C RX Error: %d", err_code);
        return false;
    }

    *x = (int16_t)((buffer[0]) | (buffer[1] << 8));
    *y = (int16_t)((buffer[2]) | (buffer[3] << 8));
    *z = (int16_t)((buffer[4]) | (buffer[5] << 8));
    return true;
}

void bno055_set_axis_remap(uint8_t remapcode) {
    write8(BNO055_ADDRESS_A, BNO055_AXIS_MAP_CONFIG_ADDR, remapcode);
    nrf_delay_ms(10);
}

void bno055_set_axis_sign(uint8_t remapsign) {
    write8(BNO055_ADDRESS_A, BNO055_AXIS_MAP_SIGN_ADDR, remapsign);
    nrf_delay_ms(10);
}

void bno055_enter_suspend_mode(void) {
    bno055_set_power_mode(POWER_MODE_SUSPEND);
}

void bno055_enter_normal_mode(void) {
    bno055_set_power_mode(POWER_MODE_NORMAL);
}

And this is my main.c code:

#include <stdio.h>
#include "boards.h"
#include "app_util_platform.h"
#include "app_error.h"
#include "nrf_drv_twi.h"
#include "bno055.h"
#include "nrf_delay.h"

#include "nrf_log.h"
#include "nrf_log_ctrl.h"
#include "nrf_log_default_backends.h"
#define NRF_LOG_FLOAT_ENABLED 1

#include <math.h>

#define G_CONSTANT 9.81f

#define TWI_INSTANCE_ID 0

#define SDA_PIN 24
#define SCL_PIN 25

static const nrf_drv_twi_t m_twi = NRF_DRV_TWI_INSTANCE(TWI_INSTANCE_ID);

void twi_master_init(void)
{
    ret_code_t err_code;

    const nrf_drv_twi_config_t twi_config = {
       .scl                = SCL_PIN,
       .sda                = SDA_PIN,
       .frequency          = NRF_DRV_TWI_FREQ_400K,
       .interrupt_priority = APP_IRQ_PRIORITY_HIGH
    };

    err_code = nrf_drv_twi_init(&m_twi, &twi_config, NULL, NULL);
    APP_ERROR_CHECK(err_code);

    nrf_drv_twi_enable(&m_twi);
}

void perform_calibration(void);

int main(void)
{
    APP_ERROR_CHECK(NRF_LOG_INIT(NULL));
    NRF_LOG_DEFAULT_BACKENDS_INIT();

    static int16_t AccValue[3], GyroValue[3];
    float acc_x;
    float acc_y;
    float acc_z;
    float gyr_x;
    float gyr_y;
    float gyr_z;

    bsp_board_init(BSP_INIT_LEDS | BSP_INIT_BUTTONS);

    twi_master_init();
    nrf_delay_ms(1000);

    if (!bno055_init())
    {
        NRF_LOG_INFO("BNO055 initialization failed!!!");
        while (1) { nrf_delay_ms(1000); }
    }

    NRF_LOG_INFO("BNO055 Init Successfully!!!");

    // Perform calibration
    perform_calibration();

    NRF_LOG_INFO("Calibration completed.");

    NRF_LOG_INFO("Reading Values from ACC & GYRO");
    nrf_delay_ms(2000);
    char log_buffer[128];

    while (true)
    {
        if (bno055_read_vector(VECTOR_ACCELEROMETER, &AccValue[0], &AccValue[1], &AccValue[2]))
        {
            acc_x = ((float)AccValue[0] / 100.0f) * G_CONSTANT;  // Convert from mg to m/s^2
            acc_y = ((float)AccValue[1] / 100.0f) * G_CONSTANT;
            acc_z = ((float)AccValue[2] / 100.0f) * G_CONSTANT;
            snprintf(log_buffer, sizeof(log_buffer), "ACC Values: x = %0.1f, y = %0.1f, z = %0.1f (m/s^2)", acc_x, acc_y, acc_z); // m/s^2
            NRF_LOG_INFO("%s", NRF_LOG_PUSH(log_buffer));
        }
        else
        {
            NRF_LOG_INFO("Reading ACC values Failed!!!");
        }

        if (bno055_read_vector(VECTOR_GYROSCOPE, &GyroValue[0], &GyroValue[1], &GyroValue[2]))
        {
            gyr_x = ((float)GyroValue[0] / 16.0f) * (M_PI / 180);  // Convert from deg/s to rad/s
            gyr_y = ((float)GyroValue[1] / 16.0f) * (M_PI / 180);
            gyr_z = ((float)GyroValue[2] / 16.0f) * (M_PI / 180);
            snprintf(log_buffer, sizeof(log_buffer), "GYRO Values: x = %0.1f, y = %0.1f, z = %0.1f (rad/s)", gyr_x, gyr_y, gyr_z); // rad/s
            NRF_LOG_INFO("%s", NRF_LOG_PUSH(log_buffer));
        }
        else
        {
            NRF_LOG_INFO("Reading GYRO values Failed!!!");
        }

        nrf_delay_ms(100);
    }
}

void perform_calibration(void)
{
    uint8_t system, gyro, accel, mag;
    NRF_LOG_INFO("Starting calibration. Please follow the instructions.");

    bno055_set_mode(OPERATION_MODE_CONFIG);
    nrf_delay_ms(25);
    bno055_set_mode(OPERATION_MODE_NDOF);
    nrf_delay_ms(20);

    while (1)
    {
        bno055_get_calibration(&system, &gyro, &accel, &mag);
        NRF_LOG_INFO("CALIBRATION: Sys=%d, Gyro=%d, Accel=%d, Mag=%d", system, gyro, accel, mag);

        if (system == 3 && gyro == 3 && accel == 3 && mag == 3)
        {
            NRF_LOG_INFO("Calibration completed.");
            break;
        }

        nrf_delay_ms(500);
    }
}

Please help..

[Vidar Berg]

Hi,

I would recommend probing the bus lines with an oscilloscope or logic analyzer if possible. This is really helpful when troubleshooting I2C issues. I see you have some I2C write errors, but I don't recognize the error ranges. I guess it is either an address or data NACK. So you may want to also double check that the i2c address is correct too. 

Best regards,

Vidar

[Joseph]

I verified that I have the correct i2c address. I think it was set as 0x28. How do I fix the data NACK problem?