EmaMaker
/
SPQR-Team1-2020
1
1
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

culetto

hardware
latino-deluxe 2019-10-14 15:25:04 +02:00
parent bfc0be7684
commit 17a8f262de
23 changed files with 142176 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
.vscode/settings.json vendored Normal file
View File

@ -0,0 +1,7 @@
{
"C_Cpp.errorSquiggles": "Disabled",
"terminal.integrated.env.linux": {
"PATH": "/home/ema/.platformio/penv/bin:/home/ema/.platformio/penv:/usr/local/bin:/usr/local/sbin:/usr/bin:/usr/lib/jvm/default/bin:/usr/bin/site_perl:/usr/bin/vendor_perl:/usr/bin/core_perl:/var/lib/snapd/snap/bin:/usr/lib/jvm/default/bin:/usr/bin/site_perl:/usr/bin/vendor_perl:/usr/bin/core_perl",
"PLATFORMIO_CALLER": "vscode"
}
}

699
lib/Adafruit_BNO055/Adafruit_BNO055.cpp Executable file
View File

@ -0,0 +1,699 @@
/***************************************************************************
This is a library for the BNO055 orientation sensor
Designed specifically to work with the Adafruit BNO055 Breakout.
Pick one up today in the adafruit shop!
------> http://www.adafruit.com/products
These sensors use I2C to communicate, 2 pins are required to interface.
Adafruit invests time and resources providing this open source code,
please support Adafruit andopen-source hardware by purchasing products
from Adafruit!
Written by KTOWN for Adafruit Industries.
MIT license, all text above must be included in any redistribution
***************************************************************************/
#if ARDUINO >= 100
#include "Arduino.h"
#else
#include "WProgram.h"
#endif
#include <math.h>
#include <limits.h>
#include "Adafruit_BNO055.h"
/***************************************************************************
CONSTRUCTOR
***************************************************************************/
/**************************************************************************/
/*!
@brief Instantiates a new Adafruit_BNO055 class
*/
/**************************************************************************/
Adafruit_BNO055::Adafruit_BNO055(int32_t sensorID, uint8_t address)
{
_sensorID = sensorID;
_address = address;
}
/***************************************************************************
PUBLIC FUNCTIONS
***************************************************************************/
/**************************************************************************/
/*!
@brief Sets up the HW
*/
/**************************************************************************/
bool Adafruit_BNO055::begin(adafruit_bno055_opmode_t mode)
{
/* Enable I2C */
Wire.begin();
// BNO055 clock stretches for 500us or more!
#ifdef ESP8266
Wire.setClockStretchLimit(1000); // Allow for 1000us of clock stretching
#endif
/* Make sure we have the right device */
uint8_t id = read8(BNO055_CHIP_ID_ADDR);
if(id != BNO055_ID)
{
delay(1000); // hold on for boot
id = read8(BNO055_CHIP_ID_ADDR);
if(id != BNO055_ID) {
return false; // still not? ok bail
}
}
/* Switch to config mode (just in case since this is the default) */
setMode(OPERATION_MODE_CONFIG);
/* Reset */
write8(BNO055_SYS_TRIGGER_ADDR, 0x20);
while (read8(BNO055_CHIP_ID_ADDR) != BNO055_ID)
{
delay(10);
}
delay(50);
/* Set to normal power mode */
write8(BNO055_PWR_MODE_ADDR, POWER_MODE_NORMAL);
delay(10);
write8(BNO055_PAGE_ID_ADDR, 0);
/* Set the output units */
/*
uint8_t unitsel = (0 << 7) | // Orientation = Android
(0 << 4) | // Temperature = Celsius
(0 << 2) | // Euler = Degrees
(1 << 1) | // Gyro = Rads
(0 << 0); // Accelerometer = m/s^2
write8(BNO055_UNIT_SEL_ADDR, unitsel);
*/
/* Configure axis mapping (see section 3.4) */
/**/
write8(BNO055_AXIS_MAP_CONFIG_ADDR, REMAP_CONFIG_P7); // P0-P7, Default is P1
delay(10);
write8(BNO055_AXIS_MAP_SIGN_ADDR, REMAP_SIGN_P7); // P0-P7, Default is P1
delay(10);
/**/
write8(BNO055_SYS_TRIGGER_ADDR, 0x0);
delay(10);
/* Set the requested operating mode (see section 3.3) */
setMode(mode);
delay(20);
return true;
}
/**************************************************************************/
/*!
@brief Puts the chip in the specified operating mode
*/
/**************************************************************************/
void Adafruit_BNO055::setMode(adafruit_bno055_opmode_t mode)
{
_mode = mode;
write8(BNO055_OPR_MODE_ADDR, _mode);
delay(30);
}
/**************************************************************************/
/*!
@brief Changes the chip's axis remap
*/
/**************************************************************************/
void Adafruit_BNO055::setAxisRemap( adafruit_bno055_axis_remap_config_t remapcode )
{
adafruit_bno055_opmode_t modeback = _mode;
setMode(OPERATION_MODE_CONFIG);
delay(25);
write8(BNO055_AXIS_MAP_CONFIG_ADDR, remapcode);
delay(10);
/* Set the requested operating mode (see section 3.3) */
setMode(modeback);
delay(20);
}
/**************************************************************************/
/*!
@brief Changes the chip's axis signs
*/
/**************************************************************************/
void Adafruit_BNO055::setAxisSign( adafruit_bno055_axis_remap_sign_t remapsign )
{
adafruit_bno055_opmode_t modeback = _mode;
setMode(OPERATION_MODE_CONFIG);
delay(25);
write8(BNO055_AXIS_MAP_SIGN_ADDR, remapsign);
delay(10);
/* Set the requested operating mode (see section 3.3) */
setMode(modeback);
delay(20);
}
/**************************************************************************/
/*!
@brief Use the external 32.768KHz crystal
*/
/**************************************************************************/
void Adafruit_BNO055::setExtCrystalUse(boolean usextal)
{
adafruit_bno055_opmode_t modeback = _mode;
/* Switch to config mode (just in case since this is the default) */
setMode(OPERATION_MODE_CONFIG);
delay(25);
write8(BNO055_PAGE_ID_ADDR, 0);
if (usextal) {
write8(BNO055_SYS_TRIGGER_ADDR, 0x80);
} else {
write8(BNO055_SYS_TRIGGER_ADDR, 0x00);
}
delay(10);
/* Set the requested operating mode (see section 3.3) */
setMode(modeback);
delay(20);
}
/**************************************************************************/
/*!
@brief Gets the latest system status info
*/
/**************************************************************************/
void Adafruit_BNO055::getSystemStatus(uint8_t *system_status, uint8_t *self_test_result, uint8_t *system_error)
{
write8(BNO055_PAGE_ID_ADDR, 0);
/* System Status (see section 4.3.58)
---------------------------------
0 = Idle
1 = System Error
2 = Initializing Peripherals
3 = System Iniitalization
4 = Executing Self-Test
5 = Sensor fusio algorithm running
6 = System running without fusion algorithms */
if (system_status != 0)
*system_status = read8(BNO055_SYS_STAT_ADDR);
/* Self Test Results (see section )
--------------------------------
1 = test passed, 0 = test failed
Bit 0 = Accelerometer self test
Bit 1 = Magnetometer self test
Bit 2 = Gyroscope self test
Bit 3 = MCU self test
0x0F = all good! */
if (self_test_result != 0)
*self_test_result = read8(BNO055_SELFTEST_RESULT_ADDR);
/* System Error (see section 4.3.59)
---------------------------------
0 = No error
1 = Peripheral initialization error
2 = System initialization error
3 = Self test result failed
4 = Register map value out of range
5 = Register map address out of range
6 = Register map write error
7 = BNO low power mode not available for selected operat ion mode
8 = Accelerometer power mode not available
9 = Fusion algorithm configuration error
A = Sensor configuration error */
if (system_error != 0)
*system_error = read8(BNO055_SYS_ERR_ADDR);
delay(200);
}
/**************************************************************************/
/*!
@brief Gets the chip revision numbers
*/
/**************************************************************************/
void Adafruit_BNO055::getRevInfo(adafruit_bno055_rev_info_t* info)
{
uint8_t a, b;
memset(info, 0, sizeof(adafruit_bno055_rev_info_t));
/* Check the accelerometer revision */
info->accel_rev = read8(BNO055_ACCEL_REV_ID_ADDR);
/* Check the magnetometer revision */
info->mag_rev = read8(BNO055_MAG_REV_ID_ADDR);
/* Check the gyroscope revision */
info->gyro_rev = read8(BNO055_GYRO_REV_ID_ADDR);
/* Check the SW revision */
info->bl_rev = read8(BNO055_BL_REV_ID_ADDR);
a = read8(BNO055_SW_REV_ID_LSB_ADDR);
b = read8(BNO055_SW_REV_ID_MSB_ADDR);
info->sw_rev = (((uint16_t)b) << 8) | ((uint16_t)a);
}
/**************************************************************************/
/*!
@brief Gets current calibration state. Each value should be a uint8_t
pointer and it will be set to 0 if not calibrated and 3 if
fully calibrated.
*/
/**************************************************************************/
void Adafruit_BNO055::getCalibration(uint8_t* sys, uint8_t* gyro, uint8_t* accel, uint8_t* mag) {
uint8_t calData = read8(BNO055_CALIB_STAT_ADDR);
if (sys != NULL) {
*sys = (calData >> 6) & 0x03;
}
if (gyro != NULL) {
*gyro = (calData >> 4) & 0x03;
}
if (accel != NULL) {
*accel = (calData >> 2) & 0x03;
}
if (mag != NULL) {
*mag = calData & 0x03;
}
}
/**************************************************************************/
/*!
@brief Gets the temperature in degrees celsius
*/
/**************************************************************************/
int8_t Adafruit_BNO055::getTemp(void)
{
int8_t temp = (int8_t)(read8(BNO055_TEMP_ADDR));
return temp;
}
/**************************************************************************/
/*!
@brief Gets a vector reading from the specified source
*/
/**************************************************************************/
imu::Vector<3> Adafruit_BNO055::getVector(adafruit_vector_type_t vector_type)
{
imu::Vector<3> xyz;
uint8_t buffer[6];
memset (buffer, 0, 6);
int16_t x, y, z;
x = y = z = 0;
/* Read vector data (6 bytes) */
readLen((adafruit_bno055_reg_t)vector_type, buffer, 6);
x = ((int16_t)buffer[0]) | (((int16_t)buffer[1]) << 8);
y = ((int16_t)buffer[2]) | (((int16_t)buffer[3]) << 8);
z = ((int16_t)buffer[4]) | (((int16_t)buffer[5]) << 8);
/* Convert the value to an appropriate range (section 3.6.4) */
/* and assign the value to the Vector type */
switch(vector_type)
{
case VECTOR_MAGNETOMETER:
/* 1uT = 16 LSB */
xyz[0] = ((double)x)/16.0;
xyz[1] = ((double)y)/16.0;
xyz[2] = ((double)z)/16.0;
break;
case VECTOR_GYROSCOPE:
/* 1dps = 16 LSB */
xyz[0] = ((double)x)/16.0;
xyz[1] = ((double)y)/16.0;
xyz[2] = ((double)z)/16.0;
break;
case VECTOR_EULER:
/* 1 degree = 16 LSB */
xyz[0] = ((double)x)/16.0;
xyz[1] = ((double)y)/16.0;
xyz[2] = ((double)z)/16.0;
break;
case VECTOR_ACCELEROMETER:
case VECTOR_LINEARACCEL:
case VECTOR_GRAVITY:
/* 1m/s^2 = 100 LSB */
xyz[0] = ((double)x)/100.0;
xyz[1] = ((double)y)/100.0;
xyz[2] = ((double)z)/100.0;
break;
}
return xyz;
}
/**************************************************************************/
/*!
@brief Gets a quaternion reading from the specified source
*/
/**************************************************************************/
imu::Quaternion Adafruit_BNO055::getQuat(void)
{
uint8_t buffer[8];
memset (buffer, 0, 8);
int16_t x, y, z, w;
x = y = z = w = 0;
/* Read quat data (8 bytes) */
readLen(BNO055_QUATERNION_DATA_W_LSB_ADDR, buffer, 8);
w = (((uint16_t)buffer[1]) << 8) | ((uint16_t)buffer[0]);
x = (((uint16_t)buffer[3]) << 8) | ((uint16_t)buffer[2]);
y = (((uint16_t)buffer[5]) << 8) | ((uint16_t)buffer[4]);
z = (((uint16_t)buffer[7]) << 8) | ((uint16_t)buffer[6]);
/* Assign to Quaternion */
/* See http://ae-bst.resource.bosch.com/media/products/dokumente/bno055/BST_BNO055_DS000_12~1.pdf
3.6.5.5 Orientation (Quaternion) */
const double scale = (1.0 / (1<<14));
imu::Quaternion quat(scale * w, scale * x, scale * y, scale * z);
return quat;
}
/**************************************************************************/
/*!
@brief Provides the sensor_t data for this sensor
*/
/**************************************************************************/
void Adafruit_BNO055::getSensor(sensor_t *sensor)
{
/* Clear the sensor_t object */
memset(sensor, 0, sizeof(sensor_t));
/* Insert the sensor name in the fixed length char array */
strncpy (sensor->name, "BNO055", sizeof(sensor->name) - 1);
sensor->name[sizeof(sensor->name)- 1] = 0;
sensor->version = 1;
sensor->sensor_id = _sensorID;
sensor->type = SENSOR_TYPE_ORIENTATION;
sensor->min_delay = 0;
sensor->max_value = 0.0F;
sensor->min_value = 0.0F;
sensor->resolution = 0.01F;
}
/**************************************************************************/
/*!
@brief Reads the sensor and returns the data as a sensors_event_t
*/
/**************************************************************************/
bool Adafruit_BNO055::getEvent(sensors_event_t *event)
{
/* Clear the event */
memset(event, 0, sizeof(sensors_event_t));
event->version = sizeof(sensors_event_t);
event->sensor_id = _sensorID;
event->type = SENSOR_TYPE_ORIENTATION;
event->timestamp = millis();
/* Get a Euler angle sample for orientation */
imu::Vector<3> euler = getVector(Adafruit_BNO055::VECTOR_EULER);
event->orientation.x = euler.x();
event->orientation.y = euler.y();
event->orientation.z = euler.z();
return true;
}
/**************************************************************************/
/*!
@brief Reads the sensor's offset registers into a byte array
*/
/**************************************************************************/
bool Adafruit_BNO055::getSensorOffsets(uint8_t* calibData)
{
if (isFullyCalibrated())
{
adafruit_bno055_opmode_t lastMode = _mode;
setMode(OPERATION_MODE_CONFIG);
readLen(ACCEL_OFFSET_X_LSB_ADDR, calibData, NUM_BNO055_OFFSET_REGISTERS);
setMode(lastMode);
return true;
}
return false;
}
/**************************************************************************/
/*!
@brief Reads the sensor's offset registers into an offset struct
*/
/**************************************************************************/
bool Adafruit_BNO055::getSensorOffsets(adafruit_bno055_offsets_t &offsets_type)
{
if (isFullyCalibrated())
{
adafruit_bno055_opmode_t lastMode = _mode;
setMode(OPERATION_MODE_CONFIG);
delay(25);
/* Accel offset range depends on the G-range:
+/-2g = +/- 2000 mg
+/-4g = +/- 4000 mg
+/-8g = +/- 8000 mg
+/-1§g = +/- 16000 mg */
offsets_type.accel_offset_x = (read8(ACCEL_OFFSET_X_MSB_ADDR) << 8) | (read8(ACCEL_OFFSET_X_LSB_ADDR));
offsets_type.accel_offset_y = (read8(ACCEL_OFFSET_Y_MSB_ADDR) << 8) | (read8(ACCEL_OFFSET_Y_LSB_ADDR));
offsets_type.accel_offset_z = (read8(ACCEL_OFFSET_Z_MSB_ADDR) << 8) | (read8(ACCEL_OFFSET_Z_LSB_ADDR));
/* Magnetometer offset range = +/- 6400 LSB where 1uT = 16 LSB */
offsets_type.mag_offset_x = (read8(MAG_OFFSET_X_MSB_ADDR) << 8) | (read8(MAG_OFFSET_X_LSB_ADDR));
offsets_type.mag_offset_y = (read8(MAG_OFFSET_Y_MSB_ADDR) << 8) | (read8(MAG_OFFSET_Y_LSB_ADDR));
offsets_type.mag_offset_z = (read8(MAG_OFFSET_Z_MSB_ADDR) << 8) | (read8(MAG_OFFSET_Z_LSB_ADDR));
/* Gyro offset range depends on the DPS range:
2000 dps = +/- 32000 LSB
1000 dps = +/- 16000 LSB
500 dps = +/- 8000 LSB
250 dps = +/- 4000 LSB
125 dps = +/- 2000 LSB
... where 1 DPS = 16 LSB */
offsets_type.gyro_offset_x = (read8(GYRO_OFFSET_X_MSB_ADDR) << 8) | (read8(GYRO_OFFSET_X_LSB_ADDR));
offsets_type.gyro_offset_y = (read8(GYRO_OFFSET_Y_MSB_ADDR) << 8) | (read8(GYRO_OFFSET_Y_LSB_ADDR));
offsets_type.gyro_offset_z = (read8(GYRO_OFFSET_Z_MSB_ADDR) << 8) | (read8(GYRO_OFFSET_Z_LSB_ADDR));
/* Accelerometer radius = +/- 1000 LSB */
offsets_type.accel_radius = (read8(ACCEL_RADIUS_MSB_ADDR) << 8) | (read8(ACCEL_RADIUS_LSB_ADDR));
/* Magnetometer radius = +/- 960 LSB */
offsets_type.mag_radius = (read8(MAG_RADIUS_MSB_ADDR) << 8) | (read8(MAG_RADIUS_LSB_ADDR));
setMode(lastMode);
return true;
}
return false;
}
/**************************************************************************/
/*!
@brief Writes an array of calibration values to the sensor's offset registers
*/
/**************************************************************************/
void Adafruit_BNO055::setSensorOffsets(const uint8_t* calibData)
{
adafruit_bno055_opmode_t lastMode = _mode;
setMode(OPERATION_MODE_CONFIG);
delay(25);
/* Note: Configuration will take place only when user writes to the last
byte of each config data pair (ex. ACCEL_OFFSET_Z_MSB_ADDR, etc.).
Therefore the last byte must be written whenever the user wants to
changes the configuration. */
/* A writeLen() would make this much cleaner */
write8(ACCEL_OFFSET_X_LSB_ADDR, calibData[0]);
write8(ACCEL_OFFSET_X_MSB_ADDR, calibData[1]);
write8(ACCEL_OFFSET_Y_LSB_ADDR, calibData[2]);
write8(ACCEL_OFFSET_Y_MSB_ADDR, calibData[3]);
write8(ACCEL_OFFSET_Z_LSB_ADDR, calibData[4]);
write8(ACCEL_OFFSET_Z_MSB_ADDR, calibData[5]);
write8(MAG_OFFSET_X_LSB_ADDR, calibData[6]);
write8(MAG_OFFSET_X_MSB_ADDR, calibData[7]);
write8(MAG_OFFSET_Y_LSB_ADDR, calibData[8]);
write8(MAG_OFFSET_Y_MSB_ADDR, calibData[9]);
write8(MAG_OFFSET_Z_LSB_ADDR, calibData[10]);
write8(MAG_OFFSET_Z_MSB_ADDR, calibData[11]);
write8(GYRO_OFFSET_X_LSB_ADDR, calibData[12]);
write8(GYRO_OFFSET_X_MSB_ADDR, calibData[13]);
write8(GYRO_OFFSET_Y_LSB_ADDR, calibData[14]);
write8(GYRO_OFFSET_Y_MSB_ADDR, calibData[15]);
write8(GYRO_OFFSET_Z_LSB_ADDR, calibData[16]);
write8(GYRO_OFFSET_Z_MSB_ADDR, calibData[17]);
write8(ACCEL_RADIUS_LSB_ADDR, calibData[18]);
write8(ACCEL_RADIUS_MSB_ADDR, calibData[19]);
write8(MAG_RADIUS_LSB_ADDR, calibData[20]);
write8(MAG_RADIUS_MSB_ADDR, calibData[21]);
setMode(lastMode);
}
/**************************************************************************/
/*!
@brief Writes to the sensor's offset registers from an offset struct
*/
/**************************************************************************/
void Adafruit_BNO055::setSensorOffsets(const adafruit_bno055_offsets_t &offsets_type)
{
adafruit_bno055_opmode_t lastMode = _mode;
setMode(OPERATION_MODE_CONFIG);
delay(25);
/* Note: Configuration will take place only when user writes to the last
byte of each config data pair (ex. ACCEL_OFFSET_Z_MSB_ADDR, etc.).
Therefore the last byte must be written whenever the user wants to
changes the configuration. */
write8(ACCEL_OFFSET_X_LSB_ADDR, (offsets_type.accel_offset_x) & 0x0FF);
write8(ACCEL_OFFSET_X_MSB_ADDR, (offsets_type.accel_offset_x >> 8) & 0x0FF);
write8(ACCEL_OFFSET_Y_LSB_ADDR, (offsets_type.accel_offset_y) & 0x0FF);
write8(ACCEL_OFFSET_Y_MSB_ADDR, (offsets_type.accel_offset_y >> 8) & 0x0FF);
write8(ACCEL_OFFSET_Z_LSB_ADDR, (offsets_type.accel_offset_z) & 0x0FF);
write8(ACCEL_OFFSET_Z_MSB_ADDR, (offsets_type.accel_offset_z >> 8) & 0x0FF);
write8(MAG_OFFSET_X_LSB_ADDR, (offsets_type.mag_offset_x) & 0x0FF);
write8(MAG_OFFSET_X_MSB_ADDR, (offsets_type.mag_offset_x >> 8) & 0x0FF);
write8(MAG_OFFSET_Y_LSB_ADDR, (offsets_type.mag_offset_y) & 0x0FF);
write8(MAG_OFFSET_Y_MSB_ADDR, (offsets_type.mag_offset_y >> 8) & 0x0FF);
write8(MAG_OFFSET_Z_LSB_ADDR, (offsets_type.mag_offset_z) & 0x0FF);
write8(MAG_OFFSET_Z_MSB_ADDR, (offsets_type.mag_offset_z >> 8) & 0x0FF);
write8(GYRO_OFFSET_X_LSB_ADDR, (offsets_type.gyro_offset_x) & 0x0FF);
write8(GYRO_OFFSET_X_MSB_ADDR, (offsets_type.gyro_offset_x >> 8) & 0x0FF);
write8(GYRO_OFFSET_Y_LSB_ADDR, (offsets_type.gyro_offset_y) & 0x0FF);
write8(GYRO_OFFSET_Y_MSB_ADDR, (offsets_type.gyro_offset_y >> 8) & 0x0FF);
write8(GYRO_OFFSET_Z_LSB_ADDR, (offsets_type.gyro_offset_z) & 0x0FF);
write8(GYRO_OFFSET_Z_MSB_ADDR, (offsets_type.gyro_offset_z >> 8) & 0x0FF);
write8(ACCEL_RADIUS_LSB_ADDR, (offsets_type.accel_radius) & 0x0FF);
write8(ACCEL_RADIUS_MSB_ADDR, (offsets_type.accel_radius >> 8) & 0x0FF);
write8(MAG_RADIUS_LSB_ADDR, (offsets_type.mag_radius) & 0x0FF);
write8(MAG_RADIUS_MSB_ADDR, (offsets_type.mag_radius >> 8) & 0x0FF);
setMode(lastMode);
}
/**************************************************************************/
/*!
@brief Checks of all cal status values are set to 3 (fully calibrated)
*/
/**************************************************************************/
bool Adafruit_BNO055::isFullyCalibrated(void)
{
uint8_t system, gyro, accel, mag;
getCalibration(&system, &gyro, &accel, &mag);
if (system < 3 || gyro < 3 || accel < 3 || mag < 3)
return false;
return true;
}
/***************************************************************************
PRIVATE FUNCTIONS
***************************************************************************/
/**************************************************************************/
/*!
@brief Writes an 8 bit value over I2C
*/
/**************************************************************************/
bool Adafruit_BNO055::write8(adafruit_bno055_reg_t reg, byte value)
{
Wire.beginTransmission(_address);
#if ARDUINO >= 100
Wire.write((uint8_t)reg);
Wire.write((uint8_t)value);
#else
Wire.send(reg);
Wire.send(value);
#endif
Wire.endTransmission();
/* ToDo: Check for error! */
return true;
}
/**************************************************************************/
/*!
@brief Reads an 8 bit value over I2C
*/
/**************************************************************************/
byte Adafruit_BNO055::read8(adafruit_bno055_reg_t reg )
{
byte value = 0;
Wire.beginTransmission(_address);
#if ARDUINO >= 100
Wire.write((uint8_t)reg);
#else
Wire.send(reg);
#endif
Wire.endTransmission();
Wire.requestFrom(_address, (byte)1);
#if ARDUINO >= 100
value = Wire.read();
#else
value = Wire.receive();
#endif
return value;
}
/**************************************************************************/
/*!
@brief Reads the specified number of bytes over I2C
*/
/**************************************************************************/
bool Adafruit_BNO055::readLen(adafruit_bno055_reg_t reg, byte * buffer, uint8_t len)
{
Wire.beginTransmission(_address);
#if ARDUINO >= 100
Wire.write((uint8_t)reg);
#else
Wire.send(reg);
#endif
Wire.endTransmission();
Wire.requestFrom(_address, (byte)len);
for (uint8_t i = 0; i < len; i++)
{
#if ARDUINO >= 100
buffer[i] = Wire.read();
#else
buffer[i] = Wire.receive();
#endif
}
/* ToDo: Check for errors! */
return true;
}

326
lib/Adafruit_BNO055/Adafruit_BNO055.h Executable file
View File

@ -0,0 +1,326 @@
/***************************************************************************
This is a library for the BNO055 orientation sensor
Designed specifically to work with the Adafruit BNO055 Breakout.
Pick one up today in the adafruit shop!
------> http://www.adafruit.com/products
These sensors use I2C to communicate, 2 pins are required to interface.
Adafruit invests time and resources providing this open source code,
please support Adafruit andopen-source hardware by purchasing products
from Adafruit!
Written by KTOWN for Adafruit Industries.
MIT license, all text above must be included in any redistribution
***************************************************************************/
#ifndef __ADAFRUIT_BNO055_H__
#define __ADAFRUIT_BNO055_H__
#if (ARDUINO >= 100)
#include "Arduino.h"
#else
#include "WProgram.h"
#endif
#ifdef __AVR_ATtiny85__
#include <TinyWireM.h>
#define Wire TinyWireM
#else
#include <Wire.h>
#endif
#include <Adafruit_Sensor.h>
#include <utility/imumaths.h>
#define BNO055_ADDRESS_A (0x28)
#define BNO055_ADDRESS_B (0x29)
#define BNO055_ID (0xA0)
#define NUM_BNO055_OFFSET_REGISTERS (22)
typedef struct
{
int16_t accel_offset_x;
int16_t accel_offset_y;
int16_t accel_offset_z;
int16_t mag_offset_x;
int16_t mag_offset_y;
int16_t mag_offset_z;
int16_t gyro_offset_x;
int16_t gyro_offset_y;
int16_t gyro_offset_z;
int16_t accel_radius;
int16_t mag_radius;
} adafruit_bno055_offsets_t;
class Adafruit_BNO055 : public Adafruit_Sensor
{
public:
typedef enum
{
/* Page id register definition */
BNO055_PAGE_ID_ADDR = 0X07,
/* PAGE0 REGISTER DEFINITION START*/
BNO055_CHIP_ID_ADDR = 0x00,
BNO055_ACCEL_REV_ID_ADDR = 0x01,
BNO055_MAG_REV_ID_ADDR = 0x02,
BNO055_GYRO_REV_ID_ADDR = 0x03,
BNO055_SW_REV_ID_LSB_ADDR = 0x04,
BNO055_SW_REV_ID_MSB_ADDR = 0x05,
BNO055_BL_REV_ID_ADDR = 0X06,
/* Accel data register */
BNO055_ACCEL_DATA_X_LSB_ADDR = 0X08,
BNO055_ACCEL_DATA_X_MSB_ADDR = 0X09,
BNO055_ACCEL_DATA_Y_LSB_ADDR = 0X0A,
BNO055_ACCEL_DATA_Y_MSB_ADDR = 0X0B,
BNO055_ACCEL_DATA_Z_LSB_ADDR = 0X0C,
BNO055_ACCEL_DATA_Z_MSB_ADDR = 0X0D,
/* Mag data register */
BNO055_MAG_DATA_X_LSB_ADDR = 0X0E,
BNO055_MAG_DATA_X_MSB_ADDR = 0X0F,
BNO055_MAG_DATA_Y_LSB_ADDR = 0X10,
BNO055_MAG_DATA_Y_MSB_ADDR = 0X11,
BNO055_MAG_DATA_Z_LSB_ADDR = 0X12,
BNO055_MAG_DATA_Z_MSB_ADDR = 0X13,
/* Gyro data registers */
BNO055_GYRO_DATA_X_LSB_ADDR = 0X14,
BNO055_GYRO_DATA_X_MSB_ADDR = 0X15,
BNO055_GYRO_DATA_Y_LSB_ADDR = 0X16,
BNO055_GYRO_DATA_Y_MSB_ADDR = 0X17,
BNO055_GYRO_DATA_Z_LSB_ADDR = 0X18,
BNO055_GYRO_DATA_Z_MSB_ADDR = 0X19,
/* Euler data registers */
BNO055_EULER_H_LSB_ADDR = 0X1A,
BNO055_EULER_H_MSB_ADDR = 0X1B,
BNO055_EULER_R_LSB_ADDR = 0X1C,
BNO055_EULER_R_MSB_ADDR = 0X1D,
BNO055_EULER_P_LSB_ADDR = 0X1E,
BNO055_EULER_P_MSB_ADDR = 0X1F,
/* Quaternion data registers */
BNO055_QUATERNION_DATA_W_LSB_ADDR = 0X20,
BNO055_QUATERNION_DATA_W_MSB_ADDR = 0X21,
BNO055_QUATERNION_DATA_X_LSB_ADDR = 0X22,
BNO055_QUATERNION_DATA_X_MSB_ADDR = 0X23,
BNO055_QUATERNION_DATA_Y_LSB_ADDR = 0X24,
BNO055_QUATERNION_DATA_Y_MSB_ADDR = 0X25,
BNO055_QUATERNION_DATA_Z_LSB_ADDR = 0X26,
BNO055_QUATERNION_DATA_Z_MSB_ADDR = 0X27,
/* Linear acceleration data registers */
BNO055_LINEAR_ACCEL_DATA_X_LSB_ADDR = 0X28,
BNO055_LINEAR_ACCEL_DATA_X_MSB_ADDR = 0X29,
BNO055_LINEAR_ACCEL_DATA_Y_LSB_ADDR = 0X2A,
BNO055_LINEAR_ACCEL_DATA_Y_MSB_ADDR = 0X2B,
BNO055_LINEAR_ACCEL_DATA_Z_LSB_ADDR = 0X2C,
BNO055_LINEAR_ACCEL_DATA_Z_MSB_ADDR = 0X2D,
/* Gravity data registers */
BNO055_GRAVITY_DATA_X_LSB_ADDR = 0X2E,
BNO055_GRAVITY_DATA_X_MSB_ADDR = 0X2F,
BNO055_GRAVITY_DATA_Y_LSB_ADDR = 0X30,
BNO055_GRAVITY_DATA_Y_MSB_ADDR = 0X31,
BNO055_GRAVITY_DATA_Z_LSB_ADDR = 0X32,
BNO055_GRAVITY_DATA_Z_MSB_ADDR = 0X33,
/* Temperature data register */
BNO055_TEMP_ADDR = 0X34,
/* Status registers */
BNO055_CALIB_STAT_ADDR = 0X35,
BNO055_SELFTEST_RESULT_ADDR = 0X36,
BNO055_INTR_STAT_ADDR = 0X37,
BNO055_SYS_CLK_STAT_ADDR = 0X38,
BNO055_SYS_STAT_ADDR = 0X39,
BNO055_SYS_ERR_ADDR = 0X3A,
/* Unit selection register */
BNO055_UNIT_SEL_ADDR = 0X3B,
BNO055_DATA_SELECT_ADDR = 0X3C,
/* Mode registers */
BNO055_OPR_MODE_ADDR = 0X3D,
BNO055_PWR_MODE_ADDR = 0X3E,
BNO055_SYS_TRIGGER_ADDR = 0X3F,
BNO055_TEMP_SOURCE_ADDR = 0X40,
/* Axis remap registers */
BNO055_AXIS_MAP_CONFIG_ADDR = 0X41,
BNO055_AXIS_MAP_SIGN_ADDR = 0X42,
/* SIC registers */
BNO055_SIC_MATRIX_0_LSB_ADDR = 0X43,
BNO055_SIC_MATRIX_0_MSB_ADDR = 0X44,
BNO055_SIC_MATRIX_1_LSB_ADDR = 0X45,
BNO055_SIC_MATRIX_1_MSB_ADDR = 0X46,
BNO055_SIC_MATRIX_2_LSB_ADDR = 0X47,
BNO055_SIC_MATRIX_2_MSB_ADDR = 0X48,
BNO055_SIC_MATRIX_3_LSB_ADDR = 0X49,
BNO055_SIC_MATRIX_3_MSB_ADDR = 0X4A,
BNO055_SIC_MATRIX_4_LSB_ADDR = 0X4B,
BNO055_SIC_MATRIX_4_MSB_ADDR = 0X4C,
BNO055_SIC_MATRIX_5_LSB_ADDR = 0X4D,
BNO055_SIC_MATRIX_5_MSB_ADDR = 0X4E,
BNO055_SIC_MATRIX_6_LSB_ADDR = 0X4F,
BNO055_SIC_MATRIX_6_MSB_ADDR = 0X50,
BNO055_SIC_MATRIX_7_LSB_ADDR = 0X51,
BNO055_SIC_MATRIX_7_MSB_ADDR = 0X52,
BNO055_SIC_MATRIX_8_LSB_ADDR = 0X53,
BNO055_SIC_MATRIX_8_MSB_ADDR = 0X54,
/* Accelerometer Offset registers */
ACCEL_OFFSET_X_LSB_ADDR = 0X55,
ACCEL_OFFSET_X_MSB_ADDR = 0X56,
ACCEL_OFFSET_Y_LSB_ADDR = 0X57,
ACCEL_OFFSET_Y_MSB_ADDR = 0X58,
ACCEL_OFFSET_Z_LSB_ADDR = 0X59,
ACCEL_OFFSET_Z_MSB_ADDR = 0X5A,
/* Magnetometer Offset registers */
MAG_OFFSET_X_LSB_ADDR = 0X5B,
MAG_OFFSET_X_MSB_ADDR = 0X5C,
MAG_OFFSET_Y_LSB_ADDR = 0X5D,
MAG_OFFSET_Y_MSB_ADDR = 0X5E,
MAG_OFFSET_Z_LSB_ADDR = 0X5F,
MAG_OFFSET_Z_MSB_ADDR = 0X60,
/* Gyroscope Offset register s*/
GYRO_OFFSET_X_LSB_ADDR = 0X61,
GYRO_OFFSET_X_MSB_ADDR = 0X62,
GYRO_OFFSET_Y_LSB_ADDR = 0X63,
GYRO_OFFSET_Y_MSB_ADDR = 0X64,
GYRO_OFFSET_Z_LSB_ADDR = 0X65,
GYRO_OFFSET_Z_MSB_ADDR = 0X66,
/* Radius registers */
ACCEL_RADIUS_LSB_ADDR = 0X67,
ACCEL_RADIUS_MSB_ADDR = 0X68,
MAG_RADIUS_LSB_ADDR = 0X69,
MAG_RADIUS_MSB_ADDR = 0X6A
} adafruit_bno055_reg_t;
typedef enum
{
POWER_MODE_NORMAL = 0X00,
POWER_MODE_LOWPOWER = 0X01,
POWER_MODE_SUSPEND = 0X02
} adafruit_bno055_powermode_t;
typedef enum
{
/* Operation mode settings*/
OPERATION_MODE_CONFIG = 0X00,
OPERATION_MODE_ACCONLY = 0X01,
OPERATION_MODE_MAGONLY = 0X02,
OPERATION_MODE_GYRONLY = 0X03,
OPERATION_MODE_ACCMAG = 0X04,
OPERATION_MODE_ACCGYRO = 0X05,
OPERATION_MODE_MAGGYRO = 0X06,
OPERATION_MODE_AMG = 0X07,
OPERATION_MODE_IMUPLUS = 0X08,
OPERATION_MODE_COMPASS = 0X09,
OPERATION_MODE_M4G = 0X0A,
OPERATION_MODE_NDOF_FMC_OFF = 0X0B,
OPERATION_MODE_NDOF = 0X0C
} adafruit_bno055_opmode_t;
typedef enum
{
REMAP_CONFIG_P0 = 0x21,
REMAP_CONFIG_P1 = 0x24, // default
REMAP_CONFIG_P2 = 0x24,
REMAP_CONFIG_P3 = 0x21,
REMAP_CONFIG_P4 = 0x24,
REMAP_CONFIG_P5 = 0x21,
REMAP_CONFIG_P6 = 0x21,
REMAP_CONFIG_P7 = 0x24
} adafruit_bno055_axis_remap_config_t;
typedef enum
{
REMAP_SIGN_P0 = 0x04,
REMAP_SIGN_P1 = 0x00, // default
REMAP_SIGN_P2 = 0x06,
REMAP_SIGN_P3 = 0x02,
REMAP_SIGN_P4 = 0x03,
REMAP_SIGN_P5 = 0x01,
REMAP_SIGN_P6 = 0x07,
REMAP_SIGN_P7 = 0x05
} adafruit_bno055_axis_remap_sign_t;
typedef struct
{
uint8_t accel_rev;
uint8_t mag_rev;
uint8_t gyro_rev;
uint16_t sw_rev;
uint8_t bl_rev;
} adafruit_bno055_rev_info_t;
typedef enum
{
VECTOR_ACCELEROMETER = BNO055_ACCEL_DATA_X_LSB_ADDR,
VECTOR_MAGNETOMETER = BNO055_MAG_DATA_X_LSB_ADDR,
VECTOR_GYROSCOPE = BNO055_GYRO_DATA_X_LSB_ADDR,
VECTOR_EULER = BNO055_EULER_H_LSB_ADDR,
VECTOR_LINEARACCEL = BNO055_LINEAR_ACCEL_DATA_X_LSB_ADDR,
VECTOR_GRAVITY = BNO055_GRAVITY_DATA_X_LSB_ADDR
} adafruit_vector_type_t;
#if defined (ARDUINO_SAMD_ZERO) && ! (ARDUINO_SAMD_FEATHER_M0)
#error "On an arduino Zero, BNO055's ADR pin must be high. Fix that, then delete this line."
Adafruit_BNO055 ( int32_t sensorID = -1, uint8_t address = BNO055_ADDRESS_B );
#else
Adafruit_BNO055 ( int32_t sensorID = -1, uint8_t address = BNO055_ADDRESS_A );
#endif
bool begin ( adafruit_bno055_opmode_t mode = OPERATION_MODE_NDOF );
void setMode ( adafruit_bno055_opmode_t mode );
void setAxisRemap ( adafruit_bno055_axis_remap_config_t remapcode );
void setAxisSign ( adafruit_bno055_axis_remap_sign_t remapsign );
void getRevInfo ( adafruit_bno055_rev_info_t* );
void displayRevInfo ( void );
void setExtCrystalUse ( boolean usextal );
void getSystemStatus ( uint8_t *system_status,
uint8_t *self_test_result,
uint8_t *system_error);
void displaySystemStatus ( void );
void getCalibration ( uint8_t* system, uint8_t* gyro, uint8_t* accel, uint8_t* mag);
imu::Vector<3> getVector ( adafruit_vector_type_t vector_type );
imu::Quaternion getQuat ( void );
int8_t getTemp ( void );
/* Adafruit_Sensor implementation */
bool getEvent ( sensors_event_t* );
void getSensor ( sensor_t* );
/* Functions to deal with raw calibration data */
bool getSensorOffsets(uint8_t* calibData);
bool getSensorOffsets(adafruit_bno055_offsets_t &offsets_type);
void setSensorOffsets(const uint8_t* calibData);
void setSensorOffsets(const adafruit_bno055_offsets_t &offsets_type);
bool isFullyCalibrated(void);
private:
byte read8 ( adafruit_bno055_reg_t );
bool readLen ( adafruit_bno055_reg_t, byte* buffer, uint8_t len );
bool write8 ( adafruit_bno055_reg_t, byte value );
uint8_t _address;
int32_t _sensorID;
adafruit_bno055_opmode_t _mode;
};
#endif

47
lib/Adafruit_BNO055/OBJLoader/OBJLoader.txt Executable file
View File

@ -0,0 +1,47 @@
# UTF-8 supported.
# The name of your library as you want it formatted
name = OBJLoader
# List of authors. Links can be provided using the syntax [author name](url)
authorList = [Tatsuya Saito](http://saito-tatsuya.net/) and [Matt Ditton](http://thequietvoid.com/)
# A web page for your library, NOT a direct link to where to download it
url = http://code.google.com/p/saitoobjloader/
# The category of your library, must be one (or many) of the following:
# "3D" "Animation" "Compilations" "Data"
# "Fabrication" "Geometry" "GUI" "Hardware"
# "I/O" "Language" "Math" "Simulation"
# "Sound" "Utilities" "Typography" "Video & Vision"
#
# If a value other than those listed is used, your library will listed as "Other."
category = 3D
# A short sentence (or fragment) to summarize the library's function. This will be
# shown from inside the PDE when the library is being installed. Avoid repeating
# the name of your library here. Also, avoid saying anything redundant like
# mentioning that its a library. This should start with a capitalized letter, and
# end with a period.
sentence = .OBJ 3D model file loader
# Additional information suitable for the Processing website. The value of
# 'sentence' always will be prepended, so you should start by writing the
# second sentence here. If your library only works on certain operating systems,
# mention it here.
paragraph =
# Links in the 'sentence' and 'paragraph' attributes can be inserted using the
# same syntax as for authors. That is, [here is a link to Processing](http://processing.org/)
# A version number that increments once with each release. This
# is used to compare different versions of the same library, and
# check if an update is available. You should think of it as a
# counter, counting the total number of releases you've had.
version = 23 # This must be parsable as an int
# The version as the user will see it. If blank, the version attribute will be used here
prettyVersion = 0.99 # This is treated as a String

BIN
lib/Adafruit_BNO055/OBJLoader/OBJLoader.zip Executable file
View File

Binary file not shown.

26
lib/Adafruit_BNO055/README.md Executable file
View File

@ -0,0 +1,26 @@
# Adafruit Unified BNO055 Driver (AHRS/Orientation) #
This driver is for the Adafruit BNO055 Breakout (http://www.adafruit.com/products/2472),
and is based on Adafruit's Unified Sensor Library (Adafruit_Sensor).
To work with the Arduino Zero, the BNO055's ADR pin must be high, and an "#error" must be removed from the .h file.
## What is the Adafruit Unified Sensor Library? ##
The Adafruit Unified Sensor Library ([Adafruit_Sensor](https://github.com/adafruit/Adafruit_Sensor)) provides a common interface and data type for any supported sensor. It defines some basic information about the sensor (sensor limits, etc.), and returns standard SI units of a specific type and scale for each supported sensor type.
It provides a simple abstraction layer between your application and the actual sensor HW, allowing you to drop in any comparable sensor with only one or two lines of code to change in your project (essentially the constructor since the functions to read sensor data and get information about the sensor are defined in the base Adafruit_Sensor class).
This is imporant useful for two reasons:
1.) You can use the data right away because it's already converted to SI units that you understand and can compare, rather than meaningless values like 0..1023.
2.) Because SI units are standardised in the sensor library, you can also do quick sanity checks when working with new sensors, or drop in any comparable sensor if you need better sensitivity or if a lower cost unit becomes available, etc.
Light sensors will always report units in lux, gyroscopes will always report units in rad/s, etc. ... freeing you up to focus on the data, rather than digging through the datasheet to understand what the sensor's raw numbers really mean.
## About this Driver ##
Adafruit invests time and resources providing this open source code. Please support Adafruit and open-source hardware by purchasing products from Adafruit!
Written by Kevin (KTOWN) Townsend for Adafruit Industries.

129
lib/Adafruit_BNO055/examples/bunny/bunny.ino Executable file
View File

@ -0,0 +1,129 @@
#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_BNO055.h>
#include <utility/imumaths.h>
/* This driver uses the Adafruit unified sensor library (Adafruit_Sensor),
which provides a common 'type' for sensor data and some helper functions.
To use this driver you will also need to download the Adafruit_Sensor
library and include it in your libraries folder.
You should also assign a unique ID to this sensor for use with
the Adafruit Sensor API so that you can identify this particular
sensor in any data logs, etc. To assign a unique ID, simply
provide an appropriate value in the constructor below (12345
is used by default in this example).
Connections
===========
Connect SCL to analog 5
Connect SDA to analog 4
Connect VDD to 3.3-5V DC
Connect GROUND to common ground
History
=======
2015/MAR/03 - First release (KTOWN)
*/
/* Set the delay between fresh samples */
#define BNO055_SAMPLERATE_DELAY_MS (100)
Adafruit_BNO055 bno = Adafruit_BNO055(55);
/**************************************************************************/
/*
Displays some basic information on this sensor from the unified
sensor API sensor_t type (see Adafruit_Sensor for more information)
*/
/**************************************************************************/
void displaySensorDetails(void)
{
sensor_t sensor;
bno.getSensor(&sensor);
Serial.println("------------------------------------");
Serial.print ("Sensor: "); Serial.println(sensor.name);
Serial.print ("Driver Ver: "); Serial.println(sensor.version);
Serial.print ("Unique ID: "); Serial.println(sensor.sensor_id);
Serial.print ("Max Value: "); Serial.print(sensor.max_value); Serial.println(" xxx");
Serial.print ("Min Value: "); Serial.print(sensor.min_value); Serial.println(" xxx");
Serial.print ("Resolution: "); Serial.print(sensor.resolution); Serial.println(" xxx");
Serial.println("------------------------------------");
Serial.println("");
delay(500);
}
/**************************************************************************/
/*
Arduino setup function (automatically called at startup)
*/
/**************************************************************************/
void setup(void)
{
Serial.begin(115200);
Serial.println("Orientation Sensor Test"); Serial.println("");
/* Initialise the sensor */
if(!bno.begin())
{
/* There was a problem detecting the BNO055 ... check your connections */
Serial.print("Ooops, no BNO055 detected ... Check your wiring or I2C ADDR!");
while(1);
}
delay(1000);
/* Use external crystal for better accuracy */
bno.setExtCrystalUse(true);
/* Display some basic information on this sensor */
displaySensorDetails();
}
/**************************************************************************/
/*
Arduino loop function, called once 'setup' is complete (your own code
should go here)
*/
/**************************************************************************/
void loop(void)
{
/* Get a new sensor event */
sensors_event_t event;
bno.getEvent(&event);
/* Board layout:
+----------+
| *| RST PITCH ROLL HEADING
ADR |* *| SCL
INT |* *| SDA ^ /->
PS1 |* *| GND | |
PS0 |* *| 3VO Y Z--> \-X
| *| VIN
+----------+
*/
/* The processing sketch expects data as roll, pitch, heading */
Serial.print(F("Orientation: "));
Serial.print((float)event.orientation.x);
Serial.print(F(" "));
Serial.print((float)event.orientation.y);
Serial.print(F(" "));
Serial.print((float)event.orientation.z);
Serial.println(F(""));
/* Also send calibration data for each sensor. */
uint8_t sys, gyro, accel, mag = 0;
bno.getCalibration(&sys, &gyro, &accel, &mag);
Serial.print(F("Calibration: "));
Serial.print(sys, DEC);
Serial.print(F(" "));
Serial.print(gyro, DEC);
Serial.print(F(" "));
Serial.print(accel, DEC);
Serial.print(F(" "));
Serial.println(mag, DEC);
delay(BNO055_SAMPLERATE_DELAY_MS);
}

181
lib/Adafruit_BNO055/examples/bunny/processing/cuberotate/cuberotate.pde Executable file
View File

@ -0,0 +1,181 @@
import processing.serial.*;
import java.awt.datatransfer.*;
import java.awt.Toolkit;
import processing.opengl.*;
import saito.objloader.*;
import g4p_controls.*;
float roll = 0.0F;
float pitch = 0.0F;
float yaw = 0.0F;
float temp = 0.0F;
float alt = 0.0F;
OBJModel model;
// Serial port state.
Serial port;
final String serialConfigFile = "serialconfig.txt";
boolean printSerial = false;
// UI controls.
GPanel configPanel;
GDropList serialList;
GLabel serialLabel;
GLabel calLabel;
GCheckbox printSerialCheckbox;
void setup()
{
size(640, 480, OPENGL);
frameRate(30);
model = new OBJModel(this);
model.load("bunny.obj");
model.scale(20);
// Serial port setup.
// Grab list of serial ports and choose one that was persisted earlier or default to the first port.
int selectedPort = 0;
String[] availablePorts = Serial.list();
if (availablePorts == null) {
println("ERROR: No serial ports available!");
exit();
}
String[] serialConfig = loadStrings(serialConfigFile);
if (serialConfig != null && serialConfig.length > 0) {
String savedPort = serialConfig[0];
// Check if saved port is in available ports.
for (int i = 0; i < availablePorts.length; ++i) {
if (availablePorts[i].equals(savedPort)) {
selectedPort = i;
}
}
}
// Build serial config UI.
configPanel = new GPanel(this, 10, 10, width-20, 90, "Configuration (click to hide/show)");
serialLabel = new GLabel(this, 0, 20, 80, 25, "Serial port:");
configPanel.addControl(serialLabel);
serialList = new GDropList(this, 90, 20, 200, 200, 6);
serialList.setItems(availablePorts, selectedPort);
configPanel.addControl(serialList);
calLabel = new GLabel(this, 300, 20, 350, 25, "Calibration: Sys=? Gyro=? Accel=? Mag=?");
configPanel.addControl(calLabel);
printSerialCheckbox = new GCheckbox(this, 5, 50, 200, 20, "Print serial data");
printSerialCheckbox.setSelected(printSerial);
configPanel.addControl(printSerialCheckbox);
// Set serial port.
setSerialPort(serialList.getSelectedText());
}
void draw()
{
background(0,0,0);
// Set a new co-ordinate space
pushMatrix();
// Simple 3 point lighting for dramatic effect.
// Slightly red light in upper right, slightly blue light in upper left, and white light from behind.
pointLight(255, 200, 200, 400, 400, 500);
pointLight(200, 200, 255, -400, 400, 500);
pointLight(255, 255, 255, 0, 0, -500);
// Move bunny from 0,0 in upper left corner to roughly center of screen.
translate(300, 380, 0);
// Rotate shapes around the X/Y/Z axis (values in radians, 0..Pi*2)
//rotateZ(radians(roll));
//rotateX(radians(pitch)); // extrinsic rotation
//rotateY(radians(yaw));
float c1 = cos(radians(roll));
float s1 = sin(radians(roll));
float c2 = cos(radians(pitch)); // intrinsic rotation
float s2 = sin(radians(pitch));
float c3 = cos(radians(yaw));
float s3 = sin(radians(yaw));
applyMatrix( c2*c3, s1*s3+c1*c3*s2, c3*s1*s2-c1*s3, 0,
-s2, c1*c2, c2*s1, 0,
c2*s3, c1*s2*s3-c3*s1, c1*c3+s1*s2*s3, 0,
0, 0, 0, 1);
pushMatrix();
noStroke();
model.draw();
popMatrix();
popMatrix();
//print("draw");
}
void serialEvent(Serial p)
{
String incoming = p.readString();
if (printSerial) {
println(incoming);
}
if ((incoming.length() > 8))
{
String[] list = split(incoming, " ");
if ( (list.length > 0) && (list[0].equals("Orientation:")) )
{
roll = float(list[3]); // Roll = Z
pitch = float(list[2]); // Pitch = Y
yaw = float(list[1]); // Yaw/Heading = X
}
if ( (list.length > 0) && (list[0].equals("Alt:")) )
{
alt = float(list[1]);
}
if ( (list.length > 0) && (list[0].equals("Temp:")) )
{
temp = float(list[1]);
}
if ( (list.length > 0) && (list[0].equals("Calibration:")) )
{
int sysCal = int(list[1]);
int gyroCal = int(list[2]);
int accelCal = int(list[3]);
int magCal = int(trim(list[4]));
calLabel.setText("Calibration: Sys=" + sysCal + " Gyro=" + gyroCal + " Accel=" + accelCal + " Mag=" + magCal);
}
}
}
// Set serial port to desired value.
void setSerialPort(String portName) {
// Close the port if it's currently open.
if (port != null) {
port.stop();
}
try {
// Open port.
port = new Serial(this, portName, 115200);
port.bufferUntil('\n');
// Persist port in configuration.
saveStrings(serialConfigFile, new String[] { portName });
}
catch (RuntimeException ex) {
// Swallow error if port can't be opened, keep port closed.
port = null;
}
}
// UI event handlers
void handlePanelEvents(GPanel panel, GEvent event) {
// Panel events, do nothing.
}
void handleDropListEvents(GDropList list, GEvent event) {
// Drop list events, check if new serial port is selected.
if (list == serialList) {
setSerialPort(serialList.getSelectedText());
}
}
void handleToggleControlEvents(GToggleControl checkbox, GEvent event) {
// Checkbox toggle events, check if print events is toggled.
if (checkbox == printSerialCheckbox) {
printSerial = printSerialCheckbox.isSelected();
}
}

13
lib/Adafruit_BNO055/examples/bunny/processing/cuberotate/data/bunny.mtl Executable file
View File

@ -0,0 +1,13 @@
# 3ds Max Wavefront OBJ Exporter v0.94b - (c)2007 guruware
# File Created: 04.07.2010 10:41:39
newmtl Body
Ns 32
d 1
Tr 1
Tf 1 1 1
illum 2
Ka 0.0000 0.0000 0.0000
Kd 0.7412 0.4784 0.4765
Ks 0.3500 0.3500 0.6500

139352
lib/Adafruit_BNO055/examples/bunny/processing/cuberotate/data/bunny.obj Executable file
View File

File diff suppressed because it is too large Load Diff

1
lib/Adafruit_BNO055/examples/bunny/processing/cuberotate/serialconfig.txt Executable file
View File

@ -0,0 +1 @@
/dev/tty.usbmodem141121

110
lib/Adafruit_BNO055/examples/rawdata/rawdata.ino Executable file
View File

@ -0,0 +1,110 @@
#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_BNO055.h>
#include <utility/imumaths.h>
/* This driver reads raw data from the BNO055
Connections
===========
Connect SCL to analog 5
Connect SDA to analog 4
Connect VDD to 3.3V DC
Connect GROUND to common ground
History
=======
2015/MAR/03 - First release (KTOWN)
*/
/* Set the delay between fresh samples */
#define BNO055_SAMPLERATE_DELAY_MS (100)
Adafruit_BNO055 bno = Adafruit_BNO055();
/**************************************************************************/
/*
Arduino setup function (automatically called at startup)
*/
/**************************************************************************/
void setup(void)
{
Serial.begin(9600);
Serial.println("Orientation Sensor Raw Data Test"); Serial.println("");
/* Initialise the sensor */
if(!bno.begin())
{
/* There was a problem detecting the BNO055 ... check your connections */
Serial.print("Ooops, no BNO055 detected ... Check your wiring or I2C ADDR!");
while(1);
}
delay(1000);
/* Display the current temperature */
int8_t temp = bno.getTemp();
Serial.print("Current Temperature: ");
Serial.print(temp);
Serial.println(" C");
Serial.println("");
bno.setExtCrystalUse(true);
Serial.println("Calibration status values: 0=uncalibrated, 3=fully calibrated");
}
/**************************************************************************/
/*
Arduino loop function, called once 'setup' is complete (your own code
should go here)
*/
/**************************************************************************/
void loop(void)
{
// Possible vector values can be:
// - VECTOR_ACCELEROMETER - m/s^2
// - VECTOR_MAGNETOMETER - uT
// - VECTOR_GYROSCOPE - rad/s
// - VECTOR_EULER - degrees
// - VECTOR_LINEARACCEL - m/s^2
// - VECTOR_GRAVITY - m/s^2
imu::Vector<3> euler = bno.getVector(Adafruit_BNO055::VECTOR_EULER);
/* Display the floating point data */
Serial.print("X: ");
Serial.print(euler.x());
Serial.print(" Y: ");
Serial.print(euler.y());
Serial.print(" Z: ");
Serial.print(euler.z());
Serial.print("\t\t");
/*
// Quaternion data
imu::Quaternion quat = bno.getQuat();
Serial.print("qW: ");
Serial.print(quat.w(), 4);
Serial.print(" qX: ");
Serial.print(quat.y(), 4);
Serial.print(" qY: ");
Serial.print(quat.x(), 4);
Serial.print(" qZ: ");
Serial.print(quat.z(), 4);
Serial.print("\t\t");
*/
/* Display calibration status for each sensor. */
uint8_t system, gyro, accel, mag = 0;
bno.getCalibration(&system, &gyro, &accel, &mag);
Serial.print("CALIBRATION: Sys=");
Serial.print(system, DEC);
Serial.print(" Gyro=");
Serial.print(gyro, DEC);
Serial.print(" Accel=");
Serial.print(accel, DEC);
Serial.print(" Mag=");
Serial.println(mag, DEC);
delay(BNO055_SAMPLERATE_DELAY_MS);
}

292
lib/Adafruit_BNO055/examples/restore_offsets/restore_offsets.ino Executable file
View File

@ -0,0 +1,292 @@
#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_BNO055.h>
#include <utility/imumaths.h>
#include <EEPROM.h>
/* This driver uses the Adafruit unified sensor library (Adafruit_Sensor),
which provides a common 'type' for sensor data and some helper functions.
To use this driver you will also need to download the Adafruit_Sensor
library and include it in your libraries folder.
You should also assign a unique ID to this sensor for use with
the Adafruit Sensor API so that you can identify this particular
sensor in any data logs, etc. To assign a unique ID, simply
provide an appropriate value in the constructor below (12345
is used by default in this example).
Connections
===========
Connect SCL to analog 5
Connect SDA to analog 4
Connect VDD to 3-5V DC
Connect GROUND to common ground
History
=======
2015/MAR/03 - First release (KTOWN)
2015/AUG/27 - Added calibration and system status helpers
2015/NOV/13 - Added calibration save and restore
*/
/* Set the delay between fresh samples */
#define BNO055_SAMPLERATE_DELAY_MS (100)
Adafruit_BNO055 bno = Adafruit_BNO055(55);
/**************************************************************************/
/*
Displays some basic information on this sensor from the unified
sensor API sensor_t type (see Adafruit_Sensor for more information)
*/
/**************************************************************************/
void displaySensorDetails(void)
{
sensor_t sensor;
bno.getSensor(&sensor);
Serial.println("------------------------------------");
Serial.print("Sensor: "); Serial.println(sensor.name);
Serial.print("Driver Ver: "); Serial.println(sensor.version);
Serial.print("Unique ID: "); Serial.println(sensor.sensor_id);
Serial.print("Max Value: "); Serial.print(sensor.max_value); Serial.println(" xxx");
Serial.print("Min Value: "); Serial.print(sensor.min_value); Serial.println(" xxx");
Serial.print("Resolution: "); Serial.print(sensor.resolution); Serial.println(" xxx");
Serial.println("------------------------------------");
Serial.println("");
delay(500);
}
/**************************************************************************/
/*
Display some basic info about the sensor status
*/
/**************************************************************************/
void displaySensorStatus(void)
{
/* Get the system status values (mostly for debugging purposes) */
uint8_t system_status, self_test_results, system_error;
system_status = self_test_results = system_error = 0;
bno.getSystemStatus(&system_status, &self_test_results, &system_error);
/* Display the results in the Serial Monitor */
Serial.println("");
Serial.print("System Status: 0x");
Serial.println(system_status, HEX);
Serial.print("Self Test: 0x");
Serial.println(self_test_results, HEX);
Serial.print("System Error: 0x");
Serial.println(system_error, HEX);
Serial.println("");
delay(500);
}
/**************************************************************************/
/*
Display sensor calibration status
*/
/**************************************************************************/
void displayCalStatus(void)
{
/* Get the four calibration values (0..3) */
/* Any sensor data reporting 0 should be ignored, */
/* 3 means 'fully calibrated" */
uint8_t system, gyro, accel, mag;
system = gyro = accel = mag = 0;
bno.getCalibration(&system, &gyro, &accel, &mag);
/* The data should be ignored until the system calibration is > 0 */
Serial.print("\t");
if (!system)
{
Serial.print("! ");
}
/* Display the individual values */
Serial.print("Sys:");
Serial.print(system, DEC);
Serial.print(" G:");
Serial.print(gyro, DEC);
Serial.print(" A:");
Serial.print(accel, DEC);
Serial.print(" M:");
Serial.print(mag, DEC);
}
/**************************************************************************/
/*
Display the raw calibration offset and radius data
*/
/**************************************************************************/
void displaySensorOffsets(const adafruit_bno055_offsets_t &calibData)
{
Serial.print("Accelerometer: ");
Serial.print(calibData.accel_offset_x); Serial.print(" ");
Serial.print(calibData.accel_offset_y); Serial.print(" ");
Serial.print(calibData.accel_offset_z); Serial.print(" ");
Serial.print("\nGyro: ");
Serial.print(calibData.gyro_offset_x); Serial.print(" ");
Serial.print(calibData.gyro_offset_y); Serial.print(" ");
Serial.print(calibData.gyro_offset_z); Serial.print(" ");
Serial.print("\nMag: ");
Serial.print(calibData.mag_offset_x); Serial.print(" ");
Serial.print(calibData.mag_offset_y); Serial.print(" ");
Serial.print(calibData.mag_offset_z); Serial.print(" ");
Serial.print("\nAccel Radius: ");
Serial.print(calibData.accel_radius);
Serial.print("\nMag Radius: ");
Serial.print(calibData.mag_radius);
}
/**************************************************************************/
/*
Arduino setup function (automatically called at startup)
*/
/**************************************************************************/
void setup(void)
{
Serial.begin(115200);
delay(1000);
Serial.println("Orientation Sensor Test"); Serial.println("");
/* Initialise the sensor */
if (!bno.begin())
{
/* There was a problem detecting the BNO055 ... check your connections */
Serial.print("Ooops, no BNO055 detected ... Check your wiring or I2C ADDR!");
while (1);
}
int eeAddress = 0;
long bnoID;
bool foundCalib = false;
EEPROM.get(eeAddress, bnoID);
adafruit_bno055_offsets_t calibrationData;
sensor_t sensor;
/*
* Look for the sensor's unique ID at the beginning oF EEPROM.
* This isn't foolproof, but it's better than nothing.
*/
bno.getSensor(&sensor);
if (bnoID != sensor.sensor_id)
{
Serial.println("\nNo Calibration Data for this sensor exists in EEPROM");
delay(500);
}
else
{
Serial.println("\nFound Calibration for this sensor in EEPROM.");
eeAddress += sizeof(long);
EEPROM.get(eeAddress, calibrationData);
displaySensorOffsets(calibrationData);
Serial.println("\n\nRestoring Calibration data to the BNO055...");
bno.setSensorOffsets(calibrationData);
Serial.println("\n\nCalibration data loaded into BNO055");
foundCalib = true;
}
delay(1000);
/* Display some basic information on this sensor */
displaySensorDetails();
/* Optional: Display current status */
displaySensorStatus();
//Crystal must be configured AFTER loading calibration data into BNO055.
bno.setExtCrystalUse(true);
sensors_event_t event;
bno.getEvent(&event);
if (foundCalib){
Serial.println("Move sensor slightly to calibrate magnetometers");
while (!bno.isFullyCalibrated())
{
bno.getEvent(&event);
delay(BNO055_SAMPLERATE_DELAY_MS);
}
}
else
{
Serial.println("Please Calibrate Sensor: ");
while (!bno.isFullyCalibrated())
{
bno.getEvent(&event);
Serial.print("X: ");
Serial.print(event.orientation.x, 4);
Serial.print("\tY: ");
Serial.print(event.orientation.y, 4);
Serial.print("\tZ: ");
Serial.print(event.orientation.z, 4);
/* Optional: Display calibration status */
displayCalStatus();
/* New line for the next sample */
Serial.println("");
/* Wait the specified delay before requesting new data */
delay(BNO055_SAMPLERATE_DELAY_MS);
}
}
Serial.println("\nFully calibrated!");
Serial.println("--------------------------------");
Serial.println("Calibration Results: ");
adafruit_bno055_offsets_t newCalib;
bno.getSensorOffsets(newCalib);
displaySensorOffsets(newCalib);
Serial.println("\n\nStoring calibration data to EEPROM...");
eeAddress = 0;
bno.getSensor(&sensor);
bnoID = sensor.sensor_id;
EEPROM.put(eeAddress, bnoID);
eeAddress += sizeof(long);
EEPROM.put(eeAddress, newCalib);
Serial.println("Data stored to EEPROM.");
Serial.println("\n--------------------------------\n");
delay(500);
}
void loop() {
/* Get a new sensor event */
sensors_event_t event;
bno.getEvent(&event);
/* Display the floating point data */
Serial.print("X: ");
Serial.print(event.orientation.x, 4);
Serial.print("\tY: ");
Serial.print(event.orientation.y, 4);
Serial.print("\tZ: ");
Serial.print(event.orientation.z, 4);
/* Optional: Display calibration status */
displayCalStatus();
/* Optional: Display sensor status (debug only) */
//displaySensorStatus();
/* New line for the next sample */
Serial.println("");
/* Wait the specified delay before requesting new data */
delay(BNO055_SAMPLERATE_DELAY_MS);
}

174
lib/Adafruit_BNO055/examples/sensorapi/sensorapi.ino Executable file
View File

@ -0,0 +1,174 @@
#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_BNO055.h>
#include <utility/imumaths.h>
/* This driver uses the Adafruit unified sensor library (Adafruit_Sensor),
which provides a common 'type' for sensor data and some helper functions.
To use this driver you will also need to download the Adafruit_Sensor
library and include it in your libraries folder.
You should also assign a unique ID to this sensor for use with
the Adafruit Sensor API so that you can identify this particular
sensor in any data logs, etc. To assign a unique ID, simply
provide an appropriate value in the constructor below (12345
is used by default in this example).
Connections
===========
Connect SCL to analog 5
Connect SDA to analog 4
Connect VDD to 3-5V DC
Connect GROUND to common ground
History
=======
2015/MAR/03 - First release (KTOWN)
2015/AUG/27 - Added calibration and system status helpers
*/
/* Set the delay between fresh samples */
#define BNO055_SAMPLERATE_DELAY_MS (100)
Adafruit_BNO055 bno = Adafruit_BNO055(55);
/**************************************************************************/
/*
Displays some basic information on this sensor from the unified
sensor API sensor_t type (see Adafruit_Sensor for more information)
*/
/**************************************************************************/
void displaySensorDetails(void)
{
sensor_t sensor;
bno.getSensor(&sensor);
Serial.println("------------------------------------");
Serial.print ("Sensor: "); Serial.println(sensor.name);
Serial.print ("Driver Ver: "); Serial.println(sensor.version);
Serial.print ("Unique ID: "); Serial.println(sensor.sensor_id);
Serial.print ("Max Value: "); Serial.print(sensor.max_value); Serial.println(" xxx");
Serial.print ("Min Value: "); Serial.print(sensor.min_value); Serial.println(" xxx");
Serial.print ("Resolution: "); Serial.print(sensor.resolution); Serial.println(" xxx");
Serial.println("------------------------------------");
Serial.println("");
delay(500);
}
/**************************************************************************/
/*
Display some basic info about the sensor status
*/
/**************************************************************************/
void displaySensorStatus(void)
{
/* Get the system status values (mostly for debugging purposes) */
uint8_t system_status, self_test_results, system_error;
system_status = self_test_results = system_error = 0;
bno.getSystemStatus(&system_status, &self_test_results, &system_error);
/* Display the results in the Serial Monitor */
Serial.println("");
Serial.print("System Status: 0x");
Serial.println(system_status, HEX);
Serial.print("Self Test: 0x");
Serial.println(self_test_results, HEX);
Serial.print("System Error: 0x");
Serial.println(system_error, HEX);
Serial.println("");
delay(500);
}
/**************************************************************************/
/*
Display sensor calibration status
*/
/**************************************************************************/
void displayCalStatus(void)
{
/* Get the four calibration values (0..3) */
/* Any sensor data reporting 0 should be ignored, */
/* 3 means 'fully calibrated" */
uint8_t system, gyro, accel, mag;
system = gyro = accel = mag = 0;
bno.getCalibration(&system, &gyro, &accel, &mag);
/* The data should be ignored until the system calibration is > 0 */
Serial.print("\t");
if (!system)
{
Serial.print("! ");
}
/* Display the individual values */
Serial.print("Sys:");
Serial.print(system, DEC);
Serial.print(" G:");
Serial.print(gyro, DEC);
Serial.print(" A:");
Serial.print(accel, DEC);
Serial.print(" M:");
Serial.print(mag, DEC);
}
/**************************************************************************/
/*
Arduino setup function (automatically called at startup)
*/
/**************************************************************************/
void setup(void)
{
Serial.begin(9600);
Serial.println("Orientation Sensor Test"); Serial.println("");
/* Initialise the sensor */
if(!bno.begin())
{
/* There was a problem detecting the BNO055 ... check your connections */
Serial.print("Ooops, no BNO055 detected ... Check your wiring or I2C ADDR!");
while(1);
}
delay(1000);
/* Display some basic information on this sensor */
displaySensorDetails();
/* Optional: Display current status */
displaySensorStatus();
bno.setExtCrystalUse(true);
}
/**************************************************************************/
/*
Arduino loop function, called once 'setup' is complete (your own code
should go here)
*/
/**************************************************************************/
void loop(void)
{
/* Get a new sensor event */
sensors_event_t event;
bno.getEvent(&event);
/* Display the floating point data */
Serial.print("X: ");
Serial.print(event.orientation.x, 4);
Serial.print("\tY: ");
Serial.print(event.orientation.y, 4);
Serial.print("\tZ: ");
Serial.print(event.orientation.z, 4);
/* Optional: Display calibration status */
displayCalStatus();
/* Optional: Display sensor status (debug only) */
//displaySensorStatus();
/* New line for the next sample */
Serial.println("");
/* Wait the specified delay before requesting nex data */
delay(BNO055_SAMPLERATE_DELAY_MS);
}

9
lib/Adafruit_BNO055/library.properties Executable file
View File

@ -0,0 +1,9 @@
name=Adafruit BNO055
version=1.1.6
author=Adafruit <info@adafruit.com>
maintainer=Adafruit <info@adafruit.com>
sentence=Library for the Adafruit BNO055 Absolute Orientation Sensor.
paragraph=Designed specifically to work with the Adafruit BNO055 Breakout, and is based on Adafruit's Unified Sensor Library.
category=Sensors
url=https://github.com/adafruit/Adafruit_BNO055
architectures=*

30
lib/Adafruit_BNO055/utility/imumaths.h Executable file
View File

@ -0,0 +1,30 @@
/*
Inertial Measurement Unit Maths Library
Copyright (C) 2013-2014 Samuel Cowen
www.camelsoftware.com
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef IMUMATH_H
#define IMUMATH_H
#include "vector.h"
#include "matrix.h"
#include "quaternion.h"
#endif

243
lib/Adafruit_BNO055/utility/matrix.h Executable file
View File

@ -0,0 +1,243 @@
/*
Inertial Measurement Unit Maths Library
Copyright (C) 2013-2014 Samuel Cowen
www.camelsoftware.com
Bug fixes and cleanups by Gé Vissers (gvissers@gmail.com)
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef IMUMATH_MATRIX_HPP
#define IMUMATH_MATRIX_HPP
#include <string.h>
#include <stdint.h>
#include "vector.h"
namespace imu
{
template <uint8_t N> class Matrix
{
public:
Matrix()
{
memset(_cell_data, 0, N*N*sizeof(double));
}
Matrix(const Matrix &m)
{
for (int ij = 0; ij < N*N; ++ij)
{
_cell_data[ij] = m._cell_data[ij];
}
}
~Matrix()
{
}
Matrix& operator=(const Matrix& m)
{
for (int ij = 0; ij < N*N; ++ij)
{
_cell_data[ij] = m._cell_data[ij];
}
return *this;
}
Vector<N> row_to_vector(int i) const
{
Vector<N> ret;
for (int j = 0; j < N; j++)
{
ret[j] = cell(i, j);
}
return ret;
}
Vector<N> col_to_vector(int j) const
{
Vector<N> ret;
for (int i = 0; i < N; i++)
{
ret[i] = cell(i, j);
}
return ret;
}
void vector_to_row(const Vector<N>& v, int i)
{
for (int j = 0; j < N; j++)
{
cell(i, j) = v[j];
}
}
void vector_to_col(const Vector<N>& v, int j)
{
for (int i = 0; i < N; i++)
{
cell(i, j) = v[i];
}
}
double operator()(int i, int j) const
{
return cell(i, j);
}
double& operator()(int i, int j)
{
return cell(i, j);
}
double cell(int i, int j) const
{
return _cell_data[i*N+j];
}
double& cell(int i, int j)
{
return _cell_data[i*N+j];
}
Matrix operator+(const Matrix& m) const
{
Matrix ret;
for (int ij = 0; ij < N*N; ++ij)
{
ret._cell_data[ij] = _cell_data[ij] + m._cell_data[ij];
}
return ret;
}
Matrix operator-(const Matrix& m) const
{
Matrix ret;
for (int ij = 0; ij < N*N; ++ij)
{
ret._cell_data[ij] = _cell_data[ij] - m._cell_data[ij];
}
return ret;
}
Matrix operator*(double scalar) const
{
Matrix ret;
for (int ij = 0; ij < N*N; ++ij)
{
ret._cell_data[ij] = _cell_data[ij] * scalar;
}
return ret;
}
Matrix operator*(const Matrix& m) const
{
Matrix ret;
for (int i = 0; i < N; i++)
{
Vector<N> row = row_to_vector(i);
for (int j = 0; j < N; j++)
{
ret(i, j) = row.dot(m.col_to_vector(j));
}
}
return ret;
}
Matrix transpose() const
{
Matrix ret;
for (int i = 0; i < N; i++)
{
for (int j = 0; j < N; j++)
{
ret(j, i) = cell(i, j);
}
}
return ret;
}
Matrix<N-1> minor_matrix(int row, int col) const
{
Matrix<N-1> ret;
for (int i = 0, im = 0; i < N; i++)
{
if (i == row)
continue;
for (int j = 0, jm = 0; j < N; j++)
{
if (j != col)
{
ret(im, jm++) = cell(i, j);
}
}
im++;
}
return ret;
}
double determinant() const
{
// specialization for N == 1 given below this class
double det = 0.0, sign = 1.0;
for (int i = 0; i < N; ++i, sign = -sign)
det += sign * cell(0, i) * minor_matrix(0, i).determinant();
return det;
}
Matrix invert() const
{
Matrix ret;
double det = determinant();
for (int i = 0; i < N; i++)
{
for (int j = 0; j < N; j++)
{
ret(i, j) = minor_matrix(j, i).determinant() / det;
if ((i+j)%2 == 1)
ret(i, j) = -ret(i, j);
}
}
return ret;
}
double trace() const
{
double tr = 0.0;
for (int i = 0; i < N; ++i)
tr += cell(i, i);
return tr;
}
private:
double _cell_data[N*N];
};
template<>
inline double Matrix<1>::determinant() const
{
return cell(0, 0);
}
};
#endif

272
lib/Adafruit_BNO055/utility/quaternion.h Executable file
View File

@ -0,0 +1,272 @@
/*
Inertial Measurement Unit Maths Library
Copyright (C) 2013-2014 Samuel Cowen
www.camelsoftware.com
Bug fixes and cleanups by Gé Vissers (gvissers@gmail.com)
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef IMUMATH_QUATERNION_HPP
#define IMUMATH_QUATERNION_HPP
#include <stdlib.h>
#include <string.h>
#include <stdint.h>
#include <math.h>
#include "matrix.h"
namespace imu
{
class Quaternion
{
public:
Quaternion(): _w(1.0), _x(0.0), _y(0.0), _z(0.0) {}
Quaternion(double w, double x, double y, double z):
_w(w), _x(x), _y(y), _z(z) {}
Quaternion(double w, Vector<3> vec):
_w(w), _x(vec.x()), _y(vec.y()), _z(vec.z()) {}
double& w()
{
return _w;
}
double& x()
{
return _x;
}
double& y()
{
return _y;
}
double& z()
{
return _z;
}
double w() const
{
return _w;
}
double x() const
{
return _x;
}
double y() const
{
return _y;
}
double z() const
{
return _z;
}
double magnitude() const
{
return sqrt(_w*_w + _x*_x + _y*_y + _z*_z);
}
void normalize()
{
double mag = magnitude();
*this = this->scale(1/mag);
}
Quaternion conjugate() const
{
return Quaternion(_w, -_x, -_y, -_z);
}
void fromAxisAngle(const Vector<3>& axis, double theta)
{
_w = cos(theta/2);
//only need to calculate sine of half theta once
double sht = sin(theta/2);
_x = axis.x() * sht;
_y = axis.y() * sht;
_z = axis.z() * sht;
}
void fromMatrix(const Matrix<3>& m)
{
double tr = m.trace();
double S;
if (tr > 0)
{
S = sqrt(tr+1.0) * 2;
_w = 0.25 * S;
_x = (m(2, 1) - m(1, 2)) / S;
_y = (m(0, 2) - m(2, 0)) / S;
_z = (m(1, 0) - m(0, 1)) / S;
}
else if (m(0, 0) > m(1, 1) && m(0, 0) > m(2, 2))
{
S = sqrt(1.0 + m(0, 0) - m(1, 1) - m(2, 2)) * 2;
_w = (m(2, 1) - m(1, 2)) / S;
_x = 0.25 * S;
_y = (m(0, 1) + m(1, 0)) / S;
_z = (m(0, 2) + m(2, 0)) / S;
}
else if (m(1, 1) > m(2, 2))
{
S = sqrt(1.0 + m(1, 1) - m(0, 0) - m(2, 2)) * 2;
_w = (m(0, 2) - m(2, 0)) / S;
_x = (m(0, 1) + m(1, 0)) / S;
_y = 0.25 * S;
_z = (m(1, 2) + m(2, 1)) / S;
}
else
{
S = sqrt(1.0 + m(2, 2) - m(0, 0) - m(1, 1)) * 2;
_w = (m(1, 0) - m(0, 1)) / S;
_x = (m(0, 2) + m(2, 0)) / S;
_y = (m(1, 2) + m(2, 1)) / S;
_z = 0.25 * S;
}
}
void toAxisAngle(Vector<3>& axis, double& angle) const
{
double sqw = sqrt(1-_w*_w);
if (sqw == 0) //it's a singularity and divide by zero, avoid
return;
angle = 2 * acos(_w);
axis.x() = _x / sqw;
axis.y() = _y / sqw;
axis.z() = _z / sqw;
}
Matrix<3> toMatrix() const
{
Matrix<3> ret;
ret.cell(0, 0) = 1 - 2*_y*_y - 2*_z*_z;
ret.cell(0, 1) = 2*_x*_y - 2*_w*_z;
ret.cell(0, 2) = 2*_x*_z + 2*_w*_y;
ret.cell(1, 0) = 2*_x*_y + 2*_w*_z;
ret.cell(1, 1) = 1 - 2*_x*_x - 2*_z*_z;
ret.cell(1, 2) = 2*_y*_z - 2*_w*_x;
ret.cell(2, 0) = 2*_x*_z - 2*_w*_y;
ret.cell(2, 1) = 2*_y*_z + 2*_w*_x;
ret.cell(2, 2) = 1 - 2*_x*_x - 2*_y*_y;
return ret;
}
// Returns euler angles that represent the quaternion. Angles are
// returned in rotation order and right-handed about the specified
// axes:
//
// v[0] is applied 1st about z (ie, roll)
// v[1] is applied 2nd about y (ie, pitch)
// v[2] is applied 3rd about x (ie, yaw)
//
// Note that this means result.x() is not a rotation about x;
// similarly for result.z().
//
Vector<3> toEuler() const
{
Vector<3> ret;
double sqw = _w*_w;
double sqx = _x*_x;
double sqy = _y*_y;
double sqz = _z*_z;
ret.x() = atan2(2.0*(_x*_y+_z*_w),(sqx-sqy-sqz+sqw));
ret.y() = asin(-2.0*(_x*_z-_y*_w)/(sqx+sqy+sqz+sqw));
ret.z() = atan2(2.0*(_y*_z+_x*_w),(-sqx-sqy+sqz+sqw));
return ret;
}
Vector<3> toAngularVelocity(double dt) const
{
Vector<3> ret;
Quaternion one(1.0, 0.0, 0.0, 0.0);
Quaternion delta = one - *this;
Quaternion r = (delta/dt);
r = r * 2;
r = r * one;
ret.x() = r.x();
ret.y() = r.y();
ret.z() = r.z();
return ret;
}
Vector<3> rotateVector(const Vector<2>& v) const
{
return rotateVector(Vector<3>(v.x(), v.y()));
}
Vector<3> rotateVector(const Vector<3>& v) const
{
Vector<3> qv(_x, _y, _z);
Vector<3> t = qv.cross(v) * 2.0;
return v + t*_w + qv.cross(t);
}
Quaternion operator*(const Quaternion& q) const
{
return Quaternion(
_w*q._w - _x*q._x - _y*q._y - _z*q._z,
_w*q._x + _x*q._w + _y*q._z - _z*q._y,
_w*q._y - _x*q._z + _y*q._w + _z*q._x,
_w*q._z + _x*q._y - _y*q._x + _z*q._w
);
}
Quaternion operator+(const Quaternion& q) const
{
return Quaternion(_w + q._w, _x + q._x, _y + q._y, _z + q._z);
}
Quaternion operator-(const Quaternion& q) const
{
return Quaternion(_w - q._w, _x - q._x, _y - q._y, _z - q._z);
}
Quaternion operator/(double scalar) const
{
return Quaternion(_w / scalar, _x / scalar, _y / scalar, _z / scalar);
}
Quaternion operator*(double scalar) const
{
return scale(scalar);
}
Quaternion scale(double scalar) const
{
return Quaternion(_w * scalar, _x * scalar, _y * scalar, _z * scalar);
}
private:
double _w, _x, _y, _z;
};
} // namespace
#endif

227
lib/Adafruit_BNO055/utility/vector.h Executable file
View File

@ -0,0 +1,227 @@
/*
Inertial Measurement Unit Maths Library
Copyright (C) 2013-2014 Samuel Cowen
www.camelsoftware.com
Bug fixes and cleanups by Gé Vissers (gvissers@gmail.com)
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef IMUMATH_VECTOR_HPP
#define IMUMATH_VECTOR_HPP
#include <string.h>
#include <stdint.h>
#include <math.h>
namespace imu
{
template <uint8_t N> class Vector
{
public:
Vector()
{
memset(p_vec, 0, sizeof(double)*N);
}
Vector(double a)
{
memset(p_vec, 0, sizeof(double)*N);
p_vec[0] = a;
}
Vector(double a, double b)
{
memset(p_vec, 0, sizeof(double)*N);
p_vec[0] = a;
p_vec[1] = b;
}
Vector(double a, double b, double c)
{
memset(p_vec, 0, sizeof(double)*N);
p_vec[0] = a;
p_vec[1] = b;
p_vec[2] = c;
}
Vector(double a, double b, double c, double d)
{
memset(p_vec, 0, sizeof(double)*N);
p_vec[0] = a;
p_vec[1] = b;
p_vec[2] = c;
p_vec[3] = d;
}
Vector(const Vector<N> &v)
{
for (int x = 0; x < N; x++)
p_vec[x] = v.p_vec[x];
}
~Vector()
{
}
uint8_t n() { return N; }
double magnitude() const
{
double res = 0;
for (int i = 0; i < N; i++)
res += p_vec[i] * p_vec[i];
return sqrt(res);
}
void normalize()
{
double mag = magnitude();
if (isnan(mag) || mag == 0.0)
return;
for (int i = 0; i < N; i++)
p_vec[i] /= mag;
}
double dot(const Vector& v) const
{
double ret = 0;
for (int i = 0; i < N; i++)
ret += p_vec[i] * v.p_vec[i];
return ret;
}
// The cross product is only valid for vectors with 3 dimensions,
// with the exception of higher dimensional stuff that is beyond
// the intended scope of this library.
// Only a definition for N==3 is given below this class, using
// cross() with another value for N will result in a link error.
Vector cross(const Vector& v) const;
Vector scale(double scalar) const
{
Vector ret;
for(int i = 0; i < N; i++)
ret.p_vec[i] = p_vec[i] * scalar;
return ret;
}
Vector invert() const
{
Vector ret;
for(int i = 0; i < N; i++)
ret.p_vec[i] = -p_vec[i];
return ret;
}
Vector& operator=(const Vector& v)
{
for (int x = 0; x < N; x++ )
p_vec[x] = v.p_vec[x];
return *this;
}
double& operator [](int n)
{
return p_vec[n];
}
double operator [](int n) const
{
return p_vec[n];
}
double& operator ()(int n)
{
return p_vec[n];
}
double operator ()(int n) const
{
return p_vec[n];
}
Vector operator+(const Vector& v) const
{
Vector ret;
for(int i = 0; i < N; i++)
ret.p_vec[i] = p_vec[i] + v.p_vec[i];
return ret;
}
Vector operator-(const Vector& v) const
{
Vector ret;
for(int i = 0; i < N; i++)
ret.p_vec[i] = p_vec[i] - v.p_vec[i];
return ret;
}
Vector operator * (double scalar) const
{
return scale(scalar);
}
Vector operator / (double scalar) const
{
Vector ret;
for(int i = 0; i < N; i++)
ret.p_vec[i] = p_vec[i] / scalar;
return ret;
}
void toDegrees()
{
for(int i = 0; i < N; i++)
p_vec[i] *= 57.2957795131; //180/pi
}
void toRadians()
{
for(int i = 0; i < N; i++)
p_vec[i] *= 0.01745329251; //pi/180
}
double& x() { return p_vec[0]; }
double& y() { return p_vec[1]; }
double& z() { return p_vec[2]; }
double x() const { return p_vec[0]; }
double y() const { return p_vec[1]; }
double z() const { return p_vec[2]; }
private:
double p_vec[N];
};
template <>
inline Vector<3> Vector<3>::cross(const Vector& v) const
{
return Vector(
p_vec[1] * v.p_vec[2] - p_vec[2] * v.p_vec[1],
p_vec[2] * v.p_vec[0] - p_vec[0] * v.p_vec[2],
p_vec[0] * v.p_vec[1] - p_vec[1] * v.p_vec[0]
);
}
} // namespace
#endif

21
lib/imu_class/imu.cpp Normal file
View File

@ -0,0 +1,21 @@
#include <Arduino.h>
#include <Adafruit_BNO055.h>
#include "imu.h"
Adafruit_BNO055 bno = Adafruit_BNO055();
void IMU::initIMU() {
bno.begin(bno.OPERATION_MODE_IMUPLUS); //Posizione impostata a P7 alle righe 105,107 di Adafruit_BNO55.cpp
bno.setExtCrystalUse(true);
}
float IMU::getEuler() {
}
void IMU::getData() {
}

8
lib/imu_class/imu.h Normal file
View File

@ -0,0 +1,8 @@
class IMU {
public:
void getEuler();
private:
void initIMU();
void getData();
};

6
src/main.cpp
View File

@ -1,11 +1,11 @@
#include <Arduino.h> #include <Arduino.h>
#include <imu_class/imu.cpp>
//culo
void setup() { void setup() {
// put your setup code here, to run once: Serial.begin(9600);
} }
void loop() { void loop() {
// put your main code here, to run repeatedly:
} }