diff --git a/src/bma250e/bma250e.cxx b/src/bma250e/bma250e.cxx index 4aa51e5e..6aaa95ad 100644 --- a/src/bma250e/bma250e.cxx +++ b/src/bma250e/bma250e.cxx @@ -173,10 +173,9 @@ BMA250E::BMA250E(std::string initStr) : mraaIo(initStr) writeReg(reg, val); } if(tok.substr(0,9) == "setRange:") { - BMA250E_RANGE_T scale = (BMA250E_RANGE_T)std::stoi(tok.substr(22),nullptr,0); + BMA250E_RANGE_T scale = (BMA250E_RANGE_T)std::stoi(tok.substr(9),nullptr,0); setRange(scale); } - if(tok.substr(0,13) == "setBandwidth:") { BMA250E_BW_T bw = (BMA250E_BW_T)std::stoi(tok.substr(13),nullptr,0); setBandwidth(bw); diff --git a/src/bmg160/bmg160.cxx b/src/bmg160/bmg160.cxx index b281bece..af1be5fd 100644 --- a/src/bmg160/bmg160.cxx +++ b/src/bmg160/bmg160.cxx @@ -29,6 +29,7 @@ #include #include +#include "upm_string_parser.hpp" #include "bmg160.hpp" using namespace upm; @@ -49,6 +50,163 @@ BMG160::BMG160(int bus, int addr, int cs) : + ": bmg160_init() failed"); } +BMG160::BMG160(std::string initStr) : mraaIo(initStr) +{ + mraa_io_descriptor* descs = mraaIo.getMraaDescriptors(); + std::vector upmTokens; + + if(!mraaIo.getLeftoverStr().empty()) { + upmTokens = UpmStringParser::parse(mraaIo.getLeftoverStr()); + } + + m_bmg160 = (bmg160_context)malloc(sizeof(struct _bmg160_context)); + if(!m_bmg160) { + throw std::runtime_error(std::string(__FUNCTION__) + + ": bmg160_init() failed"); + } + + // zero out context + memset((void *)m_bmg160, 0, sizeof(struct _bmg160_context)); + + // make sure MRAA is initialized + int mraa_rv; + if ((mraa_rv = mraa_init()) != MRAA_SUCCESS) + { + bmg160_close(m_bmg160); + throw std::runtime_error(std::string(__FUNCTION__) + + ": mraa_init() failed"); + } + + if(descs->spis) { + m_bmg160->isSPI = true; + if( !(m_bmg160->spi = descs->spis[0]) ) { + bmg160_close(m_bmg160); + throw std::runtime_error(std::string(__FUNCTION__) + + ": mraa_spi_init() failed"); + } + if(descs->gpios) { + if( !(m_bmg160->gpioCS = descs->gpios[0]) ) { + bmg160_close(m_bmg160); + throw std::runtime_error(std::string(__FUNCTION__) + + ": mraa_gpio_init() failed"); + } + mraa_gpio_dir(m_bmg160->gpioCS, MRAA_GPIO_OUT); + } else { + throw std::runtime_error(std::string(__FUNCTION__) + + ": mraa_gpio_init() failed"); + } + mraa_spi_mode(m_bmg160->spi, MRAA_SPI_MODE0); + if (mraa_spi_frequency(m_bmg160->spi, 5000000)) { + bmg160_close(m_bmg160); + throw std::runtime_error(std::string(__FUNCTION__) + + ": mraa_spi_frequency() failed"); + } + } else { + // init the i2c context + if(!descs->i2cs) { + throw std::runtime_error(std::string(__FUNCTION__) + + ": mraa_i2c_init() failed"); + } else { + if( !(m_bmg160->i2c = descs->i2cs[0]) ) + { + bmg160_close(m_bmg160); + throw std::runtime_error(std::string(__FUNCTION__) + + ": mraa_i2c_init() failed"); + } + } + } + + // check the chip id + uint8_t chipID = bmg160_get_chip_id(m_bmg160); + if (chipID != BMG160_CHIPID) + { + printf("%s: invalid chip id: %02x. Expected %02x\n", + __FUNCTION__, chipID, BMG160_CHIPID); + bmg160_close(m_bmg160); + throw std::runtime_error(std::string(__FUNCTION__) + + ": bmg160_init() failed"); + } + + // call devinit with default options + if (bmg160_devinit(m_bmg160, BMG160_POWER_MODE_NORMAL, BMG160_RANGE_250, + BMG160_BW_400_47)) + { + printf("%s: bmg160_devinit() failed.\n", __FUNCTION__); + bmg160_close(m_bmg160); + throw std::runtime_error(std::string(__FUNCTION__) + + ": bma250e_init() failed"); + + } + + std::string::size_type sz; + for(std::string tok:upmTokens) { + if(tok.substr(0,11) == "enableFIFO:") { + bool useFIFO = std::stoi(tok.substr(11),&sz,0); + enableFIFO(useFIFO); + } + if(tok.substr(0,9) == "writeReg:") { + uint8_t reg = std::stoi(tok.substr(9),&sz,0); + tok = tok.substr(9); + uint8_t val = std::stoi(tok.substr(sz+1),nullptr,0); + writeReg(reg, val); + } + if(tok.substr(0,9) == "setRange:") { + BMG160_RANGE_T scale = (BMG160_RANGE_T)std::stoi(tok.substr(22),nullptr,0); + setRange(scale); + } + if(tok.substr(0,13) == "setBandwidth:") { + BMG160_BW_T bw = (BMG160_BW_T)std::stoi(tok.substr(13),nullptr,0); + setBandwidth(bw); + } + if(tok.substr(0,13) == "setPowerMode:") { + BMG160_POWER_MODE_T power = (BMG160_POWER_MODE_T)std::stoi(tok.substr(13),nullptr,0); + setPowerMode(power); + } + if(tok.substr(0,17) == "fifoSetWatermark:") { + BMG160_RANGE_T wm = (BMG160_RANGE_T)std::stoi(tok.substr(17),nullptr,0); + fifoSetWatermark(wm); + } + if(tok.substr(0,11) == "fifoConfig:") { + BMG160_FIFO_MODE_T mode = (BMG160_FIFO_MODE_T)std::stoi(tok.substr(11),&sz,0); + tok = tok.substr(11); + BMG160_FIFO_DATA_SEL_T axes = (BMG160_FIFO_DATA_SEL_T)std::stoi(tok.substr(sz+1),nullptr,0); + fifoConfig(mode, axes); + } + if(tok.substr(0,20) == "setInterruptEnable0:") { + u_int8_t bits = (u_int8_t)std::stoi(tok.substr(20),nullptr,0); + setInterruptEnable0(bits); + } + if(tok.substr(0,17) == "setInterruptMap0:") { + u_int8_t bits = (u_int8_t)std::stoi(tok.substr(17),nullptr,0); + setInterruptMap0(bits); + } + if(tok.substr(0,17) == "setInterruptMap1:") { + u_int8_t bits = (u_int8_t)std::stoi(tok.substr(17),nullptr,0); + setInterruptMap1(bits); + } + if(tok.substr(0,16) == "setInterruptSrc:") { + u_int8_t bits = (u_int8_t)std::stoi(tok.substr(16),nullptr,0); + setInterruptSrc(bits); + } + if(tok.substr(0,26) == "setInterruptOutputControl:") { + u_int8_t bits = (u_int8_t)std::stoi(tok.substr(26),nullptr,0); + setInterruptOutputControl(bits); + } + if(tok.substr(0,26) == "setInterruptLatchBehavior:") { + BMG160_RST_LATCH_T latch = (BMG160_RST_LATCH_T)std::stoi(tok.substr(26),nullptr,0); + setInterruptLatchBehavior(latch); + } + if(tok.substr(0,24) == "enableRegisterShadowing:") { + bool shadow = std::stoi(tok.substr(24),nullptr,0); + enableRegisterShadowing(shadow); + } + if(tok.substr(0,22) == "enableOutputFiltering:") { + bool filter = std::stoi(tok.substr(22),nullptr,0); + enableOutputFiltering(filter); + } + } +} + BMG160::~BMG160() { bmg160_close(m_bmg160); diff --git a/src/bmg160/bmg160.hpp b/src/bmg160/bmg160.hpp index ac799e57..db855bbd 100644 --- a/src/bmg160/bmg160.hpp +++ b/src/bmg160/bmg160.hpp @@ -29,6 +29,7 @@ #include #include +#include #include "bmg160.h" #include @@ -94,6 +95,13 @@ namespace upm { BMG160(int bus=BMG160_DEFAULT_I2C_BUS, int addr=BMG160_DEFAULT_ADDR, int cs=-1); + /** + * Instantiates BMG160 3-axis Gyroscope based on a given string. + * + * @param initStr string containing specific information for BMG160 initialization. + */ + BMG160(std::string initStr); + /** * BMG160 Destructor. */ @@ -472,6 +480,7 @@ namespace upm { protected: bmg160_context m_bmg160; + mraa::MraaIo mraaIo; private: /* Disable implicit copy and assignment operators */