mirror of
https://github.com/eclipse/upm.git
synced 2025-03-14 20:47:30 +03:00
BNO055 9DOF: Add string based cons for Fusion Hub
Signed-off-by: Adelin Dobre <adelin.dobre@rinftech.com> Signed-off-by: Mihai Tudor Panu <mihai.tudor.panu@intel.com>
This commit is contained in:
Adelin Dobre
Mihai Tudor Panu
parent
03bc2ae26e
commit
90167f6ac1
@ -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,6 +146,26 @@ bno055_context bno055_init(int bus, uint8_t addr)
|
||||
return NULL;
|
||||
}
|
||||
|
||||
if(descs)
|
||||
{
|
||||
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;
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
if (!(dev->i2c = mraa_i2c_init(bus)))
|
||||
{
|
||||
printf("%s: mraa_i2c_init() failed.\n", __FUNCTION__);
|
||||
@ -159,6 +179,7 @@ bno055_context bno055_init(int bus, uint8_t addr)
|
||||
bno055_close(dev);
|
||||
return NULL;
|
||||
}
|
||||
}
|
||||
|
||||
_clear_data(dev);
|
||||
|
||||
|
||||
@ -28,6 +28,7 @@
|
||||
#include <stdexcept>
|
||||
|
||||
#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<std::string> 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);
|
||||
|
||||
@ -32,6 +32,7 @@
|
||||
|
||||
#include <mraa/i2c.h>
|
||||
#include <mraa/gpio.h>
|
||||
#include <mraa/initio.h>
|
||||
|
||||
#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.
|
||||
|
||||
@ -27,6 +27,8 @@
|
||||
|
||||
#include <vector>
|
||||
#include "bno055.h"
|
||||
#include "mraa/initio.hpp"
|
||||
|
||||
|
||||
#include <interfaces/iAcceleration.hpp>
|
||||
#include <interfaces/iGyroscope.hpp>
|
||||
@ -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
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user