diff --git a/src/bmx055/bmc150.cxx b/src/bmx055/bmc150.cxx index 8b8af2e6..f271cc09 100644 --- a/src/bmx055/bmc150.cxx +++ b/src/bmx055/bmc150.cxx @@ -29,6 +29,7 @@ #include #include +#include "upm_string_parser.hpp" #include "bmc150.hpp" using namespace upm; @@ -55,6 +56,71 @@ BMC150::BMC150(int accelBus, int accelAddr, int accelCS, m_mag->init(); } +BMC150::BMC150(std::string initStr) +{ + int accelBus = BMC150_DEFAULT_BUS; + int accelAddr = BMC150_DEFAULT_ACC_ADDR; + int accelCS = -1; + int magBus = BMC150_DEFAULT_BUS; + int magAddr = BMC150_DEFAULT_MAG_ADDR; + int magCS = -1; + + std::vector upmTokens; + upmTokens = UpmStringParser::parse(initStr); + + for(std::string tok : upmTokens) + { + if(tok.substr(0, 9) == "accelBus:") + accelBus = std::stoi(tok.substr(9), nullptr, 0); + if(tok.substr(0, 10) == "accelAddr:") + accelAddr = std::stoi(tok.substr(10), nullptr, 0); + if(tok.substr(0, 8) == "accelCS:") + accelCS = std::stoi(tok.substr(8), nullptr, 0); + if(tok.substr(0, 7) == "magBus:") + magBus = std::stoi(tok.substr(7), nullptr, 0); + if(tok.substr(0, 8) == "magAddr:") + magAddr = std::stoi(tok.substr(8), nullptr, 0); + if(tok.substr(0, 6) == "magCS:") + magCS = std::stoi(tok.substr(6), nullptr, 0); + } + + // if -1 is supplied as a bus for any of these, we will not + // instantiate them + + if (accelBus >= 0) + m_accel = new BMA250E(accelBus, accelAddr, accelCS); + + if (magBus >= 0) + m_mag = new BMM150(magBus, magAddr, magCS); + + // now initialize them... + if (m_accel) + m_accel->init(); + + if (m_mag) + m_mag->init(); + + std::string::size_type sz, prev_sz; + for(std::string tok : upmTokens) + { + if(tok.substr(0, 18) == "initAccelerometer:") + { + BMA250E_POWER_MODE_T pwr = (BMA250E_POWER_MODE_T)std::stoi(tok.substr(18), &sz, 0); + tok = tok.substr(18); + prev_sz = sz; + BMA250E_RANGE_T range = (BMA250E_RANGE_T)std::stoi(tok.substr(prev_sz + 1), &sz, 0); + tok = tok.substr(prev_sz + 1); + BMA250E_BW_T bw = (BMA250E_BW_T)std::stoi(tok.substr(sz + 1), nullptr, 0); + initAccelerometer(pwr, range, bw); + } + if(tok.substr(0, 17) == "initMagnetometer:") + { + BMM150_USAGE_PRESETS_T usage = (BMM150_USAGE_PRESETS_T)std::stoi(tok.substr(17), nullptr, 0); + initMagnetometer(usage); + } + } +} + BMC150::~BMC150() { if (m_accel) diff --git a/src/bmx055/bmc150.hpp b/src/bmx055/bmc150.hpp index 1f9233d0..906a32b0 100644 --- a/src/bmx055/bmc150.hpp +++ b/src/bmx055/bmc150.hpp @@ -100,6 +100,13 @@ namespace upm { int magAddr=BMC150_DEFAULT_MAG_ADDR, int magCS=-1); + /** + * Instantiates bmc150 6-axis Ecompass based on a given string. + * + * @param initStr string containing specific information for bmc150 initialization. + */ + BMC150(std::string initStr); + /** * BMC150 Destructor. */ diff --git a/src/bmx055/bmi055.cxx b/src/bmx055/bmi055.cxx index f8edd973..cba3bebf 100644 --- a/src/bmx055/bmi055.cxx +++ b/src/bmx055/bmi055.cxx @@ -30,6 +30,7 @@ #include #include "bmi055.hpp" +#include "upm_string_parser.hpp" using namespace upm; using namespace std; @@ -55,6 +56,77 @@ BMI055::BMI055(int accelBus, int accelAddr, int accelCS, m_gyro->init(); } +BMI055::BMI055(std::string initStr) : m_accel(0), m_gyro(0) +{ + int accelBus = BMA250E_DEFAULT_I2C_BUS; + int accelAddr = BMA250E_DEFAULT_ADDR; + int accelCS = -1; + int gyroBus = BMG160_DEFAULT_I2C_BUS; + int gyroAddr = BMG160_DEFAULT_ADDR; + int gyroCS = -1; + + std::vector upmTokens; + upmTokens = UpmStringParser::parse(initStr); + + for(std::string tok : upmTokens) + { + if(tok.substr(0, 9) == "accelBus:") + accelBus = std::stoi(tok.substr(9), nullptr, 0); + if(tok.substr(0, 10) == "accelAddr:") + accelAddr = std::stoi(tok.substr(10), nullptr, 0); + if(tok.substr(0, 8) == "accelCS:") + accelCS = std::stoi(tok.substr(8), nullptr, 0); + if(tok.substr(0, 8) == "gyroBus:") + gyroBus = std::stoi(tok.substr(8), nullptr, 0); + if(tok.substr(0, 9) == "gyroAddr:") + gyroAddr = std::stoi(tok.substr(9), nullptr, 0); + if(tok.substr(0, 7) == "gyroCS:") + gyroCS = std::stoi(tok.substr(7), nullptr, 0); + } + + // if -1 is supplied as a bus for any of these, we will not + // instantiate them + + if (accelBus >= 0) + m_accel = new BMA250E(accelBus, accelAddr, accelCS); + + if (gyroBus >= 0) + m_gyro = new BMG160(gyroBus, gyroAddr, gyroCS); + + // now initialize them... + if (m_accel) + m_accel->init(); + + if (m_gyro) + m_gyro->init(); + + std::string::size_type sz, prev_sz; + for(std::string tok : upmTokens) + { + if(tok.substr(0, 18) == "initAccelerometer:") + { + BMA250E_POWER_MODE_T pwr = (BMA250E_POWER_MODE_T)std::stoi(tok.substr(18), &sz, 0); + tok = tok.substr(18); + prev_sz = sz; + BMA250E_RANGE_T range = (BMA250E_RANGE_T)std::stoi(tok.substr(prev_sz + 1), &sz, 0); + tok = tok.substr(prev_sz + 1); + BMA250E_BW_T bw = (BMA250E_BW_T)std::stoi(tok.substr(sz + 1), nullptr, 0); + initAccelerometer(pwr, range, bw); + } + if(tok.substr(0, 14) == "initGyroscope:") + { + BMG160_POWER_MODE_T pwr = (BMG160_POWER_MODE_T)std::stoi(tok.substr(14), &sz, 0); + tok = tok.substr(14); + prev_sz = sz; + BMG160_RANGE_T range = (BMG160_RANGE_T)std::stoi(tok.substr(prev_sz + 1), &sz, 0); + tok = tok.substr(prev_sz + 1); + BMG160_BW_T bw = (BMG160_BW_T)std::stoi(tok.substr(sz + 1), nullptr, 0); + initGyroscope(pwr, range, bw); + } + } + +} + BMI055::~BMI055() { if (m_accel) diff --git a/src/bmx055/bmi055.hpp b/src/bmx055/bmi055.hpp index e2c50952..dbfc8081 100644 --- a/src/bmx055/bmi055.hpp +++ b/src/bmx055/bmi055.hpp @@ -94,6 +94,13 @@ namespace upm { int gyroAddr=BMG160_DEFAULT_ADDR, int gyroCS=-1); + /** + * Instantiates bmi055 based on a given string. + * + * @param initStr string containing specific information for bmi055 initialization. + */ + BMI055(std::string initStr); + /** * BMI055 Destructor. */ diff --git a/src/bmx055/bmx055.cxx b/src/bmx055/bmx055.cxx index 4fbc5c70..947dcac4 100644 --- a/src/bmx055/bmx055.cxx +++ b/src/bmx055/bmx055.cxx @@ -30,6 +30,7 @@ #include #include "bmx055.hpp" +#include "upm_string_parser.hpp" using namespace upm; using namespace std; @@ -52,6 +53,88 @@ BMX055::BMX055(int accelBus, int accelAddr, int accelCS, m_mag = new BMM150(magBus, magAddr, magCS); } +BMX055::BMX055(std::string initStr) :m_accel(0), m_gyro(0), m_mag(0) +{ + int accelBus = BMA250E_DEFAULT_I2C_BUS; + int accelAddr = BMA250E_DEFAULT_ADDR; + int accelCS = -1; + + int gyroBus = BMG160_DEFAULT_I2C_BUS; + int gyroAddr = BMG160_DEFAULT_ADDR; + int gyroCS = -1; + + int magBus = BMM150_DEFAULT_I2C_BUS; + int magAddr = BMX055_DEFAULT_MAG_I2C_ADDR; + int magCS = -1; + + std::vector upmTokens; + upmTokens = UpmStringParser::parse(initStr); + + for(std::string tok : upmTokens) + { + if(tok.substr(0, 9) == "accelBus:") + accelBus = std::stoi(tok.substr(9), nullptr, 0); + if(tok.substr(0, 10) == "accelAddr:") + accelAddr = std::stoi(tok.substr(10), nullptr, 0); + if(tok.substr(0, 8) == "accelCS:") + accelCS = std::stoi(tok.substr(8), nullptr, 0); + if(tok.substr(0, 8) == "gyroBus:") + gyroBus = std::stoi(tok.substr(8), nullptr, 0); + if(tok.substr(0, 9) == "gyroAddr:") + gyroAddr = std::stoi(tok.substr(9), nullptr, 0); + if(tok.substr(0, 7) == "gyroCS:") + gyroCS = std::stoi(tok.substr(7), nullptr, 0); + if(tok.substr(0, 7) == "magBus:") + magBus = std::stoi(tok.substr(7), nullptr, 0); + if(tok.substr(0, 8) == "magAddr:") + magAddr = std::stoi(tok.substr(8), nullptr, 0); + if(tok.substr(0, 6) == "magCS:") + magCS = std::stoi(tok.substr(6), nullptr, 0); + } + + // if -1 is supplied as a bus for any of these, we will not + // instantiate them + + if (accelBus >= 0) + m_accel = new BMA250E(accelBus, accelAddr, accelCS); + + if (gyroBus >= 0) + m_gyro = new BMG160(gyroBus, gyroAddr, gyroCS); + + if (magBus >= 0) + m_mag = new BMM150(magBus, magAddr, magCS); + + std::string::size_type sz, prev_sz; + for(std::string tok : upmTokens) + { + if(tok.substr(0, 18) == "initAccelerometer:") + { + BMA250E_POWER_MODE_T pwr = (BMA250E_POWER_MODE_T)std::stoi(tok.substr(18), &sz, 0); + tok = tok.substr(18); + prev_sz = sz; + BMA250E_RANGE_T range = (BMA250E_RANGE_T)std::stoi(tok.substr(prev_sz + 1), &sz, 0); + tok = tok.substr(prev_sz + 1); + BMA250E_BW_T bw = (BMA250E_BW_T)std::stoi(tok.substr(sz + 1), nullptr, 0); + initAccelerometer(pwr, range, bw); + } + if(tok.substr(0, 14) == "initGyroscope:") + { + BMG160_POWER_MODE_T pwr = (BMG160_POWER_MODE_T)std::stoi(tok.substr(14), &sz, 0); + tok = tok.substr(14); + prev_sz = sz; + BMG160_RANGE_T range = (BMG160_RANGE_T)std::stoi(tok.substr(prev_sz + 1), &sz, 0); + tok = tok.substr(prev_sz + 1); + BMG160_BW_T bw = (BMG160_BW_T)std::stoi(tok.substr(sz + 1), nullptr, 0); + initGyroscope(pwr, range, bw); + } + if(tok.substr(0, 17) == "initMagnetometer:") + { + BMM150_USAGE_PRESETS_T usage = (BMM150_USAGE_PRESETS_T)std::stoi(tok.substr(17), nullptr, 0); + initMagnetometer(usage); + } + } +} + BMX055::~BMX055() { if (m_accel) diff --git a/src/bmx055/bmx055.hpp b/src/bmx055/bmx055.hpp index 333cd44a..e28dce9e 100644 --- a/src/bmx055/bmx055.hpp +++ b/src/bmx055/bmx055.hpp @@ -114,6 +114,13 @@ namespace upm { int magAddr=BMX055_DEFAULT_MAG_I2C_ADDR, int magCS=-1); + /** + * Instantiates BMX055 based on a given string. + * + * @param initStr string containing specific information for BMX055 initialization. + */ + BMX055(std::string initStr); + /** * BMX055 Destructor. */