upm_result_t bno055_write_calibration_data(const bno055_context dev,
uint8_t *data,
size_t len)
{
assert(dev != NULL);
assert(data != NULL);
if (len != BNO055_CALIBRATION_DATA_SIZE)
{
printf("%s: len must equal BNO055_CALIBRATION_DATA_SIZE "
"(expected %d, got %d).\n",
__FUNCTION__, BNO055_CALIBRATION_DATA_SIZE, (int)len);
return UPM_ERROR_INVALID_SIZE;
}
// should be at page 0, but lets make sure
if (bno055_set_page(dev, 0, false))
return UPM_ERROR_OPERATION_FAILED;
// first we need to go back into config mode
BNO055_OPERATION_MODES_T currentMode = dev->currentMode;
if (bno055_set_operation_mode(dev, BNO055_OPERATION_MODE_CONFIGMODE))
return UPM_ERROR_OPERATION_FAILED;
// write the data
if (bno055_write_regs(dev, BNO055_REG_ACC_OFFSET_X_LSB, data,
BNO055_CALIBRATION_DATA_SIZE))
return UPM_ERROR_OPERATION_FAILED;
// now reset our operating mode
if (bno055_set_operation_mode(dev, currentMode))
return UPM_ERROR_OPERATION_FAILED;
return UPM_SUCCESS;
}
upm_result_t bno055_set_clock_external(const bno055_context dev,
bool extClock)
{
assert(dev != NULL);
if (bno055_set_page(dev, 0, false))
return UPM_ERROR_OPERATION_FAILED;
// first we need to be in config mode
BNO055_OPERATION_MODES_T currentMode = dev->currentMode;
if (bno055_set_operation_mode(dev, BNO055_OPERATION_MODE_CONFIGMODE))
return UPM_ERROR_OPERATION_FAILED;
uint8_t reg = 0;
if (bno055_read_reg(dev, BNO055_REG_SYS_TRIGGER, ®))
return UPM_ERROR_OPERATION_FAILED;
if (extClock)
reg |= BNO055_SYS_TRIGGER_CLK_SEL;
else
reg &= ~BNO055_SYS_TRIGGER_CLK_SEL;
if (bno055_write_reg(dev, BNO055_REG_SYS_TRIGGER, reg))
return UPM_ERROR_OPERATION_FAILED;
// now reset our operating mode
if (bno055_set_operation_mode(dev, currentMode))
return UPM_ERROR_OPERATION_FAILED;
return UPM_SUCCESS;
}
BNO055_RETURN_FUNCTION_TYPE bno055_set_power_mode(u8 v_power_mode_u8, struct bno055_t *bno055, I2c* i2c, uint8_t rx_tx_buf[ARRAY_SIZE_SIX])
{
BNO055_RETURN_FUNCTION_TYPE com_rslt = ERROR;
u8 v_data_u8r = BNO055_ZERO_U8X;
u8 v_prev_opmode_u8 = OPERATION_MODE_CONFIG;
s8 v_stat_s8 = ERROR;
/* Check the struct p_bno055 is empty */
/* The write operation effective only if the operation
mode is in config mode, this part of code is checking the
current operation mode and set the config mode */
v_stat_s8 = bno055_get_operation_mode(&v_prev_opmode_u8, bno055, i2c, rx_tx_buf);
if (v_stat_s8 == SUCCESS)
{
if (v_prev_opmode_u8 != OPERATION_MODE_CONFIG)
v_stat_s8 += bno055_set_operation_mode(OPERATION_MODE_CONFIG, bno055, i2c, rx_tx_buf);
if (v_stat_s8 == SUCCESS)
{
/* Write the value of power mode */
com_rslt = BNO055_I2C_bus_read(bno055->dev_addr,BNO055_POWER_MODE__REG, &v_data_u8r, BNO055_ONE_U8X, i2c, rx_tx_buf);
if (com_rslt == SUCCESS) {
v_data_u8r = BNO055_SET_BITSLICE(v_data_u8r,BNO055_POWER_MODE, v_power_mode_u8);
com_rslt += BNO055_I2C_bus_write(bno055->dev_addr,BNO055_POWER_MODE__REG,&v_data_u8r, BNO055_ONE_U8X, i2c, rx_tx_buf);
}
}
else
{
com_rslt = ERROR;
}
}
else
{
com_rslt = ERROR;
}
if (v_prev_opmode_u8 != OPERATION_MODE_CONFIG)
/* set the operation mode
of previous operation mode*/
com_rslt += bno055_set_operation_mode(v_prev_opmode_u8, bno055, i2c, rx_tx_buf);
return com_rslt;
}
/* IMU Class specific functions */
int IMU::start() {
if(!bno055_driver_bound) {
bno055_driver_bind();
bno055_driver_bound = true;
}
if(!bno055_initialized) {
s8 bno055_init_result = bno055_init(&bno055);
if((int) bno055_init_result != 0) {
std::cerr << TAG_ERROR << "Failed to bno055_init => " << bno055_init_result << std::endl;
return IMC_FAIL;
}
bno055_initialized = true;
}
if(!bno055_power_mode_normal) {
int bno055_power_mode_result = bno055_set_power_mode(POWER_MODE_NORMAL);
if(bno055_power_mode_result != 0) {
std::cerr << TAG_ERROR << "Failed to set bno055 power mode to normal" << std::endl;
return IMC_FAIL;
}
bno055_power_mode_normal = true;
}
if(!bno055_operation_mode_ndof) {
int bno055_set_operation_mode_result = bno055_set_operation_mode(OPERATION_MODE_NDOF);
if(bno055_set_operation_mode_result != 0) {
std::cerr << TAG_ERROR << "Failed to set bno055 operation mode to ndof" << std::endl;
return IMC_FAIL;
}
bno055_operation_mode_ndof = true;
}
return IMC_SUCCESS;
}
// init
bno055_context bno055_init(int bus, uint8_t addr)
{
bno055_context dev =
(bno055_context)malloc(sizeof(struct _bno055_context));
if (!dev)
return NULL;
// zero out context
memset((void *)dev, 0, sizeof(struct _bno055_context));
// make sure MRAA is initialized
int mraa_rv;
if ((mraa_rv = mraa_init()) != MRAA_SUCCESS)
{
printf("%s: mraa_init() failed (%d).\n", __FUNCTION__, mraa_rv);
bno055_close(dev);
return NULL;
}
if (!(dev->i2c = mraa_i2c_init(bus)))
{
printf("%s: mraa_i2c_init() failed.\n", __FUNCTION__);
bno055_close(dev);
return NULL;
}
if (mraa_i2c_address(dev->i2c, addr) != MRAA_SUCCESS)
{
printf("%s: mraa_i2c_address() failed.\n", __FUNCTION__);
bno055_close(dev);
return NULL;
}
_clear_data(dev);
// forcibly set page 0, so we are synced with the device
if (bno055_set_page(dev, 0, true))
{
printf("%s: bno055_set_page() failed.\n", __FUNCTION__);
bno055_close(dev);
return NULL;
}
// check the chip id. This has to be done after forcibly setting
// page 0, as that is the only page where the chip id is present.
uint8_t chipID = 0;
if (bno055_get_chip_id(dev, &chipID))
{
printf("%s: Could not read chip id\n", __FUNCTION__);
bno055_close(dev);
return NULL;
}
if (chipID != BNO055_CHIPID)
{
printf("%s: Invalid chip ID. Expected 0x%02x, got 0x%02x\n",
__FUNCTION__, BNO055_CHIPID, chipID);
bno055_close(dev);
return NULL;
}
upm_result_t urv = UPM_SUCCESS;
// set config mode
urv += bno055_set_operation_mode(dev, BNO055_OPERATION_MODE_CONFIGMODE);
// default to internal clock
urv += bno055_set_clock_external(dev, false);
// we specifically avoid doing a reset so that if the device is
// already calibrated, it will remain so.
// we always use C for temperature
urv += bno055_set_temperature_units_celsius(dev);
// default to accelerometer temp
urv += bno055_set_temperature_source(dev, BNO055_TEMP_SOURCE_ACC);
// set accel units to m/s^2
urv += bno055_set_accelerometer_units(dev, false);
// set gyro units to degrees
urv += bno055_set_gyroscope_units(dev, false);
// set Euler units to degrees
urv += bno055_set_euler_units(dev, false);
// by default, we set the operating mode to the NDOF fusion mode
urv += bno055_set_operation_mode(dev, BNO055_OPERATION_MODE_NDOF);
// if any of those failed, bail
if (urv != UPM_SUCCESS)
{
printf("%s: Initial device configuration failed\n", __FUNCTION__);
bno055_close(dev);
return NULL;
}
return dev;
}
/////////////////
// main routine
/////////////////
void main (void) {
int8_t ret1, ret2;
int16_t accX1, accY1, accZ1;
int16_t accX2, accY2, accZ2;
char buf[100];
uint8_t count=0, len=0;
/////////////////
// init peripherals
/////////////////
// disable interrupts
DISABLE_INTERRUPTS;
// switch to 16MHz (default is 2MHz)
CLK.CKDIVR.byte = 0x00;
// set default option bytes to assert bootloader is running
flash_OPT_default();
// init timer TIM3 for sleep and timeout (required by I2C)
tim3_init();
// init timer TIM4 for 1ms clock with interrupts
tim4_init();
// init I2C bus
i2c_init();
// init and reset LCD display
lcd_init();
// init pins for UART1 Rx(=PA4) and Tx(=PA5)
gpio_init(&PORT_A, PIN_4, INPUT_PULLUP_NOEXINT);
gpio_init(&PORT_A, PIN_5, OUTPUT_PUSHPULL_FAST);
// init UART1 to high speed (connected to PC on muBoard)
uart1_init(230400L);
// init LEDs on muBoard for visual feedback
GPIO_SET(PORT_H,PIN_2|PIN_3, 1);
gpio_init(&PORT_H, PIN_2|PIN_3, OUTPUT_PUSHPULL_FAST);
// enable interrupts
ENABLE_INTERRUPTS;
// init I2C routine pointers
I2C_routine();
// initialize sensors
do {
bno055.dev_addr = BNO055_I2C_ADDR1;
ret1 = bno055_init(&bno055);
ret1 |= bno055_set_power_mode(POWER_MODE_NORMAL);
ret1 |= bno055_set_operation_mode(OPERATION_MODE_AMG);
} while (ret1 && USE_I2C_ADDR1);
do {
bno055.dev_addr = BNO055_I2C_ADDR2;
ret2 = bno055_init(&bno055);
ret2 |= bno055_set_power_mode(POWER_MODE_NORMAL);
ret2 |= bno055_set_operation_mode(OPERATION_MODE_AMG);
} while (ret2 && USE_I2C_ADDR2);
/////////////////
// main loop
/////////////////
while (1) {
// every 1ms do
if (g_flagClock) {
g_flagClock = 0;
// every 10ms do
if (g_clock > 10) {
g_clock = 0;
// just to be sure
accX1 = accY1 = accZ1 = ret1 = 0;
accX2 = accY2 = accZ2 = ret2 = 0;
// read data from sensor 1
#if USE_I2C_ADDR1
bno055.dev_addr = BNO055_I2C_ADDR1;
ret1 = bno055_read_accel_x(&accX1);
ret1 |= bno055_read_accel_y(&accY1);
ret1 |= bno055_read_accel_z(&accZ1);
if (ret1 != 0) {
accX1 = accY1 = accZ1 = 0;
}
#endif // USE_I2C_ADDR1
// read data from sensor 2
#if USE_I2C_ADDR2
bno055.dev_addr = BNO055_I2C_ADDR2;
ret2 = bno055_read_accel_x(&accX2);
ret2 |= bno055_read_accel_y(&accY2);
ret2 |= bno055_read_accel_z(&accZ2);
if (ret2 != 0) {
accX2 = accY2 = accZ2 = 0;
}
#endif // USE_I2C_ADDR2
// send data to PC via UART1. Use SW FIFO for background operation
len = 0;
buf[len++] = (uint8_t)(accX1 >> 8); // x1-acc (MSB first)
buf[len++] = (uint8_t) accX1;
buf[len++] = (uint8_t)(accY1 >> 8); // y1-acc (MSB first)
buf[len++] = (uint8_t) accY1;
buf[len++] = (uint8_t)(accZ1 >> 8); // z1-acc (MSB first)
buf[len++] = (uint8_t) accZ1;
buf[len++] = (uint8_t)(accX2 >> 8); // x2-acc (MSB first)
buf[len++] = (uint8_t) accX2;
buf[len++] = (uint8_t)(accY2 >> 8); // y2-acc (MSB first)
buf[len++] = (uint8_t) accY2;
buf[len++] = (uint8_t)(accZ2 >> 8); // z2-acc (MSB first)
buf[len++] = (uint8_t) accZ2;
uart1_send_buf(len, buf);
// indicate I2C status via red LED (on=ok)
GPIO_SET(PORT_H,PIN_3, ret1|ret2);
// show life beat via green LED
if (++count > 20) {
count = 0;
GPIO_TOGGLE(PORT_H,PIN_2);
// print to LCD
sprintf(buf, "%02d %03d %03d %03d", (int) ret1, (int) accX1, (int) accY1, (int) accZ1);
lcd_print(1, 1, buf);
sprintf(buf, "%02d %03d %03d %03d", (int) ret2, (int) accX2, (int) accY2, (int) accZ2);
lcd_print(2, 1, buf);
}
} // loop 10ms
} // loop 1ms
} // main loop
