// 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;
}
int
bno055_config(struct bno055 *bno055, struct bno055_cfg *cfg)
{
int rc;
uint8_t id;
uint8_t mode;
/* Check if we can read the chip address */
rc = bno055_get_chip_id(&id);
if (rc) {
goto err;
}
if (id != BNO055_ID) {
os_time_delay((OS_TICKS_PER_SEC * 100)/1000 + 1);
rc = bno055_get_chip_id(&id);
if (rc) {
goto err;
}
if(id != BNO055_ID) {
rc = SYS_EINVAL;
goto err;
}
}
/* Reset sensor */
rc = bno055_write8(BNO055_SYS_TRIGGER_ADDR, BNO055_SYS_TRIGGER_RST_SYS);
if (rc) {
goto err;
}
os_time_delay(OS_TICKS_PER_SEC);
rc = bno055_set_opr_mode(BNO055_OPR_MODE_CONFIG);
if (rc) {
goto err;
}
/* Set to normal power mode */
rc = bno055_set_pwr_mode(cfg->bc_pwr_mode);
if (rc) {
goto err;
}
bno055->cfg.bc_pwr_mode = cfg->bc_pwr_mode;
/**
* As per Section 5.5 in the BNO055 Datasheet,
* external crystal should be used for accurate
* results
*/
rc = bno055_set_ext_xtal_use(cfg->bc_use_ext_xtal, BNO055_OPR_MODE_CONFIG);
if (rc) {
goto err;
}
bno055->cfg.bc_use_ext_xtal = cfg->bc_use_ext_xtal;
/* Setting units and data output format */
rc = bno055_set_units(cfg->bc_units);
if (rc) {
goto err;
}
bno055->cfg.bc_units = cfg->bc_units;
/* Change mode to requested mode */
rc = bno055_set_opr_mode(cfg->bc_opr_mode);
if (rc) {
goto err;
}
os_time_delay(OS_TICKS_PER_SEC/2);
rc = bno055_get_opr_mode(&mode);
if (rc) {
goto err;
}
if (cfg->bc_opr_mode != mode) {
/* Trying to set operation mode again */
rc = bno055_set_opr_mode(cfg->bc_opr_mode);
if (rc) {
goto err;
}
rc = bno055_get_opr_mode(&mode);
if (rc) {
goto err;
}
if (cfg->bc_opr_mode != mode) {
BNO055_ERR("Config mode and read mode do not match.\n");
rc = SYS_EINVAL;
goto err;
}
}
bno055->cfg.bc_opr_mode = cfg->bc_opr_mode;
rc = bno055_acc_cfg(cfg);
if (rc) {
goto err;
}
return 0;
err:
return rc;
}
