Toyomasa Watarai
Adafruit driver converted to Mbed OS 6.x.
Dependents: Adafruit-BNO055-test
Revision 0:22c544c8741a, committed 2015-09-16
- Comitter:
- simonscott
- Date:
- Wed Sep 16 19:48:33 2015 +0000
- Child:
- 1:b48e4192c101
- Commit message:
- First version
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Adafruit_BNO055.cpp Wed Sep 16 19:48:33 2015 +0000
@@ -0,0 +1,437 @@
+/***************************************************************************
+ 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
+ ***************************************************************************/
+
+#include <math.h>
+#include <limits.h>
+#include "mbed.h"
+
+#include "Adafruit_BNO055.h"
+
+/***************************************************************************
+ CONSTRUCTOR
+ ***************************************************************************/
+
+/**************************************************************************/
+/*!
+ @brief Instantiates a new Adafruit_BNO055 class
+*/
+/**************************************************************************/
+Adafruit_BNO055::Adafruit_BNO055(int32_t sensorID, uint8_t address, I2C* i2c_ptr)
+{
+ _sensorID = sensorID;
+ _address = address;
+ i2c = i2c_ptr;
+}
+
+/***************************************************************************
+ PUBLIC FUNCTIONS
+ ***************************************************************************/
+
+/**************************************************************************/
+/*!
+ @brief Sets up the HW
+*/
+/**************************************************************************/
+bool Adafruit_BNO055::begin(adafruit_bno055_opmode_t mode)
+{
+ /* Enable I2C */
+ i2c->frequency(100000);
+
+ /* Make sure we have the right device */
+ uint8_t id = read8(BNO055_CHIP_ID_ADDR);
+ if(id != BNO055_ID)
+ {
+ wait_ms(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)
+ {
+ wait_ms(10);
+ }
+ wait_ms(50);
+
+ /* Set to normal power mode */
+ write8(BNO055_PWR_MODE_ADDR, POWER_MODE_NORMAL);
+ wait_ms(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);
+ */
+
+ write8(BNO055_SYS_TRIGGER_ADDR, 0x0);
+ wait_ms(10);
+ /* Set the requested operating mode (see section 3.3) */
+ setMode(mode);
+ wait_ms(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);
+ wait_ms(30);
+}
+
+/**************************************************************************/
+/*!
+ @brief Use the external 32.768KHz crystal
+*/
+/**************************************************************************/
+void Adafruit_BNO055::setExtCrystalUse(bool usextal)
+{
+ adafruit_bno055_opmode_t modeback = _mode;
+
+ /* Switch to config mode (just in case since this is the default) */
+ setMode(OPERATION_MODE_CONFIG);
+ wait_ms(25);
+ write8(BNO055_PAGE_ID_ADDR, 0);
+ if (usextal) {
+ write8(BNO055_SYS_TRIGGER_ADDR, 0x80);
+ } else {
+ write8(BNO055_SYS_TRIGGER_ADDR, 0x00);
+ }
+ wait_ms(10);
+ /* Set the requested operating mode (see section 3.3) */
+ setMode(modeback);
+ wait_ms(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);
+
+ wait_ms(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;
+ unsigned char 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, (char*)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)
+{
+ unsigned char buffer[8];
+ memset (buffer, 0, 8);
+
+ int x, y, z, w;
+ x = y = z = w = 0;
+
+ /* Read quat data (8 bytes) */
+ readLen(BNO055_QUATERNION_DATA_W_LSB_ADDR, (char*)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 = 0; //TODO: fix this with a millis() call
+
+ /* 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;
+}
+
+/***************************************************************************
+ PRIVATE FUNCTIONS
+ ***************************************************************************/
+
+/**************************************************************************/
+/*!
+ @brief Writes an 8 bit value over I2C
+*/
+/**************************************************************************/
+bool Adafruit_BNO055::write8(adafruit_bno055_reg_t reg, char value)
+{
+ char reg_to_write = (char)(reg);
+ i2c->write(_address<<1, ®_to_write, 1, true);
+ i2c->write(_address<<1, &value, 1, false);
+
+ /* ToDo: Check for error! */
+ return true;
+}
+
+/**************************************************************************/
+/*!
+ @brief Reads an 8 bit value over I2C
+*/
+/**************************************************************************/
+char Adafruit_BNO055::read8(adafruit_bno055_reg_t reg )
+{
+ char to_read = 0;
+ char to_write = (char)reg;
+
+ i2c->write(_address<<1, &to_write, 1, false);
+ i2c->read(_address<<1, &to_read, 1, false);
+
+ return to_read;
+}
+
+/**************************************************************************/
+/*!
+ @brief Reads the specified number of bytes over I2C
+*/
+/**************************************************************************/
+bool Adafruit_BNO055::readLen(adafruit_bno055_reg_t reg, char* buffer, int len)
+{
+ char reg_to_write = (char)(reg);
+
+ i2c->write(_address<<1, ®_to_write, 1, false);
+ i2c->read(_address<<1, buffer, len, false);
+
+ /* ToDo: Check for errors! */
+ return true;
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Adafruit_BNO055.h Wed Sep 16 19:48:33 2015 +0000
@@ -0,0 +1,260 @@
+/***************************************************************************
+ 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__
+
+#include "Adafruit_Sensor.h"
+#include "imumaths.h"
+#include "mbed.h"
+
+#define BNO055_ADDRESS_A (0x28)
+#define BNO055_ADDRESS_B (0x29)
+#define BNO055_ID (0xA0)
+
+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 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;
+
+ Adafruit_BNO055 ( int32_t sensorID = -1, uint8_t address = BNO055_ADDRESS_A, I2C* i2c_ptr = 0 );
+
+ bool begin ( adafruit_bno055_opmode_t mode = OPERATION_MODE_NDOF );
+ void setMode ( adafruit_bno055_opmode_t mode );
+ void getRevInfo ( adafruit_bno055_rev_info_t* );
+ void displayRevInfo ( void );
+ void setExtCrystalUse ( bool 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* );
+
+ private:
+ char read8 ( adafruit_bno055_reg_t );
+ bool readLen ( adafruit_bno055_reg_t, char* buffer, int len );
+ bool write8 ( adafruit_bno055_reg_t, char value );
+
+ uint8_t _address;
+ int32_t _sensorID;
+ adafruit_bno055_opmode_t _mode;
+ I2C* i2c;
+};
+
+#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Adafruit_Sensor.h Wed Sep 16 19:48:33 2015 +0000
@@ -0,0 +1,148 @@
+/*
+* Copyright (C) 2008 The Android Open Source Project
+*
+* Licensed under the Apache License, Version 2.0 (the "License");
+* you may not use this file except in compliance with the License.
+* You may obtain a copy of the License at
+*
+* http://www.apache.org/licenses/LICENSE-2.0
+*
+* Unless required by applicable law or agreed to in writing, software< /span>
+* distributed under the License is distributed on an "AS IS" BASIS,
+* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+* See the License for the specific language governing permissions and
+* limitations under the License.
+*/
+
+/* Update by K. Townsend (Adafruit Industries) for lighter typedefs, and
+ * extended sensor support to include color, voltage and current */
+
+#ifndef _ADAFRUIT_SENSOR_H
+#define _ADAFRUIT_SENSOR_H
+
+
+/* Intentionally modeled after sensors.h in the Android API:
+ * https://github.com/android/platform_hardware_libhardware/blob/master/include/hardware/sensors.h */
+
+/* Constants */
+#define SENSORS_GRAVITY_EARTH (9.80665F) /**< Earth's gravity in m/s^2 */
+#define SENSORS_GRAVITY_MOON (1.6F) /**< The moon's gravity in m/s^2 */
+#define SENSORS_GRAVITY_SUN (275.0F) /**< The sun's gravity in m/s^2 */
+#define SENSORS_GRAVITY_STANDARD (SENSORS_GRAVITY_EARTH)
+#define SENSORS_MAGFIELD_EARTH_MAX (60.0F) /**< Maximum magnetic field on Earth's surface */
+#define SENSORS_MAGFIELD_EARTH_MIN (30.0F) /**< Minimum magnetic field on Earth's surface */
+#define SENSORS_PRESSURE_SEALEVELHPA (1013.25F) /**< Average sea level pressure is 1013.25 hPa */
+#define SENSORS_DPS_TO_RADS (0.017453293F) /**< Degrees/s to rad/s multiplier */
+#define SENSORS_GAUSS_TO_MICROTESLA (100) /**< Gauss to micro-Tesla multiplier */
+
+/** Sensor types */
+typedef enum
+{
+ SENSOR_TYPE_ACCELEROMETER = (1), /**< Gravity + linear acceleration */
+ SENSOR_TYPE_MAGNETIC_FIELD = (2),
+ SENSOR_TYPE_ORIENTATION = (3),
+ SENSOR_TYPE_GYROSCOPE = (4),
+ SENSOR_TYPE_LIGHT = (5),
+ SENSOR_TYPE_PRESSURE = (6),
+ SENSOR_TYPE_PROXIMITY = (8),
+ SENSOR_TYPE_GRAVITY = (9),
+ SENSOR_TYPE_LINEAR_ACCELERATION = (10), /**< Acceleration not including gravity */
+ SENSOR_TYPE_ROTATION_VECTOR = (11),
+ SENSOR_TYPE_RELATIVE_HUMIDITY = (12),
+ SENSOR_TYPE_AMBIENT_TEMPERATURE = (13),
+ SENSOR_TYPE_VOLTAGE = (15),
+ SENSOR_TYPE_CURRENT = (16),
+ SENSOR_TYPE_COLOR = (17)
+} sensors_type_t;
+
+/** struct sensors_vec_s is used to return a vector in a common format. */
+typedef struct {
+ union {
+ float v[3];
+ struct {
+ float x;
+ float y;
+ float z;
+ };
+ /* Orientation sensors */
+ struct {
+ float roll; /**< Rotation around the longitudinal axis (the plane body, 'X axis'). Roll is positive and increasing when moving downward. -90°<=roll<=90° */
+ float pitch; /**< Rotation around the lateral axis (the wing span, 'Y axis'). Pitch is positive and increasing when moving upwards. -180°<=pitch<=180°) */
+ float heading; /**< Angle between the longitudinal axis (the plane body) and magnetic north, measured clockwise when viewing from the top of the device. 0-359° */
+ };
+ };
+ char status;
+ unsigned char reserved[3];
+} sensors_vec_t;
+
+/** struct sensors_color_s is used to return color data in a common format. */
+typedef struct {
+ union {
+ float c[3];
+ /* RGB color space */
+ struct {
+ float r; /**< Red component */
+ float g; /**< Green component */
+ float b; /**< Blue component */
+ };
+ };
+ unsigned int rgba; /**< 24-bit RGBA value */
+} sensors_color_t;
+
+/* Sensor event (36 bytes) */
+/** struct sensor_event_s is used to provide a single sensor event in a common format. */
+typedef struct
+{
+ int version; /**< must be sizeof(struct sensors_event_t) */
+ int sensor_id; /**< unique sensor identifier */
+ int type; /**< sensor type */
+ int reserved0; /**< reserved */
+ int timestamp; /**< time is in milliseconds */
+ union
+ {
+ float data[4];
+ sensors_vec_t acceleration; /**< acceleration values are in meter per second per second (m/s^2) */
+ sensors_vec_t magnetic; /**< magnetic vector values are in micro-Tesla (uT) */
+ sensors_vec_t orientation; /**< orientation values are in degrees */
+ sensors_vec_t gyro; /**< gyroscope values are in rad/s */
+ float temperature; /**< temperature is in degrees centigrade (Celsius) */
+ float distance; /**< distance in centimeters */
+ float light; /**< light in SI lux units */
+ float pressure; /**< pressure in hectopascal (hPa) */
+ float relative_humidity; /**< relative humidity in percent */
+ float current; /**< current in milliamps (mA) */
+ float voltage; /**< voltage in volts (V) */
+ sensors_color_t color; /**< color in RGB component values */
+ };
+} sensors_event_t;
+
+/* Sensor details (40 bytes) */
+/** struct sensor_s is used to describe basic information about a specific sensor. */
+typedef struct
+{
+ char name[12]; /**< sensor name */
+ int version; /**< version of the hardware + driver */
+ int sensor_id; /**< unique sensor identifier */
+ int type; /**< this sensor's type (ex. SENSOR_TYPE_LIGHT) */
+ float max_value; /**< maximum value of this sensor's value in SI units */
+ float min_value; /**< minimum value of this sensor's value in SI units */
+ float resolution; /**< smallest difference between two values reported by this sensor */
+ int min_delay; /**< min delay in microseconds between events. zero = not a constant rate */
+} sensor_t;
+
+class Adafruit_Sensor {
+ public:
+ // Constructor(s)
+ Adafruit_Sensor() {}
+ virtual ~Adafruit_Sensor() {}
+
+ // These must be defined by the subclass
+ virtual void enableAutoRange(bool enabled) {};
+ virtual bool getEvent(sensors_event_t*) = 0;
+ virtual void getSensor(sensor_t*) = 0;
+
+ private:
+ bool _autoRange;
+};
+
+#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/imumaths.h Wed Sep 16 19:48:33 2015 +0000 @@ -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 +
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/matrix.h Wed Sep 16 19:48:33 2015 +0000
@@ -0,0 +1,248 @@
+/*
+ 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_MATRIX_HPP
+#define IMUMATH_MATRIX_HPP
+
+#include <stdlib.h>
+#include <string.h>
+#include <stdint.h>
+#include <math.h>
+
+namespace imu
+{
+
+
+template <uint8_t N> class Matrix
+{
+public:
+ Matrix()
+ {
+ int r = sizeof(double)*N;
+ _cell = &_cell_data[0];
+ memset(_cell, 0, r*r);
+ }
+
+ Matrix(const Matrix &v)
+ {
+ int r = sizeof(double)*N;
+ _cell = &_cell_data[0];
+ memset(_cell, 0, r*r);
+ for (int x = 0; x < N; x++ )
+ {
+ for(int y = 0; y < N; y++)
+ {
+ _cell[x*N+y] = v._cell[x*N+y];
+ }
+ }
+ }
+
+ ~Matrix()
+ {
+ }
+
+ void operator = (Matrix m)
+ {
+ for(int x = 0; x < N; x++)
+ {
+ for(int y = 0; y < N; y++)
+ {
+ cell(x, y) = m.cell(x, y);
+ }
+ }
+ }
+
+ Vector<N> row_to_vector(int y)
+ {
+ Vector<N> ret;
+ for(int i = 0; i < N; i++)
+ {
+ ret[i] = _cell[y*N+i];
+ }
+ return ret;
+ }
+
+ Vector<N> col_to_vector(int x)
+ {
+ Vector<N> ret;
+ for(int i = 0; i < N; i++)
+ {
+ ret[i] = _cell[i*N+x];
+ }
+ return ret;
+ }
+
+ void vector_to_row(Vector<N> v, int row)
+ {
+ for(int i = 0; i < N; i++)
+ {
+ cell(row, i) = v(i);
+ }
+ }
+
+ void vector_to_col(Vector<N> v, int col)
+ {
+ for(int i = 0; i < N; i++)
+ {
+ cell(i, col) = v(i);
+ }
+ }
+
+ double& operator ()(int x, int y)
+ {
+ return _cell[x*N+y];
+ }
+
+ double& cell(int x, int y)
+ {
+ return _cell[x*N+y];
+ }
+
+
+ Matrix operator + (Matrix m)
+ {
+ Matrix ret;
+ for(int x = 0; x < N; x++)
+ {
+ for(int y = 0; y < N; y++)
+ {
+ ret._cell[x*N+y] = _cell[x*N+y] + m._cell[x*N+y];
+ }
+ }
+ return ret;
+ }
+
+ Matrix operator - (Matrix m)
+ {
+ Matrix ret;
+ for(int x = 0; x < N; x++)
+ {
+ for(int y = 0; y < N; y++)
+ {
+ ret._cell[x*N+y] = _cell[x*N+y] - m._cell[x*N+y];
+ }
+ }
+ return ret;
+ }
+
+ Matrix operator * (double scalar)
+ {
+ Matrix ret;
+ for(int x = 0; x < N; x++)
+ {
+ for(int y = 0; y < N; y++)
+ {
+ ret._cell[x*N+y] = _cell[x*N+y] * scalar;
+ }
+ }
+ return ret;
+ }
+
+ Matrix operator * (Matrix m)
+ {
+ Matrix ret;
+ for(int x = 0; x < N; x++)
+ {
+ for(int y = 0; y < N; y++)
+ {
+ Vector<N> row = row_to_vector(x);
+ Vector<N> col = m.col_to_vector(y);
+ ret.cell(x, y) = row.dot(col);
+ }
+ }
+ return ret;
+ }
+
+ Matrix transpose()
+ {
+ Matrix ret;
+ for(int x = 0; x < N; x++)
+ {
+ for(int y = 0; y < N; y++)
+ {
+ ret.cell(y, x) = cell(x, y);
+ }
+ }
+ return ret;
+ }
+
+ Matrix<N-1> minor_matrix(int row, int col)
+ {
+ int colCount = 0, rowCount = 0;
+ Matrix<N-1> ret;
+ for(int i = 0; i < N; i++ )
+ {
+ if( i != row )
+ {
+ for(int j = 0; j < N; j++ )
+ {
+ if( j != col )
+ {
+ ret(rowCount, colCount) = cell(i, j);
+ colCount++;
+ }
+ }
+ rowCount++;
+ }
+ }
+ return ret;
+ }
+
+ double determinant()
+ {
+ if(N == 1)
+ return cell(0, 0);
+
+ float det = 0.0;
+ for(int i = 0; i < N; i++ )
+ {
+ Matrix<N-1> minor = minor_matrix(0, i);
+ det += (i%2==1?-1.0:1.0) * cell(0, i) * minor.determinant();
+ }
+ return det;
+ }
+
+ Matrix invert()
+ {
+ Matrix ret;
+ float det = determinant();
+
+ for(int x = 0; x < N; x++)
+ {
+ for(int y = 0; y < N; y++)
+ {
+ Matrix<N-1> minor = minor_matrix(y, x);
+ ret(x, y) = det*minor.determinant();
+ if( (x+y)%2 == 1)
+ ret(x, y) = -ret(x, y);
+ }
+ }
+ return ret;
+ }
+
+private:
+ double* _cell;
+ double _cell_data[N*N];
+};
+
+
+};
+
+#endif
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/quaternion.h Wed Sep 16 19:48:33 2015 +0000
@@ -0,0 +1,321 @@
+/*
+ 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_QUATERNION_HPP
+#define IMUMATH_QUATERNION_HPP
+
+#include <stdlib.h>
+#include <string.h>
+#include <stdint.h>
+#include <math.h>
+
+#include "vector.h"
+
+
+namespace imu
+{
+
+
+
+class Quaternion
+{
+public:
+ Quaternion()
+ {
+ _w = 1.0;
+ _x = _y = _z = 0.0;
+ }
+
+ Quaternion(double iw, double ix, double iy, double iz)
+ {
+ _w = iw;
+ _x = ix;
+ _y = iy;
+ _z = iz;
+ }
+
+ 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
+ {
+ double res = (_w*_w) + (_x*_x) + (_y*_y) + (_z*_z);
+ return sqrt(res);
+ }
+
+ void normalize()
+ {
+ double mag = magnitude();
+ *this = this->scale(1/mag);
+ }
+
+
+ Quaternion conjugate() const
+ {
+ Quaternion q;
+ q.w() = _w;
+ q.x() = -_x;
+ q.y() = -_y;
+ q.z() = -_z;
+ return q;
+ }
+
+ void fromAxisAngle(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(Matrix<3> m)
+ {
+ float tr = m(0, 0) + m(1, 1) + m(2, 2);
+
+ float S = 0.0;
+ 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, float& angle) const
+ {
+ float 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(float 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(Vector<2> v) const
+ {
+ Vector<3> ret(v.x(), v.y(), 0.0);
+ return rotateVector(ret);
+ }
+
+ Vector<3> rotateVector(Vector<3> v) const
+ {
+ Vector<3> qv(this->x(), this->y(), this->z());
+ Vector<3> t;
+ t = qv.cross(v) * 2.0;
+ return v + (t * _w) + qv.cross(t);
+ }
+
+
+ Quaternion operator * (Quaternion q) const
+ {
+ Quaternion ret;
+ ret._w = ((_w*q._w) - (_x*q._x) - (_y*q._y) - (_z*q._z));
+ ret._x = ((_w*q._x) + (_x*q._w) + (_y*q._z) - (_z*q._y));
+ ret._y = ((_w*q._y) - (_x*q._z) + (_y*q._w) + (_z*q._x));
+ ret._z = ((_w*q._z) + (_x*q._y) - (_y*q._x) + (_z*q._w));
+ return ret;
+ }
+
+ Quaternion operator + (Quaternion q) const
+ {
+ Quaternion ret;
+ ret._w = _w + q._w;
+ ret._x = _x + q._x;
+ ret._y = _y + q._y;
+ ret._z = _z + q._z;
+ return ret;
+ }
+
+ Quaternion operator - (Quaternion q) const
+ {
+ Quaternion ret;
+ ret._w = _w - q._w;
+ ret._x = _x - q._x;
+ ret._y = _y - q._y;
+ ret._z = _z - q._z;
+ return ret;
+ }
+
+ Quaternion operator / (float scalar) const
+ {
+ Quaternion ret;
+ ret._w = this->_w/scalar;
+ ret._x = this->_x/scalar;
+ ret._y = this->_y/scalar;
+ ret._z = this->_z/scalar;
+ return ret;
+ }
+
+ Quaternion operator * (float scalar) const
+ {
+ Quaternion ret;
+ ret._w = this->_w*scalar;
+ ret._x = this->_x*scalar;
+ ret._y = this->_y*scalar;
+ ret._z = this->_z*scalar;
+ return ret;
+ }
+
+ Quaternion scale(double scalar) const
+ {
+ Quaternion ret;
+ ret._w = this->_w*scalar;
+ ret._x = this->_x*scalar;
+ ret._y = this->_y*scalar;
+ ret._z = this->_z*scalar;
+ return ret;
+ }
+
+private:
+ double _w, _x, _y, _z;
+};
+
+
+};
+
+#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/vector.h Wed Sep 16 19:48:33 2015 +0000
@@ -0,0 +1,231 @@
+/*
+ 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_VECTOR_HPP
+#define IMUMATH_VECTOR_HPP
+
+#include <stdlib.h>
+#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()
+ {
+ double res = 0;
+ int i;
+ for(i = 0; i < N; i++)
+ res += (p_vec[i] * p_vec[i]);
+
+ if(isnan(res))
+ return 0;
+ if((fabs(res-1)) >= 0.000001) // Avoid a sqrt if possible.
+ return sqrt(res);
+ return 1;
+ }
+
+ void normalize()
+ {
+ double mag = magnitude();
+ if(abs(mag) <= 0.0001)
+ return;
+
+ int i;
+ for(i = 0; i < N; i++)
+ p_vec[i] = p_vec[i]/mag;
+ }
+
+ double dot(Vector v)
+ {
+ double ret = 0;
+ int i;
+ for(i = 0; i < N; i++)
+ ret += p_vec[i] * v.p_vec[i];
+
+ return ret;
+ }
+
+ Vector cross(Vector v)
+ {
+ Vector 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
+ if(N != 3)
+ return ret;
+
+ ret.p_vec[0] = (p_vec[1] * v.p_vec[2]) - (p_vec[2] * v.p_vec[1]);
+ ret.p_vec[1] = (p_vec[2] * v.p_vec[0]) - (p_vec[0] * v.p_vec[2]);
+ ret.p_vec[2] = (p_vec[0] * v.p_vec[1]) - (p_vec[1] * v.p_vec[0]);
+ return ret;
+ }
+
+ 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 = (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 + (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 - (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];
+};
+
+
+};
+
+#endif