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