| /* |
| * 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 celcius 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 celcius) |
| { |
| setPage(0); |
| |
| uint8_t reg = readReg(REG_UNIT_SEL); |
| |
| if (celcius) |
| reg &= ~UNIT_SEL_TEMP_UNIT; |
| else |
| reg |= UNIT_SEL_TEMP_UNIT; |
| |
| writeReg(REG_UNIT_SEL, reg); |
| |
| m_tempIsC = celcius; |
| } |
| |
| 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; |
| } |
| } |