// 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; }