mirror of
https://github.com/eclipse/upm.git
synced 2025-03-25 17:59:59 +03:00
810 lines
16 KiB
C++
810 lines
16 KiB
C++
/*
|
|
* Author: Jon Trulson <jtrulson@ics.com>
|
|
* Copyright (c) 2016 Intel Corporation.
|
|
*
|
|
* 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 <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));
|
|
}
|
|
|
|
static float c2f(float c)
|
|
{
|
|
return (c * (9.0 / 5.0) + 32.0);
|
|
}
|
|
|
|
BNO055::BNO055(int bus, uint8_t addr) :
|
|
m_i2c(bus), m_gpioIntr(0)
|
|
{
|
|
|
|
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);
|
|
}
|
|
|
|
BNO055::~BNO055()
|
|
{
|
|
uninstallISR();
|
|
}
|
|
|
|
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();
|
|
}
|
|
|
|
uint8_t BNO055::readReg(uint8_t reg)
|
|
{
|
|
return m_i2c.readReg(reg);
|
|
}
|
|
|
|
void BNO055::readRegs(uint8_t reg, uint8_t *buffer, int len)
|
|
{
|
|
m_i2c.readBytesReg(reg, buffer, len);
|
|
}
|
|
|
|
bool 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;
|
|
}
|
|
|
|
bool 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;
|
|
}
|
|
|
|
uint8_t BNO055::getChipID()
|
|
{
|
|
setPage(0);
|
|
return readReg(REG_CHIP_ID);
|
|
}
|
|
|
|
uint8_t BNO055::getACCID()
|
|
{
|
|
setPage(0);
|
|
return readReg(REG_ACC_ID);
|
|
}
|
|
|
|
uint8_t BNO055::getMAGID()
|
|
{
|
|
setPage(0);
|
|
return readReg(REG_MAG_ID);
|
|
}
|
|
|
|
uint8_t BNO055::getGYRID()
|
|
{
|
|
setPage(0);
|
|
return readReg(REG_GYR_ID);
|
|
}
|
|
|
|
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;
|
|
}
|
|
|
|
uint8_t BNO055::getBootLoaderID()
|
|
{
|
|
setPage(0);
|
|
return readReg(REG_BL_REV_ID);
|
|
}
|
|
|
|
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;
|
|
}
|
|
|
|
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);
|
|
}
|
|
|
|
void BNO055::setTemperatureSource(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;
|
|
}
|
|
|
|
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);
|
|
}
|
|
|
|
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);
|
|
}
|
|
|
|
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);
|
|
}
|
|
|
|
void BNO055::setOperationMode(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);
|
|
}
|
|
|
|
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;
|
|
}
|
|
|
|
int *BNO055::getCalibrationStatus()
|
|
{
|
|
static int v[4]; // mag, acc, gyr, sys;
|
|
|
|
getCalibrationStatus(&v[0], &v[1], &v[2], &v[3]);
|
|
return v;
|
|
}
|
|
|
|
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;
|
|
}
|
|
|
|
void BNO055::resetSystem()
|
|
{
|
|
setPage(0);
|
|
|
|
uint8_t reg = readReg(REG_SYS_TRIGGER);
|
|
|
|
reg |= SYS_TRIGGER_RST_SYS;
|
|
|
|
writeReg(REG_SYS_TRIGGER, reg);
|
|
sleep(1);
|
|
}
|
|
|
|
void BNO055::resetInterruptStatus()
|
|
{
|
|
setPage(0);
|
|
|
|
uint8_t reg = readReg(REG_SYS_TRIGGER);
|
|
|
|
reg |= SYS_TRIGGER_RST_INT;
|
|
|
|
writeReg(REG_SYS_TRIGGER, reg);
|
|
}
|
|
|
|
uint8_t BNO055::getInterruptStatus()
|
|
{
|
|
setPage(0);
|
|
|
|
return readReg(REG_INT_STA);
|
|
}
|
|
|
|
uint8_t BNO055::getInterruptEnable()
|
|
{
|
|
setPage(1);
|
|
|
|
return readReg(REG_INT_EN);
|
|
}
|
|
|
|
void BNO055::setInterruptEnable(uint8_t enables)
|
|
{
|
|
setPage(1);
|
|
|
|
writeReg(REG_INT_EN, enables);
|
|
}
|
|
|
|
uint8_t BNO055::getInterruptMask()
|
|
{
|
|
setPage(1);
|
|
|
|
return readReg(REG_INT_MSK);
|
|
}
|
|
|
|
void BNO055::setInterruptMask(uint8_t mask)
|
|
{
|
|
setPage(1);
|
|
|
|
writeReg(REG_INT_MSK, mask);
|
|
}
|
|
|
|
BNO055::SYS_STATUS_T BNO055::getSystemStatus()
|
|
{
|
|
setPage(0);
|
|
|
|
return static_cast<BNO055::SYS_STATUS_T>(readReg(REG_SYS_STATUS));
|
|
}
|
|
|
|
BNO055::SYS_ERR_T BNO055::getSystemError()
|
|
{
|
|
setPage(0);
|
|
|
|
return static_cast<BNO055::SYS_ERR_T>(readReg(REG_SYS_ERROR));
|
|
}
|
|
|
|
string BNO055::readCalibrationData()
|
|
{
|
|
if (!isFullyCalibrated())
|
|
{
|
|
cerr << __FUNCTION__ << ": Sensor must be fully calibrated first."
|
|
<< endl;
|
|
return "";
|
|
}
|
|
|
|
// 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);
|
|
|
|
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;
|
|
}
|
|
|
|
void BNO055::writeCalibrationData(string calibData)
|
|
{
|
|
if (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);
|
|
}
|
|
|
|
float BNO055::getTemperature(bool fahrenheit)
|
|
{
|
|
if (fahrenheit)
|
|
return c2f(m_temperature);
|
|
else
|
|
return m_temperature;
|
|
}
|
|
|
|
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;
|
|
}
|
|
|
|
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;
|
|
}
|
|
|
|
float *BNO055::getEulerAngles()
|
|
{
|
|
static float v[3];
|
|
getEulerAngles(&v[0], &v[1], &v[2]);
|
|
return v;
|
|
}
|
|
|
|
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;
|
|
}
|
|
|
|
float *BNO055::getQuaternions()
|
|
{
|
|
static float v[4];
|
|
getQuaternions(&v[0], &v[1], &v[2], &v[3]);
|
|
return v;
|
|
}
|
|
|
|
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;
|
|
}
|
|
|
|
float *BNO055::getLinearAcceleration()
|
|
{
|
|
static float v[3];
|
|
getLinearAcceleration(&v[0], &v[1], &v[2]);
|
|
return v;
|
|
}
|
|
|
|
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;
|
|
}
|
|
|
|
float *BNO055::getGravityVectors()
|
|
{
|
|
static float v[3];
|
|
getGravityVectors(&v[0], &v[1], &v[2]);
|
|
return v;
|
|
}
|
|
|
|
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;
|
|
}
|
|
|
|
float *BNO055::getAccelerometer()
|
|
{
|
|
static float v[3];
|
|
getAccelerometer(&v[0], &v[1], &v[2]);
|
|
return v;
|
|
}
|
|
|
|
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;
|
|
}
|
|
|
|
float *BNO055::getMagnetometer()
|
|
{
|
|
static float v[3];
|
|
getMagnetometer(&v[0], &v[1], &v[2]);
|
|
return v;
|
|
}
|
|
|
|
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;
|
|
}
|
|
|
|
float *BNO055::getGyroscope()
|
|
{
|
|
static float v[3];
|
|
getGyroscope(&v[0], &v[1], &v[2]);
|
|
return v;
|
|
}
|
|
|
|
void BNO055::setAccelerationConfig(ACC_RANGE_T range, ACC_BW_T bw,
|
|
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);
|
|
}
|
|
|
|
void BNO055::setMagnetometerConfig(MAG_ODR_T odr, MAG_OPR_T opr,
|
|
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);
|
|
}
|
|
|
|
void BNO055::setGyroscopeConfig(GYR_RANGE_T range, GYR_BW_T bw,
|
|
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);
|
|
}
|
|
|
|
#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 (*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);
|
|
}
|
|
#endif
|
|
|
|
void BNO055::uninstallISR()
|
|
{
|
|
if (m_gpioIntr)
|
|
{
|
|
m_gpioIntr->isrExit();
|
|
delete m_gpioIntr;
|
|
|
|
m_gpioIntr = 0;
|
|
}
|
|
}
|