From 90167f6ac1b5adeab16f466e4d1516ca352ee3a0 Mon Sep 17 00:00:00 2001 From: Adelin Dobre Date: Wed, 1 Aug 2018 17:37:23 +0300 Subject: [PATCH] BNO055 9DOF: Add string based cons for Fusion Hub Signed-off-by: Adelin Dobre Signed-off-by: Mihai Tudor Panu --- src/bno055/bno055.c | 41 ++++++++++++---- src/bno055/bno055.cxx | 108 +++++++++++++++++++++++++++++++++++++++++- src/bno055/bno055.h | 3 +- src/bno055/bno055.hpp | 10 ++++ 4 files changed, 150 insertions(+), 12 deletions(-) diff --git a/src/bno055/bno055.c b/src/bno055/bno055.c index 015877dd..19c09349 100644 --- a/src/bno055/bno055.c +++ b/src/bno055/bno055.c @@ -126,7 +126,7 @@ static upm_result_t _update_non_fusion_data(const bno055_context dev) } // init -bno055_context bno055_init(int bus, uint8_t addr) +bno055_context bno055_init(int bus, uint8_t addr, mraa_io_descriptor* descs) { bno055_context dev = (bno055_context)malloc(sizeof(struct _bno055_context)); @@ -146,18 +146,39 @@ bno055_context bno055_init(int bus, uint8_t addr) return NULL; } - if (!(dev->i2c = mraa_i2c_init(bus))) + if(descs) { - printf("%s: mraa_i2c_init() failed.\n", __FUNCTION__); - bno055_close(dev); - return NULL; + if(!descs->i2cs) + { + bno055_close(dev); + printf("%s: mraa_i2c_init() failed.\n", __FUNCTION__); + return NULL; + } + else + { + if( !(dev->i2c = descs->i2cs[0]) ) + { + bno055_close(dev); + printf("%s: mraa_i2c_init() failed.\n", __FUNCTION__); + return NULL; + } + } } - - if (mraa_i2c_address(dev->i2c, addr) != MRAA_SUCCESS) + else { - printf("%s: mraa_i2c_address() failed.\n", __FUNCTION__); - 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); diff --git a/src/bno055/bno055.cxx b/src/bno055/bno055.cxx index 510067a2..1915f710 100644 --- a/src/bno055/bno055.cxx +++ b/src/bno055/bno055.cxx @@ -28,6 +28,7 @@ #include #include "bno055.hpp" +#include "upm_string_parser.hpp" using namespace upm; using namespace std; @@ -40,13 +41,118 @@ static float c2f(float c) } BNO055::BNO055(int bus, uint8_t addr) : - m_bno055(bno055_init(bus, addr)) + m_bno055(bno055_init(bus, addr, nullptr)) { if (!m_bno055) throw std::runtime_error(string(__FUNCTION__) + ": bno055_init() failed"); } +BNO055::BNO055(std::string initStr) : mraaIo(initStr) +{ + mraa_io_descriptor* descs = mraaIo.getMraaDescriptors(); + std::vector upmTokens; + + if (mraaIo.getLeftoverStr() != "") { + upmTokens = UpmStringParser::parse(mraaIo.getLeftoverStr()); + } + + m_bno055 = bno055_init(0, 0, descs); + + if(!m_bno055) + throw std::runtime_error(string(__FUNCTION__) + + ": bno055_init() failed"); + + std::string::size_type sz, prev_sz; + for(std::string tok : upmTokens) + { + if(tok.substr(0, 9) == "writeReg:") + { + uint8_t reg = std::stoul(tok.substr(9), &sz, 0); + tok = tok.substr(9); + uint8_t val = std::stoul(tok.substr(sz + 1), nullptr, 0); + writeReg(reg, val); + } + if(tok.substr(0, 8) == "setPage:") + { + uint8_t page = std::stoul(tok.substr(8), &sz, 0); + tok = tok.substr(8); + bool force = std::stoi(tok.substr(sz + 1), nullptr, 0); + setPage(page, force); + } + if(tok.substr(0, 17) == "setClockExternal:") + { + bool extClock = std::stoi(tok.substr(17), nullptr, 0); + setClockExternal(extClock); + } + if(tok.substr(0, 21) == "setTemperatureSource:") + { + BNO055_TEMP_SOURCES_T src = (BNO055_TEMP_SOURCES_T)std::stoi(tok.substr(21), nullptr, 0); + setTemperatureSource(src); + } + if(tok.substr(0, 22) == "setAccelerometerUnits:") + { + bool mg = std::stoi(tok.substr(22), nullptr, 0); + setAccelerometerUnits(mg); + } + if(tok.substr(0, 18) == "setGyroscopeUnits:") + { + bool radians = std::stoi(tok.substr(18), nullptr, 0); + setGyroscopeUnits(radians); + } + if(tok.substr(0, 14) == "setEulerUnits:") + { + bool radians = std::stoi(tok.substr(14), nullptr, 0); + setEulerUnits(radians); + } + if(tok.substr(0, 17) == "setOperationMode:") + { + BNO055_OPERATION_MODES_T mode = (BNO055_OPERATION_MODES_T)std::stoi(tok.substr(17), nullptr, 0); + setOperationMode(mode); + } + if(tok.substr(0, 19) == "setInterruptEnable:") + { + uint8_t enables = std::stoul(tok.substr(19), nullptr, 0); + setInterruptEnable(enables); + } + if(tok.substr(0, 17) == "setInterruptMask:") + { + uint8_t enables = std::stoul(tok.substr(17), nullptr, 0); + setInterruptMask(enables); + } + if(tok.substr(0, 22) == "setAccelerationConfig:") + { + BNO055_ACC_RANGE_T range = (BNO055_ACC_RANGE_T)std::stoi(tok.substr(22), &sz, 0); + tok = tok.substr(22); + prev_sz = sz; + BNO055_ACC_BW_T bw = (BNO055_ACC_BW_T)std::stoi(tok.substr(prev_sz + 1), &sz, 0); + tok = tok.substr(prev_sz + 1); + BNO055_ACC_PWR_MODE_T pwr = (BNO055_ACC_PWR_MODE_T)std::stoi(tok.substr(sz + 1), nullptr, 0); + setAccelerationConfig(range, bw, pwr); + } + if(tok.substr(0, 22) == "setMagnetometerConfig:") + { + BNO055_MAG_ODR_T odr = (BNO055_MAG_ODR_T)std::stoi(tok.substr(22), &sz, 0); + tok = tok.substr(22); + prev_sz = sz; + BNO055_MAG_OPR_T opr = (BNO055_MAG_OPR_T)std::stoi(tok.substr(prev_sz + 1), &sz, 0); + tok = tok.substr(prev_sz + 1); + BNO055_MAG_POWER_T pwr = (BNO055_MAG_POWER_T)std::stoi(tok.substr(sz + 1), nullptr, 0); + setMagnetometerConfig(odr, opr, pwr); + } + if(tok.substr(0, 19) == "setGyroscopeConfig:") + { + BNO055_GYR_RANGE_T range = (BNO055_GYR_RANGE_T)std::stoi(tok.substr(19), &sz, 0); + tok = tok.substr(22); + prev_sz = sz; + BNO055_GYR_BW_T bw = (BNO055_GYR_BW_T)std::stoi(tok.substr(prev_sz + 1), &sz, 0); + tok = tok.substr(prev_sz + 1); + BNO055_GYR_POWER_MODE_T pwr = (BNO055_GYR_POWER_MODE_T)std::stoi(tok.substr(sz + 1), nullptr, 0); + setGyroscopeConfig(range, bw, pwr); + } + } +} + BNO055::~BNO055() { bno055_close(m_bno055); diff --git a/src/bno055/bno055.h b/src/bno055/bno055.h index 38244a57..cfbed7ab 100644 --- a/src/bno055/bno055.h +++ b/src/bno055/bno055.h @@ -32,6 +32,7 @@ #include #include +#include #include "bno055_regs.h" @@ -124,7 +125,7 @@ extern "C" { * @param address The I2C address for this device. * @return The device context, or NULL if an error occurred. */ - bno055_context bno055_init(int bus, uint8_t addr); + bno055_context bno055_init(int bus, uint8_t addr, mraa_io_descriptor* descs); /** * BNO055 close function. diff --git a/src/bno055/bno055.hpp b/src/bno055/bno055.hpp index 2c75c5e8..21ca4bd3 100644 --- a/src/bno055/bno055.hpp +++ b/src/bno055/bno055.hpp @@ -27,6 +27,8 @@ #include #include "bno055.h" +#include "mraa/initio.hpp" + #include #include @@ -126,6 +128,13 @@ namespace upm { BNO055(int bus=BNO055_DEFAULT_I2C_BUS, uint8_t addr=BNO055_DEFAULT_ADDR); + /** + * Instantiates BNO055 9DOF Fusion Hub based on a given string. + * + * @param initStr string containing specific information for BNO055 initialization. + */ + BNO055(std::string initStr); + /** * BNO055 Destructor. */ @@ -655,6 +664,7 @@ namespace upm { protected: bno055_context m_bno055; + mraa::MraaIo mraaIo; /** * Set the current internal device register page. This is a low