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?

[Vidar Berg]

Are you able to capture a logic trace to see if the data is indeed being NACked?

[Joseph]

I'm new to this so I'm not sure how to capture a logic trace. However, I searched online regarding the error that I was having and I found this:  nrf_drv_twi_tx is returning error code 33281 . 

They refer it as a NACK problem. 

[Vidar Berg]

You're right, the transfer function returns with NRF_ERROR_DRV_TWI_ERR_ANACK (0x8201), which means the sensor did not acknowledge the first byte containing the I2C address. How did you verify that the i2c address was correct? Datasheet shows that it can be either 0x28, 0x29, or 0x40 depending on the configuration.