alexey.zholtikov/upm
1
0
Fork 0
mirror of https://github.com/eclipse/upm.git synced 2025-03-15 04:57:30 +03:00
Code Issues Actions Packages Projects Releases Wiki Activity
upm/src/bno055/bno055.c
Noel Eck fe7bd75c91 C: Fixes for sign compares in C libraries 
Added explicit error for sign compares to CMake.  Updated a handful of C
source which compared unsigned vs signed.

Signed-off-by: Noel Eck <noel.eck@intel.com>
2018-02-07 14:29:35 -08:00

1058 lines
28 KiB
C
Raw Blame History

/*
* Author: Jon Trulson <jtrulson@ics.com>
* 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 <string.h>
#include <assert.h>
#include <upm_utilities.h>
#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; i<len; i++)
buf[i+1] = buffer[i];
if (mraa_i2c_write(dev->i2c, 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, &reg))
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, &reg))
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, &reg))
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, &reg))
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, &reg))
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, &reg))
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, &reg))
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, &reg))
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, &reg))
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, &reg))
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, &reg))
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;
}
}
Reference in New Issue View Git Blame Copy Permalink