Raspberries/upm
Public Access
2
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

bno055: C port; C++ wraps C

Browse Source 
The API has been changed in some cases - see the apichanges.md
document.

In addition, this driver uses a new upm_vectortypes.i SWIG interface
file to provide a mechanism for methods that return a vector of floats
and ints instead of a pointer to an array.

This works much nicer than C array pointers, and results in Python/JS/Java
code that looks much more "natural" to the language in use.

The Python, JS, and Java examples have been changed to use these
methods.  Support for the "old" C-style pointer methods are still
provided for backward compatibility with existing code.

As an example - to retrieve the x, y, and z data for Euler Angles from
the bno055, the original python code would look something like:

       ...
       x = sensorObj.new_floatp()
       y = sensorObj.new_floatp()
       z = sensorObj.new_floatp()
       ...
       sensor.getEulerAngles(x, y, z)
       ...
       print("Euler: Heading:", sensorObj.floatp_value(x), end=' ')
       print(" Roll:", sensorObj.floatp_value(y), end=' ')
       ...

Now the equivalent code is simply:

       floatData = sensor.getEulerAngles()
       print("Euler: Heading:", floatData[0], ...
       print(" Roll:", floatData[1], end=' ')
       ...

Additionally, interrupt handling for Java is now implemented
completely in the C++ header file now rather than the .cxx file, so no
special SWIG processing is required anymore. See Issue #518 .

Signed-off-by: Jon Trulson <jtrulson@ics.com>
This commit is contained in:
Jon Trulson
2017-03-07 12:43:44 -07:00
parent 2bdde21a2f
commit d4b536b593
16 changed files with 3382 additions and 2155 deletions
Download Patch File Download Diff File Expand all files Collapse all files

702
src/bno055/bno055.cxx
View File

@@ -1,6 +1,8 @@
/*
* Author: Jon Trulson <jtrulson@ics.com>
* Copyright (c) 2016 Intel Corporation.
* Copyright (c) 2016-17 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
@@ -22,788 +24,342 @@
* WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/
#include <unistd.h>
#include <iostream>
#include <stdexcept>
#include <string>
#include <string.h>
#include "bno055.hpp"
using namespace upm;
using namespace std;
// conversion from fahrenheit to celsius and back
static float f2c(float f)
{
return ((f - 32.0) / (9.0 / 5.0));
}
// conversion from Celsius to Fahrenheit
static float c2f(float c)
{
return (c * (9.0 / 5.0) + 32.0);
return (c * (9.0 / 5.0) + 32.0);
}
BNO055::BNO055(int bus, uint8_t addr) :
m_i2c(bus), m_gpioIntr(0)
m_bno055(bno055_init(bus, addr))
{
m_addr = addr;
clearData();
mraa::Result rv;
if ( (rv = m_i2c.address(m_addr)) != mraa::SUCCESS)
{
throw std::runtime_error(string(__FUNCTION__) +
": I2c.address() failed");
return;
}
// forcibly set page 0, so we are synced
setPage(0, true);
// set config mode
setOperationMode(OPERATION_MODE_CONFIGMODE);
// default to internal clock
setClockExternal(false);
// we specifically avoid doing a reset so that if the device is
// already calibrated, it will remain so.
// check the chip id
uint8_t chipID = readReg(REG_CHIP_ID);
if (chipID != BNO055_CHIPID)
{
throw std::runtime_error(string(__FUNCTION__)
+ ": invalid chip ID. Expected "
+ std::to_string(int(BNO055_CHIPID))
+ ", got "
+ std::to_string(int(chipID)));
return;
}
// default to temperature C
setTemperatureUnits(true);
// default to accelerometer temp
setTemperatureSource(TEMP_SOURCE_ACC);
// set accel units to m/s^2
setAccelerometerUnits(false);
// set gyro units to degrees
setGyroscopeUnits(false);
// set Euler units to degrees
setEulerUnits(false);
// by default, we set the operating mode to the NDOF fusion mode
setOperationMode(OPERATION_MODE_NDOF);
if (!m_bno055)
throw std::runtime_error(string(__FUNCTION__)
+ ": bno055_init() failed");
}
BNO055::~BNO055()
{
uninstallISR();
bno055_close(m_bno055);
}
void BNO055::update()
{
setPage(0);
// temperature first, we always store as C
float tmpF = float((int8_t)readReg(REG_TEMPERATURE));
if (m_tempIsC)
m_temperature = tmpF;
else
m_temperature = f2c(tmpF * 2.0);
updateFusionData();
updateNonFusionData();
if (bno055_update(m_bno055))
throw std::runtime_error(string(__FUNCTION__)
+ ": bno055_update() failed");
}
uint8_t BNO055::readReg(uint8_t reg)
{
return m_i2c.readReg(reg);
return bno055_read_reg(m_bno055, reg);
}
void BNO055::readRegs(uint8_t reg, uint8_t *buffer, int len)
{
m_i2c.readBytesReg(reg, buffer, len);
if (bno055_read_regs(m_bno055, reg, buffer, len))
throw std::runtime_error(string(__FUNCTION__)
+ ": bno055_read_regs() failed");
}
bool BNO055::writeReg(uint8_t reg, uint8_t val)
void BNO055::writeReg(uint8_t reg, uint8_t val)
{
mraa::Result rv;
if ((rv = m_i2c.writeReg(reg, val)) != mraa::SUCCESS)
{
throw std::runtime_error(std::string(__FUNCTION__)
+ ": I2c.writeReg() failed");
}
return true;
if (bno055_write_reg(m_bno055, reg, val))
throw std::runtime_error(string(__FUNCTION__)
+ ": bno055_write_reg() failed");
}
bool BNO055::writeRegs(uint8_t reg, uint8_t *buffer, int len)
void BNO055::writeRegs(uint8_t reg, uint8_t *buffer, int len)
{
uint8_t buf[len + 1];
buf[0] = reg;
for (int i=0; i<len; i++)
buf[i+1] = buffer[i];
mraa::Result rv;
if ((rv = m_i2c.write(buf, len+1)) != mraa::SUCCESS)
{
throw std::runtime_error(std::string(__FUNCTION__)
+ ": I2c.write() failed");
}
return true;
if (bno055_write_regs(m_bno055, reg, buffer, len))
throw std::runtime_error(string(__FUNCTION__)
+ ": bno055_write_regs() failed");
}
uint8_t BNO055::getChipID()
{
setPage(0);
return readReg(REG_CHIP_ID);
return bno055_get_chip_id(m_bno055);
}
uint8_t BNO055::getACCID()
{
setPage(0);
return readReg(REG_ACC_ID);
return bno055_get_acc_id(m_bno055);
}
uint8_t BNO055::getMAGID()
{
setPage(0);
return readReg(REG_MAG_ID);
return bno055_get_mag_id(m_bno055);
}
uint8_t BNO055::getGYRID()
{
setPage(0);
return readReg(REG_GYR_ID);
return bno055_get_gyr_id(m_bno055);
}
uint16_t BNO055::getSWRevID()
{
setPage(0);
uint16_t vers = uint16_t( readReg(REG_SW_REV_ID_LSB) |
(readReg(REG_SW_REV_ID_MSB) << 8) );
return vers;
return bno055_get_sw_revision(m_bno055);
}
uint8_t BNO055::getBootLoaderID()
{
setPage(0);
return readReg(REG_BL_REV_ID);
return bno055_get_bootloader_id(m_bno055);
}
void BNO055::setPage(uint8_t page, bool force)
{
// page can only be 0 or 1
if (!(page == 0 || page == 1))
throw std::out_of_range(string(__FUNCTION__) +
": page can only be 0 or 1");
if (force || page != m_currentPage)
writeReg(REG_PAGE_ID, page);
m_currentPage = page;
if (bno055_set_page(m_bno055, page, force))
throw std::runtime_error(string(__FUNCTION__)
+ ": bno055_set_page() failed");
}
void BNO055::setClockExternal(bool extClock)
{
setPage(0);
// first we need to be in config mode
OPERATION_MODES_T currentMode = m_currentMode;
setOperationMode(OPERATION_MODE_CONFIGMODE);
uint8_t reg = readReg(REG_SYS_TRIGGER);
if (extClock)
reg |= SYS_TRIGGER_CLK_SEL;
else
reg &= ~SYS_TRIGGER_CLK_SEL;
writeReg(REG_SYS_TRIGGER, reg);
// now reset our operating mode
setOperationMode(currentMode);
bno055_set_clock_external(m_bno055, extClock);
}
void BNO055::setTemperatureSource(TEMP_SOURCES_T src)
void BNO055::setTemperatureSource(BNO055_TEMP_SOURCES_T src)
{
setPage(0);
writeReg(REG_TEMP_SOURCE, src);
}
void BNO055::setTemperatureUnits(bool celsius)
{
setPage(0);
uint8_t reg = readReg(REG_UNIT_SEL);
if (celsius)
reg &= ~UNIT_SEL_TEMP_UNIT;
else
reg |= UNIT_SEL_TEMP_UNIT;
writeReg(REG_UNIT_SEL, reg);
m_tempIsC = celsius;
bno055_set_temperature_source(m_bno055, src);
}
void BNO055::setAccelerometerUnits(bool mg)
{
setPage(0);
uint8_t reg = readReg(REG_UNIT_SEL);
if (mg)
{
reg |= UNIT_SEL_ACC_UNIT;
m_accUnitScale = 1.0;
}
else
{
reg &= ~UNIT_SEL_ACC_UNIT;
m_accUnitScale = 100.0;
}
writeReg(REG_UNIT_SEL, reg);
bno055_set_accelerometer_units(m_bno055, mg);
}
void BNO055::setGyroscopeUnits(bool radians)
{
setPage(0);
uint8_t reg = readReg(REG_UNIT_SEL);
if (radians)
{
reg |= UNIT_SEL_GYR_UNIT;
m_gyrUnitScale = 900.0;
}
else
{
reg &= ~UNIT_SEL_GYR_UNIT;
m_gyrUnitScale = 16.0;
}
writeReg(REG_UNIT_SEL, reg);
bno055_set_gyroscope_units(m_bno055, radians);
}
void BNO055::setEulerUnits(bool radians)
{
setPage(0);
uint8_t reg = readReg(REG_UNIT_SEL);
if (radians)
{
reg |= UNIT_SEL_EUL_UNIT;
m_eulUnitScale = 900.0;
}
else
{
reg &= ~UNIT_SEL_EUL_UNIT;
m_eulUnitScale = 16.0;
}
writeReg(REG_UNIT_SEL, reg);
bno055_set_euler_units(m_bno055, radians);
}
void BNO055::setOperationMode(OPERATION_MODES_T mode)
void BNO055::setOperationMode(BNO055_OPERATION_MODES_T mode)
{
setPage(0);
// we clear all of our loaded data on mode changes
clearData();
uint8_t reg = readReg(REG_OPER_MODE);
reg &= ~(_OPR_MODE_OPERATION_MODE_MASK << _OPR_MODE_OPERATION_MODE_SHIFT);
reg |= (mode << _OPR_MODE_OPERATION_MODE_SHIFT);
writeReg(REG_OPER_MODE, reg);
m_currentMode = mode;
usleep(30);
bno055_set_operation_mode(m_bno055, mode);
}
void BNO055::getCalibrationStatus(int *mag, int *acc, int *gyr, int *sys)
{
setPage(0);
uint8_t reg = readReg(REG_CALIB_STAT);
if (mag)
*mag = (reg >> _CALIB_STAT_MAG_SHIFT) & _CALIB_STAT_MAG_MASK;
if (acc)
*acc = (reg >> _CALIB_STAT_ACC_SHIFT) & _CALIB_STAT_ACC_MASK;
if (gyr)
*gyr = (reg >> _CALIB_STAT_GYR_SHIFT) & _CALIB_STAT_GYR_MASK;
if (sys)
*sys = (reg >> _CALIB_STAT_SYS_SHIFT) & _CALIB_STAT_SYS_MASK;
bno055_get_calibration_status(m_bno055, mag, acc, gyr, sys);
}
int *BNO055::getCalibrationStatus()
vector<int> BNO055::getCalibrationStatus()
{
static int v[4]; // mag, acc, gyr, sys;
int v[4]; // mag, acc, gyr, sys;
getCalibrationStatus(&v[0], &v[1], &v[2], &v[3]);
return v;
getCalibrationStatus(&v[0], &v[1], &v[2], &v[3]);
return vector<int>(v, v+4);
}
bool BNO055::isFullyCalibrated()
{
int mag, acc, gyr, sys;
getCalibrationStatus(&mag, &acc, &gyr, &sys);
// all of them equal to 3 means fully calibrated
if (mag == 3 && acc == 3 && gyr == 3 && sys == 3)
return true;
else
return false;
return bno055_is_fully_calibrated(m_bno055);
}
void BNO055::resetSystem()
{
setPage(0);
uint8_t reg = readReg(REG_SYS_TRIGGER);
reg |= SYS_TRIGGER_RST_SYS;
writeReg(REG_SYS_TRIGGER, reg);
sleep(1);
bno055_reset_system(m_bno055);
}
void BNO055::resetInterruptStatus()
{
setPage(0);
uint8_t reg = readReg(REG_SYS_TRIGGER);
reg |= SYS_TRIGGER_RST_INT;
writeReg(REG_SYS_TRIGGER, reg);
bno055_reset_interrupt_status(m_bno055);
}
uint8_t BNO055::getInterruptStatus()
{
setPage(0);
return readReg(REG_INT_STA);
return bno055_get_interrupt_status(m_bno055);
}
uint8_t BNO055::getInterruptEnable()
{
setPage(1);
return readReg(REG_INT_EN);
return bno055_get_interrupt_enable(m_bno055);
}
void BNO055::setInterruptEnable(uint8_t enables)
{
setPage(1);
writeReg(REG_INT_EN, enables);
return bno055_set_interrupt_enable(m_bno055, enables);
}
uint8_t BNO055::getInterruptMask()
{
setPage(1);
return readReg(REG_INT_MSK);
return bno055_get_interrupt_mask(m_bno055);
}
void BNO055::setInterruptMask(uint8_t mask)
{
setPage(1);
writeReg(REG_INT_MSK, mask);
return bno055_set_interrupt_mask(m_bno055, mask);
}
BNO055::SYS_STATUS_T BNO055::getSystemStatus()
BNO055_SYS_STATUS_T BNO055::getSystemStatus()
{
setPage(0);
return static_cast<BNO055::SYS_STATUS_T>(readReg(REG_SYS_STATUS));
return bno055_get_system_status(m_bno055);
}
BNO055::SYS_ERR_T BNO055::getSystemError()
BNO055_SYS_ERR_T BNO055::getSystemError()
{
setPage(0);
return static_cast<BNO055::SYS_ERR_T>(readReg(REG_SYS_ERROR));
return bno055_get_system_error(m_bno055);
}
string BNO055::readCalibrationData()
std::vector<uint8_t> BNO055::readCalibrationData()
{
if (!isFullyCalibrated())
{
cerr << __FUNCTION__ << ": Sensor must be fully calibrated first."
<< endl;
return "";
}
uint8_t calibrationData[BNO055_CALIBRATION_DATA_SIZE];
// should be at page 0, but lets make sure
setPage(0);
if (bno055_read_calibration_data(m_bno055, calibrationData,
BNO055_CALIBRATION_DATA_SIZE))
throw std::runtime_error(string(__FUNCTION__)
+ ": bno055_read_calibration_data() failed");
// first we need to go back into config mode
OPERATION_MODES_T currentMode = m_currentMode;
setOperationMode(OPERATION_MODE_CONFIGMODE);
uint8_t calibData[calibrationDataNumBytes];
readRegs(REG_ACC_OFFSET_X_LSB, calibData, calibrationDataNumBytes);
string rv((char *)calibData, calibrationDataNumBytes);
// now reset our operating mode
setOperationMode(currentMode);
return rv;
return vector<uint8_t>(calibrationData,
calibrationData+BNO055_CALIBRATION_DATA_SIZE);
}
void BNO055::writeCalibrationData(string calibData)
void BNO055::writeCalibrationData(vector<uint8_t> calibrationData)
{
if (static_cast<int>(calibData.size()) != calibrationDataNumBytes)
{
throw std::invalid_argument(std::string(__FUNCTION__)
+ ": calibData string must be exactly "
+ std::to_string(calibrationDataNumBytes)
+ " bytes long");
}
// should be at page 0, but lets make sure
setPage(0);
// first we need to go back into config mode
OPERATION_MODES_T currentMode = m_currentMode;
setOperationMode(OPERATION_MODE_CONFIGMODE);
// write the data
writeRegs(REG_ACC_OFFSET_X_LSB, (uint8_t *)calibData.c_str(),
calibData.size());
// now reset our operating mode
setOperationMode(currentMode);
if (bno055_write_calibration_data(m_bno055, calibrationData.data(),
calibrationData.size()))
throw std::runtime_error(string(__FUNCTION__)
+ ": bno055_write_calibration_data() failed");
}
float BNO055::getTemperature(bool fahrenheit)
{
if (fahrenheit)
return c2f(m_temperature);
else
return m_temperature;
}
float temperature = bno055_get_temperature(m_bno055);
void BNO055::clearData()
{
m_magX = m_magY = m_magZ = 0;
m_accX = m_accY = m_accZ = 0;
m_gyrX = m_gyrY = m_gyrZ = 0;
m_eulHeading = m_eulRoll = m_eulPitch = 0;
m_quaW = m_quaX = m_quaY = m_quaZ = 0;
m_liaX = m_liaY = m_liaZ = 0;
m_grvX = m_grvY = m_grvZ = 0;
}
bool BNO055::updateFusionData()
{
// bail if we are in config mode, or aren't in a fusion mode...
if (m_currentMode == OPERATION_MODE_CONFIGMODE ||
m_currentMode < OPERATION_MODE_IMU)
return false;
setPage(0);
// FIXME/MAYBE? - abort early if SYS calibration is == 0?
const int fusionBytes = 26;
uint8_t buf[fusionBytes];
readRegs(REG_EUL_HEADING_LSB, buf, fusionBytes);
m_eulHeading = float(int16_t(buf[0] | (buf[1] << 8)));
m_eulRoll = float(int16_t(buf[2] | (buf[3] << 8)));
m_eulPitch = float(int16_t(buf[4] | (buf[5] << 8)));
m_quaW = float(int16_t(buf[6] | (buf[7] << 8)));
m_quaX = float(int16_t(buf[8] | (buf[9] << 8)));
m_quaY = float(int16_t(buf[10] | (buf[11] << 8)));
m_quaZ = float(int16_t(buf[12] | (buf[13] << 8)));
m_liaX = float(int16_t(buf[14] | (buf[15] << 8)));
m_liaY = float(int16_t(buf[16] | (buf[17] << 8)));
m_liaZ = float(int16_t(buf[18] | (buf[19] << 8)));
m_grvX = float(int16_t(buf[20] | (buf[21] << 8)));
m_grvY = float(int16_t(buf[22] | (buf[23] << 8)));
m_grvZ = float(int16_t(buf[24] | (buf[25] << 8)));
return true;
}
bool BNO055::updateNonFusionData()
{
// bail if we are in config mode...
if (m_currentMode == OPERATION_MODE_CONFIGMODE)
return false;
setPage(0);
const int nonFusionBytes = 18;
uint8_t buf[nonFusionBytes];
readRegs(REG_ACC_DATA_X_LSB, buf, nonFusionBytes);
m_accX = float(int16_t(buf[0] | (buf[1] << 8)));
m_accY = float(int16_t(buf[2] | (buf[3] << 8)));
m_accZ = float(int16_t(buf[4] | (buf[5] << 8)));
m_magX = float(int16_t(buf[6] | (buf[7] << 8)));
m_magY = float(int16_t(buf[8] | (buf[9] << 8)));
m_magZ = float(int16_t(buf[10] | (buf[11] << 8)));
m_gyrX = float(int16_t(buf[12] | (buf[13] << 8)));
m_gyrY = float(int16_t(buf[14] | (buf[15] << 8)));
m_gyrZ = float(int16_t(buf[16] | (buf[17] << 8)));
return true;
if (fahrenheit)
return c2f(temperature);
else
return temperature;
}
void BNO055::getEulerAngles(float *heading, float *roll, float *pitch)
{
if (heading)
*heading = m_eulHeading / m_eulUnitScale;
if (roll)
*roll = m_eulRoll / m_eulUnitScale;
if (pitch)
*pitch = m_eulPitch / m_eulUnitScale;
bno055_get_euler_angles(m_bno055, heading, roll, pitch);
}
float *BNO055::getEulerAngles()
vector<float> BNO055::getEulerAngles()
{
static float v[3];
getEulerAngles(&v[0], &v[1], &v[2]);
return v;
float v[3];
getEulerAngles(&v[0], &v[1], &v[2]);
return vector<float>(v, v+3);
}
void BNO055::getQuaternions(float *w, float *x, float *y, float *z)
{
// from the datasheet
const float scale = float(1.0 / (1 << 14));
if (w)
*w = m_quaW * scale;
if (x)
*x = m_quaX * scale;
if (y)
*y = m_quaY * scale;
if (z)
*z = m_quaZ * scale;
bno055_get_quaternions(m_bno055, w, x, y, z);
}
float *BNO055::getQuaternions()
vector<float> BNO055::getQuaternions()
{
static float v[4];
getQuaternions(&v[0], &v[1], &v[2], &v[3]);
return v;
float v[4];
getQuaternions(&v[0], &v[1], &v[2], &v[3]);
return vector<float>(v, v+4);
}
void BNO055::getLinearAcceleration(float *x, float *y, float *z)
{
if (x)
*x = m_liaX / m_accUnitScale;
if (y)
*y = m_liaY / m_accUnitScale;
if (z)
*z = m_liaZ / m_accUnitScale;
bno055_get_linear_acceleration(m_bno055, x, y, z);
}
float *BNO055::getLinearAcceleration()
vector<float> BNO055::getLinearAcceleration()
{
static float v[3];
getLinearAcceleration(&v[0], &v[1], &v[2]);
return v;
float v[3];
getLinearAcceleration(&v[0], &v[1], &v[2]);
return vector<float>(v, v+3);
}
void BNO055::getGravityVectors(float *x, float *y, float *z)
{
if (x)
*x = m_grvX / m_accUnitScale;
if (y)
*y = m_grvY / m_accUnitScale;
if (z)
*z = m_grvZ / m_accUnitScale;
bno055_get_gravity_vectors(m_bno055, x, y, z);
}
float *BNO055::getGravityVectors()
vector<float> BNO055::getGravityVectors()
{
static float v[3];
getGravityVectors(&v[0], &v[1], &v[2]);
return v;
static float v[3];
getGravityVectors(&v[0], &v[1], &v[2]);
return vector<float>(v, v+3);
}
void BNO055::getAccelerometer(float *x, float *y, float *z)
{
if (x)
*x = m_accX / m_accUnitScale;
if (y)
*y = m_accY / m_accUnitScale;
if (z)
*z = m_accZ / m_accUnitScale;
bno055_get_accelerometer(m_bno055, x, y, z);
}
float *BNO055::getAccelerometer()
vector<float> BNO055::getAccelerometer()
{
static float v[3];
getAccelerometer(&v[0], &v[1], &v[2]);
return v;
static float v[3];
getAccelerometer(&v[0], &v[1], &v[2]);
return vector<float>(v, v+3);
}
void BNO055::getMagnetometer(float *x, float *y, float *z)
{
// from the datasheet - 16 uT's per LSB
const float scale = 16.0;
if (x)
*x = m_magX / scale;
if (y)
*y = m_magY / scale;
if (z)
*z = m_magZ / scale;
bno055_get_magnetometer(m_bno055, x, y, z);
}
float *BNO055::getMagnetometer()
vector<float> BNO055::getMagnetometer()
{
static float v[3];
getMagnetometer(&v[0], &v[1], &v[2]);
return v;
float v[3];
getMagnetometer(&v[0], &v[1], &v[2]);
return vector<float>(v, v+3);
}
void BNO055::getGyroscope(float *x, float *y, float *z)
{
if (x)
*x = m_gyrX / m_gyrUnitScale;
if (y)
*y = m_gyrY / m_gyrUnitScale;
if (z)
*z = m_gyrZ / m_gyrUnitScale;
bno055_get_gyroscope(m_bno055, x, y, z);
}
float *BNO055::getGyroscope()
vector<float> BNO055::getGyroscope()
{
static float v[3];
getGyroscope(&v[0], &v[1], &v[2]);
return v;
float v[3];
getGyroscope(&v[0], &v[1], &v[2]);
return vector<float>(v, v+3);
}
void BNO055::setAccelerationConfig(ACC_RANGE_T range, ACC_BW_T bw,
ACC_PWR_MODE_T pwr)
void BNO055::setAccelerationConfig(BNO055_ACC_RANGE_T range,
BNO055_ACC_BW_T bw,
BNO055_ACC_PWR_MODE_T pwr)
{
setPage(1);
uint8_t reg = ((range << _ACC_CONFIG_ACC_RANGE_SHIFT) |
(bw << _ACC_CONFIG_ACC_BW_SHIFT) |
(pwr << _ACC_CONFIG_ACC_PWR_MODE_SHIFT));
writeReg(REG_ACC_CONFIG, reg);
bno055_set_acceleration_config(m_bno055, range, bw, pwr);
}
void BNO055::setMagnetometerConfig(MAG_ODR_T odr, MAG_OPR_T opr,
MAG_POWER_T pwr)
void BNO055::setMagnetometerConfig(BNO055_MAG_ODR_T odr,
BNO055_MAG_OPR_T opr,
BNO055_MAG_POWER_T pwr)
{
setPage(1);
uint8_t reg = ((odr << _MAG_CONFIG_MAG_ODR_SHIFT) |
(opr << _MAG_CONFIG_MAG_OPR_MODE_SHIFT) |
(pwr << _MAG_CONFIG_MAG_POWER_MODE_SHIFT));
writeReg(REG_MAG_CONFIG, reg);
bno055_set_magnetometer_config(m_bno055, odr, opr, pwr);
}
void BNO055::setGyroscopeConfig(GYR_RANGE_T range, GYR_BW_T bw,
GYR_POWER_MODE_T pwr)
void BNO055::setGyroscopeConfig(BNO055_GYR_RANGE_T range,
BNO055_GYR_BW_T bw,
BNO055_GYR_POWER_MODE_T pwr)
{
setPage(1);
uint8_t reg = ((range << _GYR_CONFIG0_GYR_RANGE_SHIFT) |
(bw << _GYR_CONFIG0_GYR_BW_SHIFT));
writeReg(REG_GYR_CONFIG0, reg);
reg = (pwr << _GYR_CONFIG1_GYR_POWER_MODE_SHIFT);
writeReg(REG_GYR_CONFIG1, reg);
bno055_set_gyroscope_config(m_bno055, range, bw, pwr);
}
#if defined(SWIGJAVA) || (JAVACALLBACK)
void BNO055::installISR(int gpio, mraa::Edge level,
jobject runnable)
{
// delete any existing ISR and GPIO context
uninstallISR();
// create gpio context
m_gpioIntr = new mraa::Gpio(gpio);
m_gpioIntr->dir(mraa::DIR_IN);
m_gpioIntr->isr(level, runnable);
}
#else
void BNO055::installISR(int gpio, mraa::Edge level,
void BNO055::installISR(int gpio, mraa_gpio_edge_t level,
void (*isr)(void *), void *arg)
{
// delete any existing ISR and GPIO context
uninstallISR();
// create gpio context
m_gpioIntr = new mraa::Gpio(gpio);
m_gpioIntr->dir(mraa::DIR_IN);
m_gpioIntr->isr(level, isr, arg);
if (bno055_install_isr(m_bno055, gpio, level, isr, arg))
throw std::runtime_error(string(__FUNCTION__)
+ ": bno055_install_isr() failed");
}
#endif
void BNO055::uninstallISR()
{
if (m_gpioIntr)
{
m_gpioIntr->isrExit();
delete m_gpioIntr;
m_gpioIntr = 0;
}
bno055_uninstall_isr(m_bno055);
}