/* * Author: Jon Trulson * Copyright (c) 2016-2017 Intel Corporation. * * The MIT License * * Permission is hereby granted, free of charge, to any person obtaining * a copy of this software and associated documentation files (the * "Software"), to deal in the Software without restriction, including * without limitation the rights to use, copy, modify, merge, publish, * distribute, sublicense, and/or sell copies of the Software, and to * permit persons to whom the Software is furnished to do so, subject to * the following conditions: * * The above copyright notice and this permission notice shall be * included in all copies or substantial portions of the Software. * * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE * LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION * OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION * WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ #include #include #include #include "bno055.h" // macro for converting a uint8_t low/high pair into a float #define INT16_TO_FLOAT(l, h) \ (float)( (int16_t)( (l) | ((h) << 8) ) ) // clear internal data items static void _clear_data(const bno055_context dev) { assert(dev != NULL); dev->magX = dev->magY = dev->magZ = 0; dev->accX = dev->accY = dev->accZ = 0; dev->gyrX = dev->gyrY = dev->gyrZ = 0; dev->eulHeading = dev->eulRoll = dev->eulPitch = 0; dev->quaW = dev->quaX = dev->quaY = dev->quaZ = 0; dev->liaX = dev->liaY = dev->liaZ = 0; dev->grvX = dev->grvY = dev->grvZ = 0; } // load fusion data static upm_result_t _update_fusion_data(const bno055_context dev) { assert(dev != NULL); // bail (with success code) if we are in config mode, or aren't in // a fusion mode... if (dev->currentMode == BNO055_OPERATION_MODE_CONFIGMODE || dev->currentMode < BNO055_OPERATION_MODE_IMU) return UPM_SUCCESS; if (bno055_set_page(dev, 0, false)) return UPM_ERROR_OPERATION_FAILED; // FIXME/MAYBE? - abort early if SYS calibration is == 0? const int fusionBytes = 26; uint8_t buf[fusionBytes]; if (bno055_read_regs(dev, BNO055_REG_EUL_HEADING_LSB, buf, fusionBytes)) return UPM_ERROR_OPERATION_FAILED; dev->eulHeading = INT16_TO_FLOAT(buf[0], buf[1]); dev->eulRoll = INT16_TO_FLOAT(buf[2], buf[3]); dev->eulPitch = INT16_TO_FLOAT(buf[4], buf[5]); dev->quaW = INT16_TO_FLOAT(buf[6], buf[7]); dev->quaX = INT16_TO_FLOAT(buf[8], buf[9]); dev->quaY = INT16_TO_FLOAT(buf[10], buf[11]); dev->quaZ = INT16_TO_FLOAT(buf[12], buf[13]); dev->liaX = INT16_TO_FLOAT(buf[14], buf[15]); dev->liaY = INT16_TO_FLOAT(buf[16], buf[17]); dev->liaZ = INT16_TO_FLOAT(buf[18], buf[19]); dev->grvX = INT16_TO_FLOAT(buf[20], buf[21]); dev->grvY = INT16_TO_FLOAT(buf[22], buf[23]); dev->grvZ = INT16_TO_FLOAT(buf[24], buf[25]); return UPM_SUCCESS; } // update non-fusion data static upm_result_t _update_non_fusion_data(const bno055_context dev) { assert(dev != NULL); // bail (with success code) if we are in config mode... if (dev->currentMode == BNO055_OPERATION_MODE_CONFIGMODE) return UPM_SUCCESS; if (bno055_set_page(dev, 0, false)) return UPM_ERROR_OPERATION_FAILED; const int nonFusionBytes = 18; uint8_t buf[nonFusionBytes]; if (bno055_read_regs(dev, BNO055_REG_ACC_DATA_X_LSB, buf, nonFusionBytes)) return UPM_ERROR_OPERATION_FAILED; dev->accX = INT16_TO_FLOAT(buf[0], buf[1]); dev->accY = INT16_TO_FLOAT(buf[2], buf[3]); dev->accZ = INT16_TO_FLOAT(buf[4], buf[5]); dev->magX = INT16_TO_FLOAT(buf[6], buf[7]); dev->magY = INT16_TO_FLOAT(buf[8], buf[9]); dev->magZ = INT16_TO_FLOAT(buf[10], buf[11]); dev->gyrX = INT16_TO_FLOAT(buf[12], buf[13]); dev->gyrY = INT16_TO_FLOAT(buf[14], buf[15]); dev->gyrZ = INT16_TO_FLOAT(buf[16], buf[17]); return UPM_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; } void bno055_close(bno055_context dev) { assert(dev != NULL); bno055_uninstall_isr(dev); if (dev->i2c) mraa_i2c_stop(dev->i2c); free(dev); } upm_result_t bno055_update(const bno055_context dev) { assert(dev != NULL); if (bno055_set_page(dev, 0, false)) return UPM_ERROR_OPERATION_FAILED; // temperature first, always in Celsius uint8_t tempreg = 0; if (bno055_read_reg(dev, BNO055_REG_TEMPERATURE, &tempreg)) return UPM_ERROR_OPERATION_FAILED; dev->temperature = (float)((int8_t)tempreg); if (_update_fusion_data(dev)) return UPM_ERROR_OPERATION_FAILED; if (_update_non_fusion_data(dev)) return UPM_ERROR_OPERATION_FAILED; return UPM_SUCCESS; } upm_result_t bno055_read_reg(const bno055_context dev, uint8_t reg, uint8_t *retval) { assert(dev != NULL); int rv = mraa_i2c_read_byte_data(dev->i2c, reg); if (rv < 0) { printf("%s: mraa_i2c_read_byte_data() failed\n", __FUNCTION__); return UPM_ERROR_OPERATION_FAILED; } if (retval) *retval = (uint8_t)(rv & 0xff); return UPM_SUCCESS; } upm_result_t bno055_read_regs(const bno055_context dev, uint8_t reg, uint8_t *buffer, size_t len) { assert(dev != NULL); if (mraa_i2c_read_bytes_data(dev->i2c, reg, buffer, len) < 0) { printf("%s: mraa_i2c_read_bytes() failed\n", __FUNCTION__); return UPM_ERROR_OPERATION_FAILED; } return UPM_SUCCESS; } upm_result_t bno055_write_reg(const bno055_context dev, uint8_t reg, uint8_t val) { assert(dev != NULL); if (mraa_i2c_write_byte_data(dev->i2c, val, reg)) { printf("%s: mraa_i2c_write_byte_data() failed\n", __FUNCTION__); return UPM_ERROR_OPERATION_FAILED; } return UPM_SUCCESS; } upm_result_t bno055_write_regs(const bno055_context dev, uint8_t reg, uint8_t *buffer, size_t len) { assert(dev != NULL); uint8_t buf[len + 1]; buf[0] = reg; for (size_t i=0; ii2c, buf, len + 1)) { printf("%s: mraa_i2c_write() failed\n", __FUNCTION__); return UPM_ERROR_OPERATION_FAILED; } return UPM_SUCCESS; } upm_result_t bno055_get_chip_id(const bno055_context dev, uint8_t *chip_id) { assert(dev != NULL); if (bno055_set_page(dev, 0, false)) return UPM_ERROR_OPERATION_FAILED; return bno055_read_reg(dev, BNO055_REG_CHIP_ID, chip_id); } upm_result_t bno055_get_acc_id(const bno055_context dev, uint8_t *chip_id) { assert(dev != NULL); if (bno055_set_page(dev, 0, false)) return UPM_ERROR_OPERATION_FAILED; return bno055_read_reg(dev, BNO055_REG_ACC_ID, chip_id); } upm_result_t bno055_get_mag_id(const bno055_context dev, uint8_t *chip_id) { assert(dev != NULL); if (bno055_set_page(dev, 0, false)) return UPM_ERROR_OPERATION_FAILED; return bno055_read_reg(dev, BNO055_REG_MAG_ID, chip_id); } upm_result_t bno055_get_gyr_id(const bno055_context dev, uint8_t *chip_id) { assert(dev != NULL); if (bno055_set_page(dev, 0, false)) return UPM_ERROR_OPERATION_FAILED; return bno055_read_reg(dev, BNO055_REG_GYR_ID, chip_id); } upm_result_t bno055_get_sw_revision(const bno055_context dev, uint16_t *sw_rev) { assert(dev != NULL); if (bno055_set_page(dev, 0, false)) return UPM_ERROR_OPERATION_FAILED; uint8_t lsb = 0, msb = 0; if (bno055_read_reg(dev, BNO055_REG_SW_REV_ID_LSB, &lsb)) return UPM_ERROR_OPERATION_FAILED; if (bno055_read_reg(dev, BNO055_REG_SW_REV_ID_MSB, &msb)) return UPM_ERROR_OPERATION_FAILED; if (sw_rev) *sw_rev = (uint16_t)(lsb | (msb << 8)); return UPM_SUCCESS; } upm_result_t bno055_get_bootloader_id(const bno055_context dev, uint8_t *bl_id) { assert(dev != NULL); if (bno055_set_page(dev, 0, false)) return UPM_ERROR_OPERATION_FAILED; return bno055_read_reg(dev, BNO055_REG_BL_REV_ID, bl_id); } upm_result_t bno055_set_page(const bno055_context dev, uint8_t page, bool force) { assert(dev != NULL); // page can only be 0 or 1 if (!(page == 0 || page == 1)) { printf("%s: page number can only be 0 or 1.\n", __FUNCTION__); return UPM_ERROR_INVALID_PARAMETER; } if (force || page != dev->currentPage) { if (bno055_write_reg(dev, BNO055_REG_PAGE_ID, page)) return UPM_ERROR_OPERATION_FAILED; } dev->currentPage = page; 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; } upm_result_t bno055_set_temperature_source(const bno055_context dev, BNO055_TEMP_SOURCES_T src) { assert(dev != NULL); if (bno055_set_page(dev, 0, false)) return UPM_ERROR_OPERATION_FAILED; return bno055_write_reg(dev, BNO055_REG_TEMP_SOURCE, src); } upm_result_t bno055_set_temperature_units_celsius(const bno055_context dev) { assert(dev != NULL); if (bno055_set_page(dev, 0, false)) return UPM_ERROR_OPERATION_FAILED; uint8_t reg = 0; if (bno055_read_reg(dev, BNO055_REG_UNIT_SEL, ®)) return UPM_ERROR_OPERATION_FAILED; reg &= ~BNO055_UNIT_SEL_TEMP_UNIT; return bno055_write_reg(dev, BNO055_REG_UNIT_SEL, reg); } upm_result_t bno055_set_accelerometer_units(const bno055_context dev, bool mg) { assert(dev != NULL); if (bno055_set_page(dev, 0, false)) return UPM_ERROR_OPERATION_FAILED; uint8_t reg = 0; if (bno055_read_reg(dev, BNO055_REG_UNIT_SEL, ®)) return UPM_ERROR_OPERATION_FAILED; if (mg) { reg |= BNO055_UNIT_SEL_ACC_UNIT; dev->accUnitScale = 1.0; } else { reg &= ~BNO055_UNIT_SEL_ACC_UNIT; dev->accUnitScale = 100.0; } return bno055_write_reg(dev, BNO055_REG_UNIT_SEL, reg); } upm_result_t bno055_set_gyroscope_units(const bno055_context dev, bool radians) { assert(dev != NULL); if (bno055_set_page(dev, 0, false)) return UPM_ERROR_OPERATION_FAILED; uint8_t reg = 0; if (bno055_read_reg(dev, BNO055_REG_UNIT_SEL, ®)) return UPM_ERROR_OPERATION_FAILED; if (radians) { reg |= BNO055_UNIT_SEL_GYR_UNIT; dev->gyrUnitScale = 900.0; } else { reg &= ~BNO055_UNIT_SEL_GYR_UNIT; dev->gyrUnitScale = 16.0; } return bno055_write_reg(dev, BNO055_REG_UNIT_SEL, reg); } upm_result_t bno055_set_euler_units(const bno055_context dev, bool radians) { assert(dev != NULL); if (bno055_set_page(dev, 0, false)) return UPM_ERROR_OPERATION_FAILED; uint8_t reg = 0; if (bno055_read_reg(dev, BNO055_REG_UNIT_SEL, ®)) return UPM_ERROR_OPERATION_FAILED; if (radians) { reg |= BNO055_UNIT_SEL_EUL_UNIT; dev->eulUnitScale = 900.0; } else { reg &= ~BNO055_UNIT_SEL_EUL_UNIT; dev->eulUnitScale = 16.0; } return bno055_write_reg(dev, BNO055_REG_UNIT_SEL, reg); } upm_result_t bno055_set_operation_mode(const bno055_context dev, BNO055_OPERATION_MODES_T mode) { assert(dev != NULL); if (bno055_set_page(dev, 0, false)) return UPM_ERROR_OPERATION_FAILED; // we clear all of our loaded data on mode changes _clear_data(dev); uint8_t reg = 0; if (bno055_read_reg(dev, BNO055_REG_OPER_MODE, ®)) return UPM_ERROR_OPERATION_FAILED; reg &= ~(_BNO055_OPR_MODE_OPERATION_MODE_MASK << _BNO055_OPR_MODE_OPERATION_MODE_SHIFT); reg |= (mode << _BNO055_OPR_MODE_OPERATION_MODE_SHIFT); if (bno055_write_reg(dev, BNO055_REG_OPER_MODE, reg)) return UPM_ERROR_OPERATION_FAILED; dev->currentMode = mode; upm_delay_us(30); return UPM_SUCCESS; } upm_result_t bno055_get_calibration_status(const bno055_context dev, int *mag, int *acc, int *gyr, int *sys) { assert(dev != NULL); if (bno055_set_page(dev, 0, false)) return UPM_ERROR_OPERATION_FAILED; uint8_t reg = 0; if (bno055_read_reg(dev, BNO055_REG_CALIB_STAT, ®)) return UPM_ERROR_OPERATION_FAILED; if (mag) *mag = (reg >> _BNO055_CALIB_STAT_MAG_SHIFT) & _BNO055_CALIB_STAT_MAG_MASK; if (acc) *acc = (reg >> _BNO055_CALIB_STAT_ACC_SHIFT) & _BNO055_CALIB_STAT_ACC_MASK; if (gyr) *gyr = (reg >> _BNO055_CALIB_STAT_GYR_SHIFT) & _BNO055_CALIB_STAT_GYR_MASK; if (sys) *sys = (reg >> _BNO055_CALIB_STAT_SYS_SHIFT) & _BNO055_CALIB_STAT_SYS_MASK; return UPM_SUCCESS; } bool bno055_is_fully_calibrated(const bno055_context dev) { assert(dev != NULL); int mag, acc, gyr, sys; // fail on error if (bno055_get_calibration_status(dev, &mag, &acc, &gyr, &sys)) return false; // all of them equal to 3 means fully calibrated if (mag == 3 && acc == 3 && gyr == 3 && sys == 3) return true; else return false; } upm_result_t bno055_reset_system(const bno055_context dev) { assert(dev != NULL); if (bno055_set_page(dev, 0, false)) return UPM_ERROR_OPERATION_FAILED; uint8_t reg = 0; if (bno055_read_reg(dev, BNO055_REG_SYS_TRIGGER, ®)) return UPM_ERROR_OPERATION_FAILED; reg |= BNO055_SYS_TRIGGER_RST_SYS; if (bno055_write_reg(dev, BNO055_REG_SYS_TRIGGER, reg)) return UPM_ERROR_OPERATION_FAILED; upm_delay(1); return UPM_SUCCESS; } upm_result_t bno055_reset_interrupt_status(const bno055_context dev) { assert(dev != NULL); if (bno055_set_page(dev, 0, false)) return UPM_ERROR_OPERATION_FAILED; uint8_t reg = 0; if (bno055_read_reg(dev, BNO055_REG_SYS_TRIGGER, ®)) return UPM_ERROR_OPERATION_FAILED; reg |= BNO055_SYS_TRIGGER_RST_INT; return bno055_write_reg(dev, BNO055_REG_SYS_TRIGGER, reg); } upm_result_t bno055_get_interrupt_status(const bno055_context dev, uint8_t *istat) { assert(dev != NULL); if (bno055_set_page(dev, 0, false)) return UPM_ERROR_OPERATION_FAILED; return bno055_read_reg(dev, BNO055_REG_INT_STA, istat); } upm_result_t bno055_get_interrupt_enable(const bno055_context dev, uint8_t *ienable) { assert(dev != NULL); if (bno055_set_page(dev, 1, false)) return UPM_ERROR_OPERATION_FAILED; return bno055_read_reg(dev, BNO055_REG_INT_EN, ienable); } upm_result_t bno055_set_interrupt_enable(const bno055_context dev, uint8_t enables) { assert(dev != NULL); if (bno055_set_page(dev, 1, false)) return UPM_ERROR_OPERATION_FAILED; return bno055_write_reg(dev, BNO055_REG_INT_EN, enables); } upm_result_t bno055_get_interrupt_mask(const bno055_context dev, uint8_t *imask) { assert(dev != NULL); if (bno055_set_page(dev, 1, false)) return UPM_ERROR_OPERATION_FAILED; return bno055_read_reg(dev, BNO055_REG_INT_MSK, imask); } upm_result_t bno055_set_interrupt_mask(const bno055_context dev, uint8_t mask) { assert(dev != NULL); if (bno055_set_page(dev, 1, false)) return UPM_ERROR_OPERATION_FAILED; return bno055_write_reg(dev, BNO055_REG_INT_MSK, mask); } upm_result_t bno055_get_system_status(const bno055_context dev, BNO055_SYS_STATUS_T *sys_stat) { assert(dev != NULL); if (bno055_set_page(dev, 0, false)) return UPM_ERROR_OPERATION_FAILED; uint8_t reg = 0; if (bno055_read_reg(dev, BNO055_REG_SYS_STATUS, ®)) return UPM_ERROR_OPERATION_FAILED; if (sys_stat) *sys_stat = (BNO055_SYS_STATUS_T)reg; return UPM_SUCCESS; } upm_result_t bno055_get_system_error(const bno055_context dev, BNO055_SYS_ERR_T *sys_err) { assert(dev != NULL); if (bno055_set_page(dev, 0, false)) return UPM_ERROR_OPERATION_FAILED; uint8_t reg = 0; if (bno055_read_reg(dev, BNO055_REG_SYS_ERROR, ®)) return UPM_ERROR_OPERATION_FAILED; if (sys_err) *sys_err = (BNO055_SYS_ERR_T)reg; return UPM_SUCCESS; } upm_result_t bno055_read_calibration_data(const bno055_context dev, uint8_t *data, size_t len) { assert(dev != NULL); assert(data != NULL); if (!bno055_is_fully_calibrated(dev)) { printf("%s: Sensor must be fully calibrated first.\n", __FUNCTION__); return UPM_ERROR_NO_DATA; } if (len != BNO055_CALIBRATION_DATA_SIZE) { printf("%s: len must equal BNO055_CALIBRATION_DATA_SIZE (%d).\n", __FUNCTION__, BNO055_CALIBRATION_DATA_SIZE); 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; if (bno055_read_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_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; } float bno055_get_temperature(const bno055_context dev) { assert(dev != NULL); return dev->temperature; } void bno055_get_euler_angles(const bno055_context dev, float *heading, float *roll, float *pitch) { assert(dev != NULL); if (heading) *heading = dev->eulHeading / dev->eulUnitScale; if (roll) *roll = dev->eulRoll / dev->eulUnitScale; if (pitch) *pitch = dev->eulPitch / dev->eulUnitScale; } void bno055_get_quaternions(const bno055_context dev, float *w, float *x, float *y, float *z) { assert(dev != NULL); // from the datasheet const float scale = (float)(1.0 / (float)(1 << 14)); if (w) *w = dev->quaW * scale; if (x) *x = dev->quaX * scale; if (y) *y = dev->quaY * scale; if (z) *z = dev->quaZ * scale; } void bno055_get_linear_acceleration(const bno055_context dev, float *x, float *y, float *z) { assert(dev != NULL); if (x) *x = dev->liaX / dev->accUnitScale; if (y) *y = dev->liaY / dev->accUnitScale; if (z) *z = dev->liaZ / dev->accUnitScale; } void bno055_get_gravity_vectors(const bno055_context dev, float *x, float *y, float *z) { assert(dev != NULL); if (x) *x = dev->grvX / dev->accUnitScale; if (y) *y = dev->grvY / dev->accUnitScale; if (z) *z = dev->grvZ / dev->accUnitScale; } void bno055_get_accelerometer(const bno055_context dev, float *x, float *y, float *z) { assert(dev != NULL); if (x) *x = dev->accX / dev->accUnitScale; if (y) *y = dev->accY / dev->accUnitScale; if (z) *z = dev->accZ / dev->accUnitScale; } void bno055_get_magnetometer(const bno055_context dev, float *x, float *y, float *z) { assert(dev != NULL); // from the datasheet - 16 uT's per LSB const float scale = 16.0; if (x) *x = dev->magX / scale; if (y) *y = dev->magY / scale; if (z) *z = dev->magZ / scale; } void bno055_get_gyroscope(const bno055_context dev, float *x, float *y, float *z) { assert(dev != NULL); if (x) *x = dev->gyrX / dev->gyrUnitScale; if (y) *y = dev->gyrY / dev->gyrUnitScale; if (z) *z = dev->gyrZ / dev->gyrUnitScale; } upm_result_t bno055_set_acceleration_config(const bno055_context dev, BNO055_ACC_RANGE_T range, BNO055_ACC_BW_T bw, BNO055_ACC_PWR_MODE_T pwr) { assert(dev != NULL); if (bno055_set_page(dev, 1, false)) return UPM_ERROR_OPERATION_FAILED; uint8_t reg = ((range << _BNO055_ACC_CONFIG_ACC_RANGE_SHIFT) | (bw << _BNO055_ACC_CONFIG_ACC_BW_SHIFT) | (pwr << _BNO055_ACC_CONFIG_ACC_PWR_MODE_SHIFT)); return bno055_write_reg(dev, BNO055_REG_ACC_CONFIG, reg); } upm_result_t bno055_set_magnetometer_config(const bno055_context dev, BNO055_MAG_ODR_T odr, BNO055_MAG_OPR_T opr, BNO055_MAG_POWER_T pwr) { assert(dev != NULL); if (bno055_set_page(dev, 1, false)) return UPM_ERROR_OPERATION_FAILED; uint8_t reg = ((odr << _BNO055_MAG_CONFIG_MAG_ODR_SHIFT) | (opr << _BNO055_MAG_CONFIG_MAG_OPR_MODE_SHIFT) | (pwr << _BNO055_MAG_CONFIG_MAG_POWER_MODE_SHIFT)); return bno055_write_reg(dev, BNO055_REG_MAG_CONFIG, reg); } upm_result_t bno055_set_gyroscope_config(const bno055_context dev, BNO055_GYR_RANGE_T range, BNO055_GYR_BW_T bw, BNO055_GYR_POWER_MODE_T pwr) { assert(dev != NULL); if (bno055_set_page(dev, 1, false)) return UPM_ERROR_OPERATION_FAILED; uint8_t reg = ((range << _BNO055_GYR_CONFIG0_GYR_RANGE_SHIFT) | (bw << _BNO055_GYR_CONFIG0_GYR_BW_SHIFT)); if (bno055_write_reg(dev, BNO055_REG_GYR_CONFIG0, reg)) return UPM_ERROR_OPERATION_FAILED; reg = (pwr << _BNO055_GYR_CONFIG1_GYR_POWER_MODE_SHIFT); return bno055_write_reg(dev, BNO055_REG_GYR_CONFIG1, reg); } upm_result_t bno055_install_isr(const bno055_context dev, int gpio, mraa_gpio_edge_t level, void (*isr)(void *), void *arg) { assert(dev != NULL); // delete any existing ISR and GPIO context bno055_uninstall_isr(dev); // create gpio context if (!(dev->gpio = mraa_gpio_init(gpio))) { printf("%s: mraa_gpio_init() failed.\n", __FUNCTION__); return UPM_ERROR_OPERATION_FAILED; } mraa_gpio_dir(dev->gpio, MRAA_GPIO_IN); if (mraa_gpio_isr(dev->gpio, level, isr, arg)) { mraa_gpio_close(dev->gpio); dev->gpio = NULL; printf("%s: mraa_gpio_isr() failed.\n", __FUNCTION__); return UPM_ERROR_OPERATION_FAILED; } return UPM_SUCCESS; } void bno055_uninstall_isr(const bno055_context dev) { assert(dev != NULL); if (dev->gpio) { mraa_gpio_isr_exit(dev->gpio); mraa_gpio_close(dev->gpio); dev->gpio = NULL; } }