caperren/school_archives
1
0
Fork 0
mirror of https://github.com/caperren/school_archives.git synced 2025-11-09 21:51:15 +00:00
Code Issues Packages Projects Releases Wiki Activity

Added 2017-2018 mars rover repository.

Browse Source
This commit is contained in:
Corwin Perren
2018-08-22 14:54:52 -07:00
parent a56690ca18
commit 7fd2641766
750 changed files with 2019222 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

718
OSU Robotics Club/Mars Rover 2017-2018/software/firmware/libraries/Adafruit_BNO055_t3/Adafruit_BNO055_t3.cpp Normal file
View File

@@ -0,0 +1,718 @@
/***************************************************************************
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.
!!!!!MODIFIED VERSION TO USE I2C_T3 LIBRARY FOR TEENSY INSTEAD OF WIRE
IN ORDER TO USE TEENSY'S BOTH I2C PORTS
Roald Baudoux - 2016-03
Updated 2016-03-15: i2c_bus type defined to set choice of Wire or Wire1
Updated 2016-08-17: added delay of 30 ms after function call "write8(BNO055_SYS_TRIGGER_ADDR, 0x20);"
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_t3.h" // MODIFIED RB
/***************************************************************************
CONSTRUCTOR
***************************************************************************/
/**************************************************************************/
/*!
@brief Instantiates a new Adafruit_BNO055 class
!!!!! MODIFIED TO BE COMPATIBLE WITH I2C_T3 library instead of Wire
// Params : I2C bus, sensor ID, address, I2C mode (master/slave), pins, pullups, freq, opMode (interrupt/DMA/immediate)
*/
/**************************************************************************/
// OLD : Adafruit_BNO055::Adafruit_BNO055(int32_t sensorID, uint8_t address)
Adafruit_BNO055::Adafruit_BNO055(i2c_bus iBus, int32_t sensorID, uint8_t aAddress, i2c_mode iMode, i2c_pins pins, i2c_pullup pullup, i2c_rate iRate, i2c_op_mode opeMode)
{
_iBus = iBus; // ADDED RB 0 for Wire() and 1 for Wire1();
_sensorID = sensorID;
_aAddress = aAddress;
_iMode = iMode; // ADDED RB
_pins = pins; // ADDED RB
_pullup = pullup; // ADDED RB
_iRate = iRate; // ADDED RB
_opeMode = opeMode; // ADDED RB
}
/***************************************************************************
PUBLIC FUNCTIONS
***************************************************************************/
/**************************************************************************/
/*!
@brief Sets up the HW
*/
/**************************************************************************/
bool Adafruit_BNO055::begin(adafruit_bno055_opmode_t aMode)
{
/* Enable I2C */
if (_iBus == WIRE_BUS)
{
Wire.begin(_iMode, _aAddress, _pins, _pullup, _iRate, _opeMode);
}
else
{
Wire1.begin(_iMode, _aAddress, _pins, _pullup, _iRate, _opeMode);
}
/* Make sure we have the right device */
uint8_t id = read8(BNO055_CHIP_ID_ADDR);
if(id != BNO055_ID)
{
delay(1000);
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);
delay(30); // ADDED RB
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_P2); // P0-P7, Default is P1
delay(10);
write8(BNO055_AXIS_MAP_SIGN_ADDR, REMAP_SIGN_P2); // 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(aMode);
delay(20);
return true;
}
/**************************************************************************/
/*!
@brief Puts the chip in the specified operating mode
*/
/**************************************************************************/
void Adafruit_BNO055::setMode(adafruit_bno055_opmode_t aMode)
{
_aMode = aMode;
write8(BNO055_OPR_MODE_ADDR, _aMode);
delay(30);
}
/**************************************************************************/
/*!
@brief Use the external 32.768KHz crystal
*/
/**************************************************************************/
void Adafruit_BNO055::setExtCrystalUse(boolean usextal)
{
adafruit_bno055_opmode_t modeback = _aMode;
/* 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:
/* 1rps = 900 LSB */
xyz[0] = ((double)x)/900.0;
xyz[1] = ((double)y)/900.0;
xyz[2] = ((double)z)/900.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 = _aMode;
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 = _aMode;
setMode(OPERATION_MODE_CONFIG);
delay(25);
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));
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));
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));
offsets_type.accel_radius = (read8(ACCEL_RADIUS_MSB_ADDR) << 8) | (read8(ACCEL_RADIUS_LSB_ADDR));
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 = _aMode;
setMode(OPERATION_MODE_CONFIG);
delay(25);
/* 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(GYRO_OFFSET_X_LSB_ADDR, calibData[6]);
write8(GYRO_OFFSET_X_MSB_ADDR, calibData[7]);
write8(GYRO_OFFSET_Y_LSB_ADDR, calibData[8]);
write8(GYRO_OFFSET_Y_MSB_ADDR, calibData[9]);
write8(GYRO_OFFSET_Z_LSB_ADDR, calibData[10]);
write8(GYRO_OFFSET_Z_MSB_ADDR, calibData[11]);
write8(MAG_OFFSET_X_LSB_ADDR, calibData[12]);
write8(MAG_OFFSET_X_MSB_ADDR, calibData[13]);
write8(MAG_OFFSET_Y_LSB_ADDR, calibData[14]);
write8(MAG_OFFSET_Y_MSB_ADDR, calibData[15]);
write8(MAG_OFFSET_Z_LSB_ADDR, calibData[16]);
write8(MAG_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 = _aMode;
setMode(OPERATION_MODE_CONFIG);
delay(25);
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(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(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(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);
}
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)
{
if (_iBus == WIRE_BUS)
{
Wire.beginTransmission(_aAddress); //// MODIFIED RB
#if ARDUINO >= 100
Wire.write((uint8_t)reg); /// TO BE MODIFIED? NO
Wire.write((uint8_t)value); /// TO BE MODIFIED? NO
#else
Wire.send(reg); /// TO BE MODIFIED? NO
Wire.send(value); /// TO BE MODIFIED? NO
#endif
Wire.endTransmission(); /// TO BE MODIFIED? NO
/* ToDo: Check for error! */
return true;
}
else
{
Wire1.beginTransmission(_aAddress); //// MODIFIED RB
#if ARDUINO >= 100
Wire1.write((uint8_t)reg); /// TO BE MODIFIED? NO
Wire1.write((uint8_t)value); /// TO BE MODIFIED? NO
#else
Wire1.send(reg); /// TO BE MODIFIED? NO
Wire1.send(value); /// TO BE MODIFIED? NO
#endif
Wire1.endTransmission(); /// TO BE MODIFIED? NO
/* 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;
if (_iBus == WIRE_BUS)
{
Wire.beginTransmission(_aAddress); /// TO BE MODIFIED? NO
#if ARDUINO >= 100
Wire.write((uint8_t)reg); /// TO BE MODIFIED? NO
#else
Wire.send(reg);
#endif
Wire.endTransmission();
Wire.requestFrom(_aAddress, (byte)1); /// TO BE MODIFIED? NO
#if ARDUINO >= 100
value = Wire.read(); /// TO BE MODIFIED? NO
#else
value = Wire.receive(); //// MODIFIED RB receive -> readByte
//value = Wire.readByte(); //// MODIFIED RB receive -> readByte
#endif
return value;
}
else
{
Wire1.beginTransmission(_aAddress); /// TO BE MODIFIED? NO
#if ARDUINO >= 100
Wire1.write((uint8_t)reg); /// TO BE MODIFIED? NO
#else
Wire1.send(reg);
#endif
Wire1.endTransmission();
Wire1.requestFrom(_aAddress, (byte)1); /// TO BE MODIFIED? NO
#if ARDUINO >= 100
value = Wire1.read(); /// TO BE MODIFIED? NO
#else
value = Wire1.readByte(); //// MODIFIED RB receive -> readByte
#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)
{
if (_iBus == WIRE_BUS)
{
Wire.beginTransmission(_aAddress); /// TO BE MODIFIED? NO
#if ARDUINO >= 100
Wire.write((uint8_t)reg); /// TO BE MODIFIED? NO
#else
Wire.send(reg); /// TO BE MODIFIED? NO
#endif
Wire.endTransmission();
Wire.requestFrom(_aAddress, (byte)len); /// TO BE MODIFIED? NO
for (uint8_t i = 0; i < len; i++)
{
#if ARDUINO >= 100
buffer[i] = Wire.read(); /// TO BE MODIFIED? NO
#else
buffer[i] = Wire.readByte(); //// MODIFIE RB receive -> readByte
#endif
}
/* ToDo: Check for errors! */
return true;
}
else
{
Wire1.beginTransmission(_aAddress); /// TO BE MODIFIED? NO
#if ARDUINO >= 100
Wire1.write((uint8_t)reg); /// TO BE MODIFIED? NO
#else
Wire1.send(reg); /// TO BE MODIFIED? NO
#endif
Wire1.endTransmission();
Wire1.requestFrom(_aAddress, (byte)len); /// TO BE MODIFIED? NO
for (uint8_t i = 0; i < len; i++)
{
#if ARDUINO >= 100
buffer[i] = Wire1.read(); /// TO BE MODIFIED? NO
#else
buffer[i] = Wire1.readByte(); //// MODIFIE RB receive -> readByte
#endif
}
/* ToDo: Check for errors! */
return true;
}
}

344
OSU Robotics Club/Mars Rover 2017-2018/software/firmware/libraries/Adafruit_BNO055_t3/Adafruit_BNO055_t3.h Normal file
View File

@@ -0,0 +1,344 @@
/***************************************************************************
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.
!!!!!MODIFIED VERSION TO USE I2C_T3 LIBRARY FOR TEENSY INSTEAD OF WIRE
IN ORDER TO USE TEENSY'S BOTH I2C PORTS
Roald Baudoux - 2016-03
Updated 2016-03-15: i2c_bus type defined to set choice of Wire or Wire1
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
// code for AT_TINY85 deleted (no AT_TINY85 on Teensy boards)
#include <i2c_t3.h> // MODIFIED FROM Wire.h to i2c_t3.h
#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)
enum i2c_bus {WIRE_BUS, WIRE1_BUS}; // ADDED RB
typedef struct
{
uint16_t accel_offset_x;
uint16_t accel_offset_y;
uint16_t accel_offset_z;
uint16_t gyro_offset_x;
uint16_t gyro_offset_y;
uint16_t gyro_offset_z;
uint16_t mag_offset_x;
uint16_t mag_offset_y;
uint16_t mag_offset_z;
uint16_t accel_radius;
uint16_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;
// Arduino Zero board check removed (No Zero in Teensy range)
/*
#ifdef ARDUINO_SAMD_ZERO
#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
*/
Adafruit_BNO055 (i2c_bus iBus = WIRE_BUS, int32_t sensorID = -1, uint8_t aAddress = BNO055_ADDRESS_A, i2c_mode iMode = I2C_MASTER, i2c_pins pins = I2C_PINS_18_19, i2c_pullup pullup = I2C_PULLUP_INT, i2c_rate iRate = I2C_RATE_100, i2c_op_mode opeMode = I2C_OP_MODE_ISR); // MODIFIED
bool begin ( adafruit_bno055_opmode_t aMode = OPERATION_MODE_NDOF );
void setMode ( adafruit_bno055_opmode_t aMode );
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 );
// bool _i2cBus; // ADDED : O for Wire and 1 for Wire1
uint8_t _aAddress;
int32_t _sensorID;
adafruit_bno055_opmode_t _aMode;
// ADDED RB :
i2c_bus _iBus; // Wire or Wire 1
i2c_mode _iMode; // MASTER OR SLAVE MODE
i2c_pins _pins;
i2c_pullup _pullup;
i2c_rate _iRate;
i2c_op_mode _opeMode;
};
#endif

30
OSU Robotics Club/Mars Rover 2017-2018/software/firmware/libraries/Adafruit_BNO055_t3/utility/imumaths.h Normal 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
OSU Robotics Club/Mars Rover 2017-2018/software/firmware/libraries/Adafruit_BNO055_t3/utility/matrix.h Normal 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
OSU Robotics Club/Mars Rover 2017-2018/software/firmware/libraries/Adafruit_BNO055_t3/utility/quaternion.h Normal 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
OSU Robotics Club/Mars Rover 2017-2018/software/firmware/libraries/Adafruit_BNO055_t3/utility/vector.h Normal 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