BMG160: Add string based cons for 3-axis Gyroscope

Signed-off-by: Adelin Dobre <adelin.dobre@rinftech.com>

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);

src/bmg160/bmg160.cxx

@@ -29,6 +29,7 @@
 #include <stdexcept>
 #include <string>
 
+#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<std::string> 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);

src/bmg160/bmg160.hpp

@@ -29,6 +29,7 @@
 #include <vector>
 
 #include <mraa/gpio.hpp>
+#include <mraa/initio.hpp>
 #include "bmg160.h"
 
 namespace upm {
@@ -92,6 +93,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.
          */
@@ -470,6 +478,7 @@ namespace upm {
 
     protected:
         bmg160_context m_bmg160;
+        mraa::MraaIo mraaIo;
 
     private:
         /* Disable implicit copy and assignment operators */