alexey.zholtikov/upm
1
0
Fork 0
mirror of https://github.com/eclipse/upm.git synced 2025-03-15 04:57:30 +03:00
Code Issues Actions Packages Projects Releases Wiki Activity

bno055: Initial implementation

Browse Source 
This driver implements support for the Bosch BNO055 Absolute
Orientation 9DOF Fusion Hub.   It was implemented on the Adafruit
variant at https://www.adafruit.com/products/2472.

The BNO055 is a System in Package (SiP), integrating a triaxial 14-bit
accelerometer, a triaxial 16-bit gyroscope with a range of ±2000
degrees per second, a triaxial geomagnetic sensor and a 32-bit cortex
M0+ microcontroller running Bosch Sensortec sensor fusion software, in
a single package.

This sensor handles the hard problem of combining various sensor
information into a reliable measurement of sensor orientation (refered
to as 'sensor fusion').  The onboard MCU runs this software and can
provide fusion output in the form of Euler Angles, Quaternions, Linear
Acceleration, and Gravity Vectors in 3 axes.

The focus on this driver has been on supporting the fusion components.
Less support is available for use of this device as a generic
accelerometer, gyroscope and magnetometer, however enough
infrastructure is available to add any missing functionality.

Signed-off-by: Jon Trulson <jtrulson@ics.com>
Signed-off-by: Noel Eck <noel.eck@intel.com>
This commit is contained in:
Jon Trulson 2016-04-15 10:47:40 -06:00 committed by Noel Eck
parent d045dded7c
commit 456bde0726
12 changed files with 2901 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
examples/c++/CMakeLists.txt
View File

@ -267,6 +267,7 @@ add_example (vcap)
add_example (ds2413)
add_example (ds18b20)
add_example (bmp280)
add_example (bno055)
# These are special cases where you specify example binary, source file and module(s)
include_directories (${PROJECT_SOURCE_DIR}/src)

129
examples/c++/bno055.cxx Normal file
View File

@ -0,0 +1,129 @@
/*
* Author: Jon Trulson <jtrulson@ics.com>
* Copyright (c) 2016 Intel Corporation.
*
* Permission is hereby granted, free of charge, to any person obtaining
* a copy of this software and associated documentation files (the
* "Software"), to deal in the Software without restriction, including
* without limitation the rights to use, copy, modify, merge, publish,
* distribute, sublicense, and/or sell copies of the Software, and to
* permit persons to whom the Software is furnished to do so, subject to
* the following conditions:
*
* The above copyright notice and this permission notice shall be
* included in all copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
* NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE
* LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
* OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION
* WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/
#include <unistd.h>
#include <iostream>
#include <signal.h>
#include "bno055.hpp"
using namespace std;
int shouldRun = true;
void sig_handler(int signo)
{
if (signo == SIGINT)
shouldRun = false;
}
int main(int argc, char **argv)
{
signal(SIGINT, sig_handler);
//! [Interesting]
// Instantiate an BNO055 using default parameters (bus 0, addr
// 0x28). The default running mode is NDOF absolute orientation
// mode.
upm::BNO055 *sensor = new upm::BNO055();
// First we need to calibrate....
cout << "First we need to calibrate. 4 numbers will be output every"
<< endl;
cout << "second for each sensor. 0 means uncalibrated, and 3 means"
<< endl;
cout << "fully calibrated."
<< endl;
cout << "See the UPM documentation on this sensor for instructions on"
<< endl;
cout << "what actions are required to calibrate."
<< endl;
cout << endl;
// do the calibration...
while (shouldRun && !sensor->isFullyCalibrated())
{
int mag, acc, gyr, sys;
sensor->getCalibrationStatus(&mag, &acc, &gyr, &sys);
cout << "Magnetometer: " << mag
<< " Accelerometer: " << acc
<< " Gyroscope: " << gyr
<< " System: " << sys
<< endl;
sleep(1);
}
cout << endl;
cout << "Calibration complete." << endl;
cout << endl;
// now output various fusion data every 250 milliseconds
while (shouldRun)
{
float w, x, y, z;
sensor->update();
sensor->getEulerAngles(&x, &y, &z);
cout << "Euler: Heading: " << x
<< " Roll: " << y
<< " Pitch: " << z
<< " degrees"
<< endl;
sensor->getQuaternions(&w, &x, &y, &z);
cout << "Quaternion: W: " << w
<< " X: " << x
<< " Y: " << y
<< " Z: " << z
<< endl;
sensor->getLinearAcceleration(&x, &y, &z);
cout << "Linear Acceleration: X: " << x
<< " Y: " << y
<< " Z: " << z
<< " m/s^2"
<< endl;
sensor->getGravityVectors(&x, &y, &z);
cout << "Gravity Vector: X: " << x
<< " Y: " << y
<< " Z: " << z
<< " m/s^2"
<< endl;
cout << endl;
usleep(250000);
}
//! [Interesting]
cout << "Exiting..." << endl;
delete sensor;
return 0;
}

99
examples/java/BNO055_Example.java Normal file
View File

@ -0,0 +1,99 @@
/*
* Author: Jon Trulson <jtrulson@ics.com>
* Copyright (c) 2016 Intel Corporation.
*
* Permission is hereby granted, free of charge, to any person obtaining
* a copy of this software and associated documentation files (the
* "Software"), to deal in the Software without restriction, including
* without limitation the rights to use, copy, modify, merge, publish,
* distribute, sublicense, and/or sell copies of the Software, and to
* permit persons to whom the Software is furnished to do so, subject to
* the following conditions:
*
* The above copyright notice and this permission notice shall be
* included in all copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
* NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE
* LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
* OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION
* WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/
import upm_bno055.BNO055;
public class BNO055_Example
{
public static void main(String[] args) throws InterruptedException
{
// ! [Interesting]
System.out.println("Initializing...");
// Instantiate an BNO055 using default parameters (bus 0, addr
// 0x28). The default running mode is NDOF absolute orientation
// mode.
BNO055 sensor = new BNO055();
System.out.println("First we need to calibrate. 4 numbers will be output every");
System.out.println("second for each sensor. 0 means uncalibrated, and 3 means");
System.out.println("fully calibrated.");
System.out.println("See the UPM documentation on this sensor for instructions on");
System.out.println("what actions are required to calibrate.");
System.out.println("");
while (!sensor.isFullyCalibrated())
{
int calData[] = sensor.getCalibrationStatus();
System.out.println("Magnetometer: " + calData[0]
+ " Accelerometer: " + calData[1]
+ " Gyroscope: " + calData[2]
+ " System: " + calData[3]);
Thread.sleep(1000);
}
System.out.println("");
System.out.println("Calibration complete.");
System.out.println("");
while (true)
{
// update our values from the sensor
sensor.update();
float dataE[] = sensor.getEulerAngles();
System.out.println("Euler: Heading: " + dataE[0] +
" Roll: " + dataE[1] +
" Pitch: " + dataE[2] +
" degrees");
float dataQ[] = sensor.getQuaternions();
System.out.println("Quaternion: W: " + dataQ[0] +
" X:" + dataQ[1] +
" Y: " + dataQ[2] +
" Z: " + dataQ[3]);
float dataL[] = sensor.getLinearAcceleration();
System.out.println("Linear Acceleration: X: " + dataL[0] +
" Y: " + dataL[1] +
" Z: " + dataL[2] +
" m/s^2");
float dataG[] = sensor.getGravityVectors();
System.out.println("Gravity Vector: X: " + dataG[0] +
" Y: " + dataG[1] +
" Z: " + dataG[2] +
" m/s^2");
System.out.println();
Thread.sleep(250);
}
// ! [Interesting]
}
}

1
examples/java/CMakeLists.txt
View File

@ -127,6 +127,7 @@ if (BACNET_FOUND)
endif()
add_example(VCAP_Example vcap)
add_example(BMP280_Example bmp280)
add_example(BNO055_Example bno055)
add_example_with_path(Jhd1313m1_lcdSample lcd i2clcd)
add_example_with_path(Jhd1313m1Sample lcd i2clcd)

117
examples/javascript/bno055.js Normal file
View File

@ -0,0 +1,117 @@
/*jslint node:true, vars:true, bitwise:true, unparam:true */
/*jshint unused:true */
/*
* Author: Jon Trulson <jtrulson@ics.com>
* Copyright (c) 2016 Intel Corporation.
*
* Permission is hereby granted, free of charge, to any person obtaining
* a copy of this software and associated documentation files (the
* "Software"), to deal in the Software without restriction, including
* without limitation the rights to use, copy, modify, merge, publish,
* distribute, sublicense, and/or sell copies of the Software, and to
* permit persons to whom the Software is furnished to do so, subject to
* the following conditions:
*
* The above copyright notice and this permission notice shall be
* included in all copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
* NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE
* LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
* OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION
* WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/
var sensorObj = require('jsupm_bno055');
// Instantiate an BNO055 using default parameters (bus 0, addr
// 0x28). The default running mode is NDOF absolute orientation
// mode.
var sensor = new sensorObj.BNO055();
var mag = new sensorObj.new_intp();
var acc = new sensorObj.new_intp();
var gyr = new sensorObj.new_intp();
var syst = new sensorObj.new_intp();
var w = new sensorObj.new_floatp();
var x = new sensorObj.new_floatp();
var y = new sensorObj.new_floatp();
var z = new sensorObj.new_floatp();
console.log("First we need to calibrate. 4 numbers will be output every");
console.log("second for each sensor. 0 means uncalibrated, and 3 means");
console.log("fully calibrated.");
console.log("See the UPM documentation on this sensor for instructions on");
console.log("what actions are required to calibrate.");
console.log("");
// do the calibration...
var calInterval = setInterval(function()
{
if (sensor.isFullyCalibrated())
{
clearInterval(calInterval);
console.log("");
console.log("Calibration complete.");
console.log("");
setInterval(outputData, 250)
}
else
{
sensor.getCalibrationStatus(mag, acc, gyr, syst);
console.log("Magnetometer: " + sensorObj.intp_value(mag) +
" Accelerometer: " + sensorObj.intp_value(acc) +
" Gyroscope: " + sensorObj.intp_value(gyr) +
" System: " + sensorObj.intp_value(syst));
}
}, 1000);
// now output various fusion data every 250 milliseconds
function outputData()
{
sensor.update();
sensor.getEulerAngles(x, y, z);
console.log("Euler: Heading: " + sensorObj.floatp_value(x) +
" Roll: " + sensorObj.floatp_value(y) +
" Pitch: " + sensorObj.floatp_value(z) +
" degrees");
sensor.getQuaternions(w, x, y, z);
console.log("Quaternion: W: " + sensorObj.floatp_value(w) +
" X:" + sensorObj.floatp_value(x) +
" Y: " + sensorObj.floatp_value(y) +
" Z: " + sensorObj.floatp_value(z));
sensor.getLinearAcceleration(x, y, z);
console.log("Linear Acceleration: X: " + sensorObj.floatp_value(x) +
" Y: " + sensorObj.floatp_value(y) +
" Z: " + sensorObj.floatp_value(z) +
" m/s^2");
sensor.getGravityVectors(x, y, z);
console.log("Gravity Vector: X: " + sensorObj.floatp_value(x) +
" Y: " + sensorObj.floatp_value(y) +
" Z: " + sensorObj.floatp_value(z) +
" m/s^2");
console.log("");
};
// exit on ^C
process.on('SIGINT', function()
{
sensor = null;
sensorObj.cleanUp();
sensorObj = null;
console.log("Exiting.");
process.exit(0);
});

105
examples/python/bno055.py Normal file
View File

@ -0,0 +1,105 @@
#!/usr/bin/python
# Author: Jon Trulson <jtrulson@ics.com>
# Copyright (c) 2016 Intel Corporation.
#
# Permission is hereby granted, free of charge, to any person obtaining
# a copy of this software and associated documentation files (the
# "Software"), to deal in the Software without restriction, including
# without limitation the rights to use, copy, modify, merge, publish,
# distribute, sublicense, and/or sell copies of the Software, and to
# permit persons to whom the Software is furnished to do so, subject to
# the following conditions:
#
# The above copyright notice and this permission notice shall be
# included in all copies or substantial portions of the Software.
#
# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
# EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
# MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
# NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE
# LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
# OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION
# WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
import time, sys, signal, atexit
import pyupm_bno055 as sensorObj
# Instantiate an BNO055 using default parameters (bus 0, addr
# 0x28). The default running mode is NDOF absolute orientation
# mode.
sensor = sensorObj.BNO055()
## Exit handlers ##
# This function stops python from printing a stacktrace when you hit control-C
def SIGINTHandler(signum, frame):
raise SystemExit
# This function lets you run code on exit
def exitHandler():
print "Exiting..."
sys.exit(0)
# Register exit handlers
atexit.register(exitHandler)
signal.signal(signal.SIGINT, SIGINTHandler)
mag = sensorObj.new_intp()
acc = sensorObj.new_intp()
gyr = sensorObj.new_intp()
syst = sensorObj.new_intp()
w = sensorObj.new_floatp()
x = sensorObj.new_floatp()
y = sensorObj.new_floatp()
z = sensorObj.new_floatp()
print "First we need to calibrate. 4 numbers will be output every"
print "second for each sensor. 0 means uncalibrated, and 3 means"
print "fully calibrated."
print "See the UPM documentation on this sensor for instructions on"
print "what actions are required to calibrate."
print
while (not sensor.isFullyCalibrated()):
sensor.getCalibrationStatus(mag, acc, gyr, syst)
print "Magnetometer:", sensorObj.intp_value(mag),
print " Accelerometer:", sensorObj.intp_value(acc),
print " Gyroscope:", sensorObj.intp_value(gyr),
print " System:", sensorObj.intp_value(syst),
time.sleep(1)
print
print "Calibration complete."
print
# now output various fusion data every 250 milliseconds
while (True):
sensor.update()
sensor.getEulerAngles(x, y, z)
print "Euler: Heading:", sensorObj.floatp_value(x),
print " Roll:", sensorObj.floatp_value(y),
print " Pitch:", sensorObj.floatp_value(z),
print " degrees"
sensor.getQuaternions(w, x, y, z)
print "Quaternion: W:", sensorObj.floatp_value(w),
print " X:", sensorObj.floatp_value(x),
print " Y:", sensorObj.floatp_value(y),
print " Z:", sensorObj.floatp_value(z)
sensor.getLinearAcceleration(x, y, z)
print "Linear Acceleration: X:", sensorObj.floatp_value(x),
print " Y:", sensorObj.floatp_value(y),
print " Z:", sensorObj.floatp_value(z),
print " m/s^2"
sensor.getGravityVectors(x, y, z)
print "Gravity Vector: X:", sensorObj.floatp_value(x),
print " Y:", sensorObj.floatp_value(y),
print " Z:", sensorObj.floatp_value(z),
print " m/s^2"
print
time.sleep(.25);

5
src/bno055/CMakeLists.txt Normal file
View File

@ -0,0 +1,5 @@
set (libname "bno055")
set (libdescription "Bosch bno055 intelligent orientation sensor 9dof fusion")
set (module_src ${libname}.cxx)
set (module_hpp ${libname}.hpp)
upm_module_init()

809
src/bno055/bno055.cxx Normal file
View File

@ -0,0 +1,809 @@
/*
* Author: Jon Trulson <jtrulson@ics.com>
* Copyright (c) 2016 Intel Corporation.
*
* Permission is hereby granted, free of charge, to any person obtaining
* a copy of this software and associated documentation files (the
* "Software"), to deal in the Software without restriction, including
* without limitation the rights to use, copy, modify, merge, publish,
* distribute, sublicense, and/or sell copies of the Software, and to
* permit persons to whom the Software is furnished to do so, subject to
* the following conditions:
*
* The above copyright notice and this permission notice shall be
* included in all copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
* NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE
* LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
* OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION
* WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/
#include <unistd.h>
#include <iostream>
#include <stdexcept>
#include <string>
#include <string.h>
#include "bno055.hpp"
using namespace upm;
using namespace std;
// conversion from fahrenheit to celcius and back
static float f2c(float f)
{
return ((f - 32.0) / (9.0 / 5.0));
}
static float c2f(float c)
{
return (c * (9.0 / 5.0) + 32.0);
}
BNO055::BNO055(int bus, uint8_t addr) :
m_i2c(bus), m_gpioIntr(0)
{
m_addr = addr;
clearData();
mraa::Result rv;
if ( (rv = m_i2c.address(m_addr)) != mraa::SUCCESS)
{
throw std::runtime_error(string(__FUNCTION__) +
": I2c.address() failed");
return;
}
// forcibly set page 0, so we are synced
setPage(0, true);
// set config mode
setOperationMode(OPERATION_MODE_CONFIGMODE);
// default to internal clock
setClockExternal(false);
// we specifically avoid doing a reset so that if the device is
// already calibrated, it will remain so.
// check the chip id
uint8_t chipID = readReg(REG_CHIP_ID);
if (chipID != BNO055_CHIPID)
{
throw std::runtime_error(string(__FUNCTION__)
+ ": invalid chip ID. Expected "
+ std::to_string(int(BNO055_CHIPID))
+ ", got "
+ std::to_string(int(chipID)));
return;
}
// default to temperature C
setTemperatureUnits(true);
// default to accelerometer temp
setTemperatureSource(TEMP_SOURCE_ACC);
// set accel units to m/s^2
setAccelerometerUnits(false);
// set gyro units to degrees
setGyroscopeUnits(false);
// set Euler units to degrees
setEulerUnits(false);
// by default, we set the operating mode to the NDOF fusion mode
setOperationMode(OPERATION_MODE_NDOF);
}
BNO055::~BNO055()
{
uninstallISR();
}
void BNO055::update()
{
setPage(0);
// temperature first, we always store as C
float tmpF = float((int8_t)readReg(REG_TEMPERATURE));
if (m_tempIsC)
m_temperature = tmpF;
else
m_temperature = f2c(tmpF * 2.0);
updateFusionData();
updateNonFusionData();
}
uint8_t BNO055::readReg(uint8_t reg)
{
return m_i2c.readReg(reg);
}
void BNO055::readRegs(uint8_t reg, uint8_t *buffer, int len)
{
m_i2c.readBytesReg(reg, buffer, len);
}
bool BNO055::writeReg(uint8_t reg, uint8_t val)
{
mraa::Result rv;
if ((rv = m_i2c.writeReg(reg, val)) != mraa::SUCCESS)
{
throw std::runtime_error(std::string(__FUNCTION__)
+ ": I2c.writeReg() failed");
}
return true;
}
bool BNO055::writeRegs(uint8_t reg, uint8_t *buffer, int len)
{
uint8_t buf[len + 1];
buf[0] = reg;
for (int i=0; i<len; i++)
buf[i+1] = buffer[i];
mraa::Result rv;
if ((rv = m_i2c.write(buf, len+1)) != mraa::SUCCESS)
{
throw std::runtime_error(std::string(__FUNCTION__)
+ ": I2c.write() failed");
}
return true;
}
uint8_t BNO055::getChipID()
{
setPage(0);
return readReg(REG_CHIP_ID);
}
uint8_t BNO055::getACCID()
{
setPage(0);
return readReg(REG_ACC_ID);
}
uint8_t BNO055::getMAGID()
{
setPage(0);
return readReg(REG_MAG_ID);
}
uint8_t BNO055::getGYRID()
{
setPage(0);
return readReg(REG_GYR_ID);
}
uint16_t BNO055::getSWRevID()
{
setPage(0);
uint16_t vers = uint16_t( readReg(REG_SW_REV_ID_LSB) |
(readReg(REG_SW_REV_ID_MSB) << 8) );
return vers;
}
uint8_t BNO055::getBootLoaderID()
{
setPage(0);
return readReg(REG_BL_REV_ID);
}
void BNO055::setPage(uint8_t page, bool force)
{
// page can only be 0 or 1
if (!(page == 0 || page == 1))
throw std::out_of_range(string(__FUNCTION__) +
": page can only be 0 or 1");
if (force || page != m_currentPage)
writeReg(REG_PAGE_ID, page);
m_currentPage = page;
}
void BNO055::setClockExternal(bool extClock)
{
setPage(0);
// first we need to be in config mode
OPERATION_MODES_T currentMode = m_currentMode;
setOperationMode(OPERATION_MODE_CONFIGMODE);
uint8_t reg = readReg(REG_SYS_TRIGGER);
if (extClock)
reg |= SYS_TRIGGER_CLK_SEL;
else
reg &= ~SYS_TRIGGER_CLK_SEL;
writeReg(REG_SYS_TRIGGER, reg);
// now reset our operating mode
setOperationMode(currentMode);
}
void BNO055::setTemperatureSource(TEMP_SOURCES_T src)
{
setPage(0);
writeReg(REG_TEMP_SOURCE, src);
}
void BNO055::setTemperatureUnits(bool celcius)
{
setPage(0);
uint8_t reg = readReg(REG_UNIT_SEL);
if (celcius)
reg &= ~UNIT_SEL_TEMP_UNIT;
else
reg |= UNIT_SEL_TEMP_UNIT;
writeReg(REG_UNIT_SEL, reg);
m_tempIsC = celcius;
}
void BNO055::setAccelerometerUnits(bool mg)
{
setPage(0);
uint8_t reg = readReg(REG_UNIT_SEL);
if (mg)
{
reg |= UNIT_SEL_ACC_UNIT;
m_accUnitScale = 1.0;
}
else
{
reg &= ~UNIT_SEL_ACC_UNIT;
m_accUnitScale = 100.0;
}
writeReg(REG_UNIT_SEL, reg);
}
void BNO055::setGyroscopeUnits(bool radians)
{
setPage(0);
uint8_t reg = readReg(REG_UNIT_SEL);
if (radians)
{
reg |= UNIT_SEL_GYR_UNIT;
m_gyrUnitScale = 900.0;
}
else
{
reg &= ~UNIT_SEL_GYR_UNIT;
m_gyrUnitScale = 16.0;
}
writeReg(REG_UNIT_SEL, reg);
}
void BNO055::setEulerUnits(bool radians)
{
setPage(0);
uint8_t reg = readReg(REG_UNIT_SEL);
if (radians)
{
reg |= UNIT_SEL_EUL_UNIT;
m_eulUnitScale = 900.0;
}
else
{
reg &= ~UNIT_SEL_EUL_UNIT;
m_eulUnitScale = 16.0;
}
writeReg(REG_UNIT_SEL, reg);
}
void BNO055::setOperationMode(OPERATION_MODES_T mode)
{
setPage(0);
// we clear all of our loaded data on mode changes
clearData();
uint8_t reg = readReg(REG_OPER_MODE);
reg &= ~(_OPR_MODE_OPERATION_MODE_MASK << _OPR_MODE_OPERATION_MODE_SHIFT);
reg |= (mode << _OPR_MODE_OPERATION_MODE_SHIFT);
writeReg(REG_OPER_MODE, reg);
m_currentMode = mode;
usleep(30);
}
void BNO055::getCalibrationStatus(int *mag, int *acc, int *gyr, int *sys)
{
setPage(0);
uint8_t reg = readReg(REG_CALIB_STAT);
if (mag)
*mag = (reg >> _CALIB_STAT_MAG_SHIFT) & _CALIB_STAT_MAG_MASK;
if (acc)
*acc = (reg >> _CALIB_STAT_ACC_SHIFT) & _CALIB_STAT_ACC_MASK;
if (gyr)
*gyr = (reg >> _CALIB_STAT_GYR_SHIFT) & _CALIB_STAT_GYR_MASK;
if (sys)
*sys = (reg >> _CALIB_STAT_SYS_SHIFT) & _CALIB_STAT_SYS_MASK;
}
int *BNO055::getCalibrationStatus()
{
static int v[4]; // mag, acc, gyr, sys;
getCalibrationStatus(&v[0], &v[1], &v[2], &v[3]);
return v;
}
bool BNO055::isFullyCalibrated()
{
int mag, acc, gyr, sys;
getCalibrationStatus(&mag, &acc, &gyr, &sys);
// all of them equal to 3 means fully calibrated
if (mag == 3 && acc == 3 && gyr == 3 && sys == 3)
return true;
else
return false;
}
void BNO055::resetSystem()
{
setPage(0);
uint8_t reg = readReg(REG_SYS_TRIGGER);
reg |= SYS_TRIGGER_RST_SYS;
writeReg(REG_SYS_TRIGGER, reg);
sleep(1);
}
void BNO055::resetInterruptStatus()
{
setPage(0);
uint8_t reg = readReg(REG_SYS_TRIGGER);
reg |= SYS_TRIGGER_RST_INT;
writeReg(REG_SYS_TRIGGER, reg);
}
uint8_t BNO055::getInterruptStatus()
{
setPage(0);
return readReg(REG_INT_STA);
}
uint8_t BNO055::getInterruptEnable()
{
setPage(1);
return readReg(REG_INT_EN);
}
void BNO055::setInterruptEnable(uint8_t enables)
{
setPage(1);
writeReg(REG_INT_EN, enables);
}
uint8_t BNO055::getInterruptMask()
{
setPage(1);
return readReg(REG_INT_MSK);
}
void BNO055::setInterruptMask(uint8_t mask)
{
setPage(1);
writeReg(REG_INT_MSK, mask);
}
BNO055::SYS_STATUS_T BNO055::getSystemStatus()
{
setPage(0);
return static_cast<BNO055::SYS_STATUS_T>(readReg(REG_SYS_STATUS));
}
BNO055::SYS_ERR_T BNO055::getSystemError()
{
setPage(0);
return static_cast<BNO055::SYS_ERR_T>(readReg(REG_SYS_ERROR));
}
string BNO055::readCalibrationData()
{
if (!isFullyCalibrated())
{
cerr << __FUNCTION__ << ": Sensor must be fully calibrated first."
<< endl;
return "";
}
// should be at page 0, but lets make sure
setPage(0);
// first we need to go back into config mode
OPERATION_MODES_T currentMode = m_currentMode;
setOperationMode(OPERATION_MODE_CONFIGMODE);
uint8_t calibData[calibrationDataNumBytes];
readRegs(REG_ACC_OFFSET_X_LSB, calibData, calibrationDataNumBytes);
string rv((char *)calibData, calibrationDataNumBytes);
// now reset our operating mode
setOperationMode(currentMode);
return rv;
}
void BNO055::writeCalibrationData(string calibData)
{
if (calibData.size() != calibrationDataNumBytes)
{
throw std::invalid_argument(std::string(__FUNCTION__)
+ ": calibData string must be exactly "
+ std::to_string(calibrationDataNumBytes)
+ " bytes long");
}
// should be at page 0, but lets make sure
setPage(0);
// first we need to go back into config mode
OPERATION_MODES_T currentMode = m_currentMode;
setOperationMode(OPERATION_MODE_CONFIGMODE);
// write the data
writeRegs(REG_ACC_OFFSET_X_LSB, (uint8_t *)calibData.c_str(),
calibData.size());
// now reset our operating mode
setOperationMode(currentMode);
}
float BNO055::getTemperature(bool fahrenheit)
{
if (fahrenheit)
return c2f(m_temperature);
else
return m_temperature;
}
void BNO055::clearData()
{
m_magX = m_magY = m_magZ = 0;
m_accX = m_accY = m_accZ = 0;
m_gyrX = m_gyrY = m_gyrZ = 0;
m_eulHeading = m_eulRoll = m_eulPitch = 0;
m_quaW = m_quaX = m_quaY = m_quaZ = 0;
m_liaX = m_liaY = m_liaZ = 0;
m_grvX = m_grvY = m_grvZ = 0;
}
bool BNO055::updateFusionData()
{
// bail if we are in config mode, or aren't in a fusion mode...
if (m_currentMode == OPERATION_MODE_CONFIGMODE ||
m_currentMode < OPERATION_MODE_IMU)
return false;
setPage(0);
// FIXME/MAYBE? - abort early if SYS calibration is == 0?
const int fusionBytes = 26;
uint8_t buf[fusionBytes];
readRegs(REG_EUL_HEADING_LSB, buf, fusionBytes);
m_eulHeading = float(int16_t(buf[0] | (buf[1] << 8)));
m_eulRoll = float(int16_t(buf[2] | (buf[3] << 8)));
m_eulPitch = float(int16_t(buf[4] | (buf[5] << 8)));
m_quaW = float(int16_t(buf[6] | (buf[7] << 8)));
m_quaX = float(int16_t(buf[8] | (buf[9] << 8)));
m_quaY = float(int16_t(buf[10] | (buf[11] << 8)));
m_quaZ = float(int16_t(buf[12] | (buf[13] << 8)));
m_liaX = float(int16_t(buf[14] | (buf[15] << 8)));
m_liaY = float(int16_t(buf[16] | (buf[17] << 8)));
m_liaZ = float(int16_t(buf[18] | (buf[19] << 8)));
m_grvX = float(int16_t(buf[20] | (buf[21] << 8)));
m_grvY = float(int16_t(buf[22] | (buf[23] << 8)));
m_grvZ = float(int16_t(buf[24] | (buf[25] << 8)));
return true;
}
bool BNO055::updateNonFusionData()
{
// bail if we are in config mode...
if (m_currentMode == OPERATION_MODE_CONFIGMODE)
return false;
setPage(0);
const int nonFusionBytes = 18;
uint8_t buf[nonFusionBytes];
readRegs(REG_ACC_DATA_X_LSB, buf, nonFusionBytes);
m_accX = float(int16_t(buf[0] | (buf[1] << 8)));
m_accY = float(int16_t(buf[2] | (buf[3] << 8)));
m_accZ = float(int16_t(buf[4] | (buf[5] << 8)));
m_magX = float(int16_t(buf[6] | (buf[7] << 8)));
m_magY = float(int16_t(buf[8] | (buf[9] << 8)));
m_magZ = float(int16_t(buf[10] | (buf[11] << 8)));
m_gyrX = float(int16_t(buf[12] | (buf[13] << 8)));
m_gyrY = float(int16_t(buf[14] | (buf[15] << 8)));
m_gyrZ = float(int16_t(buf[16] | (buf[17] << 8)));
return true;
}
void BNO055::getEulerAngles(float *heading, float *roll, float *pitch)
{
if (heading)
*heading = m_eulHeading / m_eulUnitScale;
if (roll)
*roll = m_eulRoll / m_eulUnitScale;
if (pitch)
*pitch = m_eulPitch / m_eulUnitScale;
}
float *BNO055::getEulerAngles()
{
static float v[3];
getEulerAngles(&v[0], &v[1], &v[2]);
return v;
}
void BNO055::getQuaternions(float *w, float *x, float *y, float *z)
{
// from the datasheet
const float scale = float(1.0 / (1 << 14));
if (w)
*w = m_quaW * scale;
if (x)
*x = m_quaX * scale;
if (y)
*y = m_quaY * scale;
if (z)
*z = m_quaZ * scale;
}
float *BNO055::getQuaternions()
{
static float v[4];
getQuaternions(&v[0], &v[1], &v[2], &v[3]);
return v;
}
void BNO055::getLinearAcceleration(float *x, float *y, float *z)
{
if (x)
*x = m_liaX / m_accUnitScale;
if (y)
*y = m_liaY / m_accUnitScale;
if (z)
*z = m_liaZ / m_accUnitScale;
}
float *BNO055::getLinearAcceleration()
{
static float v[3];
getLinearAcceleration(&v[0], &v[1], &v[2]);
return v;
}
void BNO055::getGravityVectors(float *x, float *y, float *z)
{
if (x)
*x = m_grvX / m_accUnitScale;
if (y)
*y = m_grvY / m_accUnitScale;
if (z)
*z = m_grvZ / m_accUnitScale;
}
float *BNO055::getGravityVectors()
{
static float v[3];
getGravityVectors(&v[0], &v[1], &v[2]);
return v;
}
void BNO055::getAccelerometer(float *x, float *y, float *z)
{
if (x)
*x = m_accX / m_accUnitScale;
if (y)
*y = m_accY / m_accUnitScale;
if (z)
*z = m_accZ / m_accUnitScale;
}
float *BNO055::getAccelerometer()
{
static float v[3];
getAccelerometer(&v[0], &v[1], &v[2]);
return v;
}
void BNO055::getMagnetometer(float *x, float *y, float *z)
{
// from the datasheet - 16 uT's per LSB
const float scale = 16.0;
if (x)
*x = m_magX / scale;
if (y)
*y = m_magY / scale;
if (z)
*z = m_magZ / scale;
}
float *BNO055::getMagnetometer()
{
static float v[3];
getMagnetometer(&v[0], &v[1], &v[2]);
return v;
}
void BNO055::getGyroscope(float *x, float *y, float *z)
{
if (x)
*x = m_gyrX / m_gyrUnitScale;
if (y)
*y = m_gyrY / m_gyrUnitScale;
if (z)
*z = m_gyrZ / m_gyrUnitScale;
}
float *BNO055::getGyroscope()
{
static float v[3];
getGyroscope(&v[0], &v[1], &v[2]);
return v;
}
void BNO055::setAccelerationConfig(ACC_RANGE_T range, ACC_BW_T bw,
ACC_PWR_MODE_T pwr)
{
setPage(1);
uint8_t reg = ((range << _ACC_CONFIG_ACC_RANGE_SHIFT) |
(bw << _ACC_CONFIG_ACC_BW_SHIFT) |
(pwr << _ACC_CONFIG_ACC_PWR_MODE_SHIFT));
writeReg(REG_ACC_CONFIG, reg);
}
void BNO055::setMagnetometerConfig(MAG_ODR_T odr, MAG_OPR_T opr,
MAG_POWER_T pwr)
{
setPage(1);
uint8_t reg = ((odr << _MAG_CONFIG_MAG_ODR_SHIFT) |
(opr << _MAG_CONFIG_MAG_OPR_MODE_SHIFT) |
(pwr << _MAG_CONFIG_MAG_POWER_MODE_SHIFT));
writeReg(REG_MAG_CONFIG, reg);
}
void BNO055::setGyroscopeConfig(GYR_RANGE_T range, GYR_BW_T bw,
GYR_POWER_MODE_T pwr)
{
setPage(1);
uint8_t reg = ((range << _GYR_CONFIG0_GYR_RANGE_SHIFT) |
(bw << _GYR_CONFIG0_GYR_BW_SHIFT));
writeReg(REG_GYR_CONFIG0, reg);
reg = (pwr << _GYR_CONFIG1_GYR_POWER_MODE_SHIFT);
writeReg(REG_GYR_CONFIG1, reg);
}
#if defined(SWIGJAVA) || (JAVACALLBACK)
void BNO055::installISR(int gpio, mraa::Edge level,
jobject runnable)
{
// delete any existing ISR and GPIO context
uninstallISR();
// create gpio context
m_gpioIntr = new mraa::Gpio(gpio);
m_gpioIntr->dir(mraa::DIR_IN);
m_gpioIntr->isr(level, runnable);
}
#else
void BNO055::installISR(int gpio, mraa::Edge level,
void (*isr)(void *), void *arg)
{
// delete any existing ISR and GPIO context
uninstallISR();
// create gpio context
m_gpioIntr = new mraa::Gpio(gpio);
m_gpioIntr->dir(mraa::DIR_IN);
m_gpioIntr->isr(level, isr, arg);
}
#endif
void BNO055::uninstallISR()
{
if (m_gpioIntr)
{
m_gpioIntr->isrExit();
delete m_gpioIntr;
m_gpioIntr = 0;
}
}

1508
src/bno055/bno055.hpp Normal file
View File

File diff suppressed because it is too large Load Diff

93
src/bno055/javaupm_bno055.i Normal file
View File

@ -0,0 +1,93 @@
%module javaupm_bno055
%include "../upm.i"
%include "cpointer.i"
%include "typemaps.i"
%include "arrays_java.i";
%include "../java_buffer.i"
%apply int {mraa::Edge};
%apply float *INOUT { float *x, float *y, float *z };
%apply float *INOUT { float *heading, float *roll, float *pitch };
%typemap(jni) float* "jfloatArray"
%typemap(jstype) float* "float[]"
%typemap(jtype) float* "float[]"
%typemap(javaout) float* {
return $jnicall;
}
%typemap(jni) int* "jintArray"
%typemap(jstype) int* "int[]"
%typemap(jtype) int* "int[]"
%typemap(javaout) int* {
return $jnicall;
}
%typemap(out) float *getAccelerometer {
$result = JCALL1(NewFloatArray, jenv, 3);
JCALL4(SetFloatArrayRegion, jenv, $result, 0, 3, $1);
}
%typemap(out) float *getMagnetometer {
$result = JCALL1(NewFloatArray, jenv, 3);
JCALL4(SetFloatArrayRegion, jenv, $result, 0, 3, $1);
}
%typemap(out) float *getGyroscope {
$result = JCALL1(NewFloatArray, jenv, 3);
JCALL4(SetFloatArrayRegion, jenv, $result, 0, 3, $1);
}
%typemap(out) float *getEulerAngles {
$result = JCALL1(NewFloatArray, jenv, 3);
JCALL4(SetFloatArrayRegion, jenv, $result, 0, 3, $1);
}
%typemap(out) float *getQuaternions {
$result = JCALL1(NewFloatArray, jenv, 4);
JCALL4(SetFloatArrayRegion, jenv, $result, 0, 4, $1);
}
%typemap(out) float *getLinearAcceleration {
$result = JCALL1(NewFloatArray, jenv, 3);
JCALL4(SetFloatArrayRegion, jenv, $result, 0, 3, $1);
}
%typemap(out) float *getGravityVectors {
$result = JCALL1(NewFloatArray, jenv, 3);
JCALL4(SetFloatArrayRegion, jenv, $result, 0, 3, $1);
}
%typemap(out) int *getCalibrationStatus {
$result = JCALL1(NewIntArray, jenv, 4);
JCALL4(SetIntArrayRegion, jenv, $result, 0, 4, (const int*)$1);
}
%ignore getCalibrationStatus(int *, int *, int *, int *);
%ignore getAccelerometer(float *, float *, float *);
%ignore getMagnetometer(float *, float *, float *);
%ignore getGyroscope(float *, float *, float *);
%ignore getEulerAngles(float *, float *, float *);
%ignore getQuaternions(float *, float *, float *, float *);
%ignore getLinearAcceleration(float *, float *, float *);
%ignore getGravityVectors(float *, float *, float *);
%{
#include "bno055.hpp"
%}
%include "bno055.hpp"
%pragma(java) jniclasscode=%{
static {
try {
System.loadLibrary("javaupm_bno055");
} catch (UnsatisfiedLinkError e) {
System.err.println("Native code library failed to load. \n" + e);
System.exit(1);
}
}
%}

12
src/bno055/jsupm_bno055.i Normal file
View File

@ -0,0 +1,12 @@
%module jsupm_bno055
%include "../upm.i"
%include "cpointer.i"
/* Send "int *" and "float *" to JavaScript as intp and floatp */
%pointer_functions(int, intp);
%pointer_functions(float, floatp);
%include "bno055.hpp"
%{
#include "bno055.hpp"
%}

22
src/bno055/pyupm_bno055.i Normal file
View File

@ -0,0 +1,22 @@
// Include doxygen-generated documentation
%include "pyupm_doxy2swig.i"
%module pyupm_bno055
%include "../upm.i"
%include "cpointer.i"
%include "stdint.i"
/* Send "int *" and "float *" to python as intp and floatp */
%pointer_functions(int, intp);
%pointer_functions(float, floatp);
%feature("autodoc", "3");
#ifdef DOXYGEN
%include "bno055_doc.i"
#endif
%include "bno055.hpp"
%{
#include "bno055.hpp"
%}