diff --git a/Examples/CMakeLists.txt b/Examples/CMakeLists.txt
index ad900633..4d222954 100644
--- a/Examples/CMakeLists.txt
+++ b/Examples/CMakeLists.txt
@@ -10,6 +10,7 @@ add_subdirectory(Libpropeller_Pwm32)
add_subdirectory(libPropelleruino_Blinky)
add_subdirectory(PropWare_ADXL345)
add_subdirectory(PropWare_Blinky)
+add_subdirectory(PropWare_BNO055)
add_subdirectory(PropWare_BufferedUART)
add_subdirectory(PropWare_DualPWM)
add_subdirectory(PropWare_Eeprom)
diff --git a/Examples/PropWare_BNO055/BNO055.pdf b/Examples/PropWare_BNO055/BNO055.pdf
new file mode 100644
index 00000000..6511f7b4
Binary files /dev/null and b/Examples/PropWare_BNO055/BNO055.pdf differ
diff --git a/Examples/PropWare_BNO055/BNO055_Demo.cpp b/Examples/PropWare_BNO055/BNO055_Demo.cpp
new file mode 100644
index 00000000..57197d6a
--- /dev/null
+++ b/Examples/PropWare_BNO055/BNO055_Demo.cpp
@@ -0,0 +1,115 @@
+/**
+ * @file BNO055_Demo.cpp
+ *
+ * @author David Zemon
+ *
+ * @copyright
+ * The MIT License (MIT)
+ *
Copyright (c) 2013 David Zemon
+ *
Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"), to
+ * deal in the Software without restriction, including without limitation the
+ * rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
+ * sell copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
The above copyright notice and this permission notice shall be included
+ * in all copies or substantial portions of the Software.
+ *
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
+ * OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#include
+#include
+
+using PropWare::I2CMaster;
+using PropWare::BNO055;
+using PropWare::Pin;
+
+uint8_t get_byte (i2c *bus) {
+ i2c_start(bus);
+ i2c_writeByte(bus, BNO055::DEVICE_ADDRESS_A);
+ i2c_writeByte(bus, BNO055::convert(BNO055::Address::CHIP_ID));
+ const uint8_t result = static_cast(i2c_readByte(bus, 1));
+ i2c_stop(bus);
+ return result;
+}
+
+PropWare::ErrorCode begin (i2c *bus) {
+ auto id = get_byte(bus);
+ if (id != BNO055::ID) {
+ waitcnt(SECOND + CNT); // hold on for boot
+ id = get_byte(bus);
+ if (id != BNO055::ID) {
+ pwOut << "fail 0\n";
+ return BNO055::FAILED_TO_BOOT;
+ }
+ }
+ return 0;
+}
+
+int ping (i2c *bus) {
+ return i2c_poll(bus, BNO055::DEVICE_ADDRESS_A);
+}
+
+/**
+ * @example BNO055_Demo.cpp
+ *
+ *
+ * @include PropWare_BNO055/CMakeLists.txt
+ */
+int main () {
+ PropWare::ErrorCode error;
+ const Pin resetPin(PropWare::Port::P23, Pin::Dir::OUT);
+ waitcnt(100 * MILLISECOND + CNT);
+
+ i2c bus;
+ i2c_open(&bus, Pin::from_mask(I2CMaster::DEFAULT_SCL_MASK), Pin::from_mask(I2CMaster::DEFAULT_SDA_MASK), 0);
+
+ for (unsigned int i = 0; i < 5; ++i) {
+ resetPin.clear();
+ waitcnt(MILLISECOND + CNT);
+ resetPin.set();
+
+ while (ping(&bus))
+ waitcnt(10 * MILLISECOND + CNT);
+ int ack = i2c_writeByte(&bus, 0);
+ i2c_readByte(&bus, 0);
+ i2c_stop(&bus);
+ waitcnt(10 * MILLISECOND + CNT);
+
+ error = begin(&bus);
+ if (!error)
+ break;
+ }
+
+ //BNO055 sensor;
+ //for (int i = 0; i < 5; ++i) {
+ // resetPin.clear();
+ // waitcnt(MILLISECOND + CNT);
+ // resetPin.set();
+ //
+ // while (!pwI2c.ping(BNO055::DEVICE_ADDRESS_A))
+ // waitcnt(10 * MILLISECOND + CNT);
+ // pwI2c.send_byte(0);
+ // pwI2c.get(BNO055::DEVICE_ADDRESS_A, static_cast(0));
+ // pwI2c.stop();
+ // waitcnt(10 * MILLISECOND + CNT);
+ //
+ // error = sensor.begin();
+ // if (!error)
+ // break;
+ //}
+
+ if (error)
+ pwOut << "Error: " << error << '\n';
+ else
+ while (1) {
+ pwOut << "Temperature = " << /*sensor.get_temperature() <<*/ '\n';
+ waitcnt(SECOND + CNT);
+ }
+}
diff --git a/Examples/PropWare_BNO055/CMakeLists.txt b/Examples/PropWare_BNO055/CMakeLists.txt
new file mode 100644
index 00000000..1274330f
--- /dev/null
+++ b/Examples/PropWare_BNO055/CMakeLists.txt
@@ -0,0 +1,6 @@
+cmake_minimum_required(VERSION 3.3)
+find_package(PropWare REQUIRED)
+
+project(BNO055_Demo)
+
+create_simple_executable(${PROJECT_NAME} BNO055_Demo.cpp)
diff --git a/PropWare/CMakeLists.txt b/PropWare/CMakeLists.txt
index 8a8519f8..87c4ab42 100644
--- a/PropWare/CMakeLists.txt
+++ b/PropWare/CMakeLists.txt
@@ -33,6 +33,7 @@ set(PROPWARE_SOURCES
${CMAKE_CURRENT_LIST_DIR}/sensor/analog/mcp3xxx.h
${CMAKE_CURRENT_LIST_DIR}/sensor/analog/pcf8591.h
${CMAKE_CURRENT_LIST_DIR}/sensor/distance/ping.h
+ ${CMAKE_CURRENT_LIST_DIR}/sensor/fusion/bno055.h
${CMAKE_CURRENT_LIST_DIR}/sensor/gyroscope/l3g.h
${CMAKE_CURRENT_LIST_DIR}/sensor/temperature/max6675.h
${CMAKE_CURRENT_LIST_DIR}/serial/can/mcp2515.h
@@ -55,6 +56,9 @@ set(PROPWARE_SOURCES
${CMAKE_CURRENT_LIST_DIR}/utility/comparator.cpp
${CMAKE_CURRENT_LIST_DIR}/utility/comparator.h
${CMAKE_CURRENT_LIST_DIR}/utility/utility.h
+ ${CMAKE_CURRENT_LIST_DIR}/utility/ahrs/matrix.h
+ ${CMAKE_CURRENT_LIST_DIR}/utility/ahrs/quaternion.h
+ ${CMAKE_CURRENT_LIST_DIR}/utility/ahrs/vector.h
${CMAKE_CURRENT_LIST_DIR}/c++allocate.h
${CMAKE_CURRENT_LIST_DIR}/PropWare.cpp
${CMAKE_CURRENT_LIST_DIR}/PropWare.h
diff --git a/PropWare/sensor/accelerometer/adxl345.h b/PropWare/sensor/accelerometer/adxl345.h
index 4ad2298a..12edf999 100644
--- a/PropWare/sensor/accelerometer/adxl345.h
+++ b/PropWare/sensor/accelerometer/adxl345.h
@@ -369,7 +369,7 @@ class ADXL345 {
/**
* Set the data polling rate.
*/
- DataRate dataRate: 3;
+ DataRate dataRate: 4;
/**
* Enable or disable low-power mode. Low power mode has a somewhat higher noise but reduces power
* consumption. True to enable low power mode, false for normal operation.
diff --git a/PropWare/sensor/fusion/bno055.h b/PropWare/sensor/fusion/bno055.h
new file mode 100644
index 00000000..d7d9631a
--- /dev/null
+++ b/PropWare/sensor/fusion/bno055.h
@@ -0,0 +1,723 @@
+/**
+ * @file PropWare/sensor/fusion/bno055.h
+ *
+ * @author David Zemon (Only modified for use in PropWare)
+ *
+ * @copyright
+ * The MIT License (MIT)
+ *
Copyright (c) 2013 David Zemon
+ *
Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"), to
+ * deal in the Software without restriction, including without limitation the
+ * rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
+ * sell copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
The above copyright notice and this permission notice shall be included
+ * in all copies or substantial portions of the Software.
+ *
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
+ * OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#pragma once
+
+#include
+#include
+#include
+
+namespace PropWare {
+
+/**
+ * @brief 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
+ */
+class BNO055 {
+ public:
+ static const uint8_t DEVICE_ADDRESS_A = 0x28 << 1;
+ static const uint8_t DEVICE_ADDRESS_B = 0x29 << 1;
+ static const uint8_t ID = 0xA0;
+
+ enum class Address {
+ /* Page id register definition */
+ PAGE_ID = 0X07,
+
+ /* PAGE0 REGISTER DEFINITION START*/
+ CHIP_ID = 0x00,
+ ACCEL_REV_ID = 0x01,
+ MAG_REV_ID = 0x02,
+ GYRO_REV_ID = 0x03,
+ SW_REV_ID_LSB = 0x04,
+ SW_REV_ID_MSB = 0x05,
+ BL_REV_ID = 0X06,
+
+ /* Accel data register */
+ ACCEL_DATA_X_LSB = 0X08,
+ ACCEL_DATA_X_MSB = 0X09,
+ ACCEL_DATA_Y_LSB = 0X0A,
+ ACCEL_DATA_Y_MSB = 0X0B,
+ ACCEL_DATA_Z_LSB = 0X0C,
+ ACCEL_DATA_Z_MSB = 0X0D,
+
+ /* Mag data register */
+ MAG_DATA_X_LSB = 0X0E,
+ MAG_DATA_X_MSB = 0X0F,
+ MAG_DATA_Y_LSB = 0X10,
+ MAG_DATA_Y_MSB = 0X11,
+ MAG_DATA_Z_LSB = 0X12,
+ MAG_DATA_Z_MSB = 0X13,
+
+ /* Gyro data registers */
+ GYRO_DATA_X_LSB = 0X14,
+ GYRO_DATA_X_MSB = 0X15,
+ GYRO_DATA_Y_LSB = 0X16,
+ GYRO_DATA_Y_MSB = 0X17,
+ GYRO_DATA_Z_LSB = 0X18,
+ GYRO_DATA_Z_MSB = 0X19,
+
+ /* Euler data registers */
+ EULER_H_LSB = 0X1A,
+ EULER_H_MSB = 0X1B,
+ EULER_R_LSB = 0X1C,
+ EULER_R_MSB = 0X1D,
+ EULER_P_LSB = 0X1E,
+ EULER_P_MSB = 0X1F,
+
+ /* Quaternion data registers */
+ QUATERNION_DATA_W_LSB = 0X20,
+ QUATERNION_DATA_W_MSB = 0X21,
+ QUATERNION_DATA_X_LSB = 0X22,
+ QUATERNION_DATA_X_MSB = 0X23,
+ QUATERNION_DATA_Y_LSB = 0X24,
+ QUATERNION_DATA_Y_MSB = 0X25,
+ QUATERNION_DATA_Z_LSB = 0X26,
+ QUATERNION_DATA_Z_MSB = 0X27,
+
+ /* Linear acceleration data registers */
+ LINEAR_ACCEL_DATA_X_LSB = 0X28,
+ LINEAR_ACCEL_DATA_X_MSB = 0X29,
+ LINEAR_ACCEL_DATA_Y_LSB = 0X2A,
+ LINEAR_ACCEL_DATA_Y_MSB = 0X2B,
+ LINEAR_ACCEL_DATA_Z_LSB = 0X2C,
+ LINEAR_ACCEL_DATA_Z_MSB = 0X2D,
+
+ /* Gravity data registers */
+ GRAVITY_DATA_X_LSB = 0X2E,
+ GRAVITY_DATA_X_MSB = 0X2F,
+ GRAVITY_DATA_Y_LSB = 0X30,
+ GRAVITY_DATA_Y_MSB = 0X31,
+ GRAVITY_DATA_Z_LSB = 0X32,
+ GRAVITY_DATA_Z_MSB = 0X33,
+
+ /* Temperature data register */
+ TEMP = 0X34,
+
+ /* Status registers */
+ CALIB_STAT = 0X35,
+ SELFTEST_RESULT = 0X36,
+ INTR_STAT = 0X37,
+
+ SYS_CLK_STAT = 0X38,
+ SYS_STAT = 0X39,
+ SYS_ERR = 0X3A,
+
+ /* Unit selection register */
+ UNIT_SEL = 0X3B,
+ DATA_SELECT = 0X3C,
+
+ /* Mode registers */
+ OPR_MODE = 0X3D,
+ PWR_MODE = 0X3E,
+
+ SYS_TRIGGER = 0X3F,
+ TEMP_SOURCE = 0X40,
+
+ /* Axis remap registers */
+ AXIS_MAP_CONFIG = 0X41,
+ AXIS_MAP_SIGN = 0X42,
+
+ /* SIC registers */
+ SIC_MATRIX_0_LSB = 0X43,
+ SIC_MATRIX_0_MSB = 0X44,
+ SIC_MATRIX_1_LSB = 0X45,
+ SIC_MATRIX_1_MSB = 0X46,
+ SIC_MATRIX_2_LSB = 0X47,
+ SIC_MATRIX_2_MSB = 0X48,
+ SIC_MATRIX_3_LSB = 0X49,
+ SIC_MATRIX_3_MSB = 0X4A,
+ SIC_MATRIX_4_LSB = 0X4B,
+ SIC_MATRIX_4_MSB = 0X4C,
+ SIC_MATRIX_5_LSB = 0X4D,
+ SIC_MATRIX_5_MSB = 0X4E,
+ SIC_MATRIX_6_LSB = 0X4F,
+ SIC_MATRIX_6_MSB = 0X50,
+ SIC_MATRIX_7_LSB = 0X51,
+ SIC_MATRIX_7_MSB = 0X52,
+ SIC_MATRIX_8_LSB = 0X53,
+ SIC_MATRIX_8_MSB = 0X54,
+
+ /* Accelerometer Offset registers */
+ ACCEL_OFFSET_X_LSB = 0X55,
+ ACCEL_OFFSET_X_MSB = 0X56,
+ ACCEL_OFFSET_Y_LSB = 0X57,
+ ACCEL_OFFSET_Y_MSB = 0X58,
+ ACCEL_OFFSET_Z_LSB = 0X59,
+ ACCEL_OFFSET_Z_MSB = 0X5A,
+
+ /* Magnetometer Offset registers */
+ MAG_OFFSET_X_LSB = 0X5B,
+ MAG_OFFSET_X_MSB = 0X5C,
+ MAG_OFFSET_Y_LSB = 0X5D,
+ MAG_OFFSET_Y_MSB = 0X5E,
+ MAG_OFFSET_Z_LSB = 0X5F,
+ MAG_OFFSET_Z_MSB = 0X60,
+
+ /* Gyroscope Offset register s*/
+ GYRO_OFFSET_X_LSB = 0X61,
+ GYRO_OFFSET_X_MSB = 0X62,
+ GYRO_OFFSET_Y_LSB = 0X63,
+ GYRO_OFFSET_Y_MSB = 0X64,
+ GYRO_OFFSET_Z_LSB = 0X65,
+ GYRO_OFFSET_Z_MSB = 0X66,
+
+ /* Radius registers */
+ ACCEL_RADIUS_LSB = 0X67,
+ ACCEL_RADIUS_MSB = 0X68,
+ MAG_RADIUS_LSB = 0X69,
+ MAG_RADIUS_MSB = 0X6A
+ };
+
+ typedef enum {
+ POWER_MODE_NORMAL = 0X00,
+ POWER_MODE_LOWPOWER = 0X01,
+ POWER_MODE_SUSPEND = 0X02
+ } PowerMode;
+
+ enum class OperationMode {
+ /* Operation mode settings*/
+ CONFIG = 0X00,
+ ACCONLY = 0X01,
+ MAGONLY = 0X02,
+ GYRONLY = 0X03,
+ ACCMAG = 0X04,
+ ACCGYRO = 0X05,
+ MAGGYRO = 0X06,
+ AMG = 0X07,
+ IMUPLUS = 0X08,
+ COMPASS = 0X09,
+ M4G = 0X0A,
+ NDOF_FMC_OFF = 0X0B,
+ NDOF = 0X0C
+ };
+
+ 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
+ } AxisRemapConfiguration;
+
+ typedef enum {
+ P0 = 0x04,
+ P1 = 0x00, // default
+ P2 = 0x06,
+ P3 = 0x02,
+ P4 = 0x03,
+ P5 = 0x01,
+ P6 = 0x07,
+ P7 = 0x05
+ } AxisRemapSign;
+
+ typedef enum {
+ ACCELEROMETER = static_cast(Address::ACCEL_DATA_X_LSB),
+ MAGNETOMETER = static_cast(Address::MAG_DATA_X_LSB),
+ GYROSCOPE = static_cast(Address::GYRO_DATA_X_LSB),
+ EULER = static_cast(Address::EULER_H_LSB),
+ LINEARACCEL = static_cast(Address::LINEAR_ACCEL_DATA_X_LSB),
+ GRAVITY = static_cast(Address::GRAVITY_DATA_X_LSB)
+ } VectorType;
+
+ typedef enum {
+ NO_ERROR = 0,
+ BEG_ERROR = 256,
+ FAILED_TO_BOOT = BEG_ERROR,
+ END_ERROR = FAILED_TO_BOOT
+ } ErrorCode;
+
+ struct Offsets {
+ uint16_t accel_x;
+ uint16_t accel_y;
+ uint16_t accel_z;
+ uint16_t gyro_x;
+ uint16_t gyro_y;
+ uint16_t gyro_z;
+ uint16_t mag_x;
+ uint16_t mag_y;
+ uint16_t mag_z;
+
+ uint16_t accel_radius;
+ uint16_t mag_radius;
+ };
+
+ struct RevisionInfo {
+ uint8_t accel;
+ uint8_t mag;
+ uint8_t gyro;
+ uint16_t sw;
+ uint8_t bl;
+ };
+
+ public:
+ static uint8_t convert (const Address address) {
+ return static_cast(address);
+ }
+
+ static uint8_t convert (const OperationMode operationMode) {
+ return static_cast(operationMode);
+ }
+
+ public:
+ BNO055 (const uint8_t address = DEVICE_ADDRESS_A, const I2CMaster &bus = pwI2c)
+ : m_bus(&bus),
+ m_address(address) {
+ }
+
+ uint8_t get_byte(const Address address) const {
+ const uint8_t x = convert(address);
+ return this->m_bus->get(this->m_address, x);
+ }
+
+ PropWare::ErrorCode begin (const OperationMode mode = OperationMode::NDOF) {
+ /* Make sure we have the right device */
+ auto id = this->get_byte(Address::CHIP_ID);
+ if (id != ID) {
+ waitcnt(SECOND + CNT); // hold on for boot
+ id = this->get_byte(Address::CHIP_ID);
+ if (id != ID) {
+ pwOut << "fail 0\n";
+ return FAILED_TO_BOOT; // still not? ok bail
+ }
+ }
+
+ /* Switch to config mode (just in case since this is the default) */
+ if (!this->set_mode(OperationMode::CONFIG)) {
+ pwOut << "fail 1\n";
+ return FAILED_TO_BOOT;
+ }
+
+ /* Reset */
+ if (!this->m_bus->put(this->m_address, convert(Address::SYS_TRIGGER), 0x20)) {
+ pwOut << "fail 2\n";
+ return FAILED_TO_BOOT;
+ }
+ while (this->m_bus->get(this->m_address, convert(Address::CHIP_ID)) != ID)
+ waitcnt(10 * MILLISECOND + CNT);
+ waitcnt(50 * MILLISECOND + CNT);
+
+ /* Set to normal power mode */
+ if (!this->m_bus->put(this->m_address, convert(Address::PWR_MODE), POWER_MODE_NORMAL)) {
+ pwOut << "fail 3\n";
+ return FAILED_TO_BOOT;
+ }
+ waitcnt(10 * MILLISECOND + CNT);
+
+ if (!this->m_bus->put(this->m_address, convert(Address::PAGE_ID), 0)) {
+ pwOut << "fail 4\n";
+ return FAILED_TO_BOOT;
+ }
+
+ /* 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(Address::UNIT_SEL, unitsel);
+ */
+
+ /* Configure axis mapping (see section 3.4) */
+ /*
+ write8(Address::AXIS_MAP_CONFIG, REMAP_CONFIG_P2); // P0-P7, Default is P1
+ delay(10);
+ write8(Address::AXIS_MAP_SIGN, REMAP_SIGN_P2); // P0-P7, Default is P1
+ delay(10);
+ */
+
+ if (!this->m_bus->put(this->m_address, convert(Address::SYS_TRIGGER), 0x0)) {
+ pwOut << "fail 5\n";
+ return FAILED_TO_BOOT;
+ }
+ waitcnt(10 * MILLISECOND + CNT);
+
+ /* Set the requested operating mode (see section 3.3) */
+ if (!this->set_mode(mode)) {
+ pwOut << "fail 6\n";
+ return FAILED_TO_BOOT;
+ }
+ waitcnt(20 * MILLISECOND + CNT);
+
+ return NO_ERROR;
+ }
+
+ /**
+ * @brief Puts the chip in the specified operating mode
+ *
+ * @param[out] mode Desired mode of operation
+ *
+ * @return True if the write was ACKed, false otherwise
+ */
+ bool set_mode (const OperationMode mode) {
+ this->_mode = mode;
+ const auto ack = this->m_bus->put(this->m_address, convert(Address::OPR_MODE), convert(mode));
+ waitcnt(30 * MILLISECOND + CNT);
+ return ack;
+ }
+
+ /**
+ * @brief Use the external 32.768KHz crystal
+ *
+ * @param[in] useExternalCrystal When set, the external crystal will be used.
+ */
+ void set_external_crystal_use (const bool useExternalCrystal) {
+ const OperationMode startingMode = _mode;
+
+ /* Switch to config mode (just in case since this is the default) */
+ this->set_mode(OperationMode::CONFIG);
+ waitcnt(25 * MILLISECOND + CNT);
+ this->m_bus->put(this->m_address, convert(Address::PAGE_ID), 0);
+ if (useExternalCrystal)
+ this->m_bus->put(this->m_address, convert(Address::SYS_TRIGGER), 0x80);
+ else
+ this->m_bus->put(this->m_address, convert(Address::SYS_TRIGGER), 0x00);
+ waitcnt(10 * MILLISECOND + CNT);
+
+ /* Set the requested operating mode (see section 3.3) */
+ this->set_mode(startingMode);
+ waitcnt(20 * MILLISECOND + CNT);
+ }
+
+ /**
+ * @brief Gets the chip's revision numbers
+ *
+ * @param[out] info
+ */
+ void get_revision_info (RevisionInfo *info) const {
+
+ memset(info, 0, sizeof(RevisionInfo));
+
+ /* Check the accelerometer revision */
+ info->accel = this->m_bus->get(this->m_address, convert(Address::ACCEL_REV_ID));
+
+ /* Check the magnetometer revision */
+ info->mag = this->m_bus->get(this->m_address, convert(Address::MAG_REV_ID));
+
+ /* Check the gyroscope revision */
+ info->gyro = this->m_bus->get(this->m_address, convert(Address::GYRO_REV_ID));
+
+ /* Check the SW revision */
+ info->bl = this->m_bus->get(this->m_address, convert(Address::BL_REV_ID));
+
+ const auto a = this->m_bus->get(this->m_address, convert(Address::SW_REV_ID_LSB));
+ const auto b = this->m_bus->get(this->m_address, convert(Address::SW_REV_ID_MSB));
+ info->sw = (static_cast(b) << 8) | a;
+ }
+
+ /**
+ * @brief Gets the latest system status information
+ *
+ * @param[out] systemStatus
+ * @param[out] selfTestResult
+ * @param[out] systemError
+ */
+ void get_system_status (uint8_t *systemStatus, uint8_t *selfTestResult, uint8_t *systemError) const {
+ this->m_bus->put(this->m_address, convert(Address::PAGE_ID), 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 (0 != systemStatus)
+ *systemStatus = this->m_bus->get(this->m_address, convert(Address::SYS_STAT));
+
+ /* 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 (0 != selfTestResult)
+ *selfTestResult = this->m_bus->get(this->m_address, convert(Address::SELFTEST_RESULT));
+
+ /* 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 (systemError != 0)
+ *systemError = this->m_bus->get(this->m_address, convert(Address::SYS_ERR));
+
+ waitcnt(200 * MILLISECOND + CNT);
+ }
+
+ /**
+ * @brief Gets current calibration state.
+ *
+ * Each value will be set to 0 if not calibrated and 3 if fully calibrated.
+ *
+ * @param[out] system
+ * @param[out] gyro
+ * @param[out] accel
+ * @param[out] magnitude
+ */
+ void get_calibration (uint8_t *system, uint8_t *gyro, uint8_t *accel, uint8_t *magnitude) const {
+ const auto calData = this->m_bus->get(this->m_address, convert(Address::CALIB_STAT));
+ if (NULL != system)
+ *system = (calData >> 6) & static_cast(0x03);
+ if (NULL != gyro)
+ *gyro = (calData >> 4) & static_cast(0x03);
+ if (NULL != accel)
+ *accel = (calData >> 2) & static_cast(0x03);
+ if (NULL != magnitude)
+ *magnitude = calData & static_cast(0x03);
+ }
+
+ /**
+ * @brief Gets a vector reading from the specified source
+ *
+ * @param[in] type
+ * @param[out] result
+ */
+ imu::Vector<3> get_vector (const VectorType type) const {
+ uint8_t buffer[6];
+ this->m_bus->get(this->m_address, static_cast(type), buffer, 6);
+
+ const auto x = ((int16_t) buffer[0]) | (((int16_t) buffer[1]) << 8);
+ const auto y = ((int16_t) buffer[2]) | (((int16_t) buffer[3]) << 8);
+ const auto 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 */
+ double scalar;
+ switch (type) {
+ case MAGNETOMETER:
+ case GYROSCOPE:
+ case EULER:
+ scalar = 16;
+ break;
+ case ACCELEROMETER:
+ case LINEARACCEL:
+ case GRAVITY:
+ scalar = 100;
+ break;
+ default:
+ scalar = 1;
+ }
+
+ return imu::Vector<3>(x / scalar, y / scalar, z / scalar);
+ }
+
+ imu::Quaternion get_quaternion () const {
+ uint8_t buffer[8];
+ this->m_bus->get(this->m_address, convert(Address::QUATERNION_DATA_W_LSB), buffer, 8);
+ const auto w = (static_cast(buffer[1]) << 8) | buffer[0];
+ const auto x = (static_cast(buffer[3]) << 8) | buffer[2];
+ const auto y = (static_cast(buffer[5]) << 8) | buffer[4];
+ const auto z = (static_cast(buffer[7]) << 8) | buffer[6];
+
+ /* Assign to Quaternion */
+ /* See http://ae-bst.resource.bosch.com/media/products/dokumente/bno055/BST_Address::DS000_12~1.pdf
+ 3.6.5.5 Orientation (Quaternion) */
+ const double scale = 1.0 / (1 << 14);
+ return imu::Quaternion(scale * w, scale * x, scale * y, scale * z);
+ }
+
+ int8_t get_temperature () const {
+ return this->m_bus->get(this->m_address, convert(Address::TEMP));
+ }
+
+ /* Functions to deal with raw calibration data */
+ bool get_sensor_offsets (uint8_t *calibrationData) {
+ if (is_fully_calibrated()) {
+ const OperationMode lastMode = this->_mode;
+ this->set_mode(OperationMode::CONFIG);
+
+ const auto bytes = static_cast(
+ convert(Address::MAG_RADIUS_MSB)
+ - convert(Address::ACCEL_OFFSET_X_LSB)
+ + 1
+ );
+ this->m_bus->get(this->m_address, convert(Address::ACCEL_OFFSET_X_LSB), calibrationData, bytes);
+
+ this->set_mode(lastMode);
+ return true;
+ } else
+ return false;
+ }
+
+ bool get_sensor_offsets (Offsets &offsets) {
+ if (is_fully_calibrated()) {
+ const OperationMode lastMode = this->_mode;
+ this->set_mode(OperationMode::CONFIG);
+ waitcnt(25 * MILLISECOND + CNT);
+
+ const auto bus = this->m_bus;
+ offsets.accel_x = (bus->get(this->m_address, convert(Address::ACCEL_OFFSET_X_MSB)) << 8)
+ | bus->get(this->m_address, convert(Address::ACCEL_OFFSET_X_LSB));
+ offsets.accel_y = (bus->get(this->m_address, convert(Address::ACCEL_OFFSET_Y_MSB)) << 8)
+ | bus->get(this->m_address, convert(Address::ACCEL_OFFSET_Y_LSB));
+ offsets.accel_z = (bus->get(this->m_address, convert(Address::ACCEL_OFFSET_Z_MSB)) << 8)
+ | bus->get(this->m_address, convert(Address::ACCEL_OFFSET_Z_LSB));
+
+ offsets.gyro_x = (bus->get(this->m_address, convert(Address::GYRO_OFFSET_X_MSB)) << 8)
+ | bus->get(this->m_address, convert(Address::GYRO_OFFSET_X_LSB));
+ offsets.gyro_y = (bus->get(this->m_address, convert(Address::GYRO_OFFSET_Y_MSB)) << 8)
+ | bus->get(this->m_address, convert(Address::GYRO_OFFSET_Y_LSB));
+ offsets.gyro_z = (bus->get(this->m_address, convert(Address::GYRO_OFFSET_Z_MSB)) << 8)
+ | bus->get(this->m_address, convert(Address::GYRO_OFFSET_Z_LSB));
+
+ offsets.mag_x = (bus->get(this->m_address, convert(Address::MAG_OFFSET_X_MSB)) << 8)
+ | bus->get(this->m_address, convert(Address::MAG_OFFSET_X_LSB));
+ offsets.mag_y = (bus->get(this->m_address, convert(Address::MAG_OFFSET_Y_MSB)) << 8)
+ | bus->get(this->m_address, convert(Address::MAG_OFFSET_Y_LSB));
+ offsets.mag_z = (bus->get(this->m_address, convert(Address::MAG_OFFSET_Z_MSB)) << 8)
+ | bus->get(this->m_address, convert(Address::MAG_OFFSET_Z_LSB));
+
+ offsets.accel_radius = (bus->get(this->m_address, convert(Address::ACCEL_RADIUS_MSB)) << 8)
+ | bus->get(this->m_address, convert(Address::ACCEL_RADIUS_LSB));
+ offsets.mag_radius = (bus->get(this->m_address, convert(Address::MAG_RADIUS_MSB)) << 8)
+ | bus->get(this->m_address, convert(Address::MAG_RADIUS_LSB));
+
+ this->set_mode(lastMode);
+ return true;
+ } else
+ return false;
+ }
+
+ void set_sensor_offsets (const uint8_t *calibrationData) {
+ const OperationMode lastMode = this->_mode;
+ this->set_mode(OperationMode::CONFIG);
+ waitcnt(25 * MILLISECOND + CNT);
+
+ /* A writeLen() would make this much cleaner */
+ this->m_bus->put(this->m_address, convert(Address::ACCEL_OFFSET_X_LSB), calibrationData[0]);
+ this->m_bus->put(this->m_address, convert(Address::ACCEL_OFFSET_X_MSB), calibrationData[1]);
+ this->m_bus->put(this->m_address, convert(Address::ACCEL_OFFSET_Y_LSB), calibrationData[2]);
+ this->m_bus->put(this->m_address, convert(Address::ACCEL_OFFSET_Y_MSB), calibrationData[3]);
+ this->m_bus->put(this->m_address, convert(Address::ACCEL_OFFSET_Z_LSB), calibrationData[4]);
+ this->m_bus->put(this->m_address, convert(Address::ACCEL_OFFSET_Z_MSB), calibrationData[5]);
+
+ this->m_bus->put(this->m_address, convert(Address::GYRO_OFFSET_X_LSB), calibrationData[6]);
+ this->m_bus->put(this->m_address, convert(Address::GYRO_OFFSET_X_MSB), calibrationData[7]);
+ this->m_bus->put(this->m_address, convert(Address::GYRO_OFFSET_Y_LSB), calibrationData[8]);
+ this->m_bus->put(this->m_address, convert(Address::GYRO_OFFSET_Y_MSB), calibrationData[9]);
+ this->m_bus->put(this->m_address, convert(Address::GYRO_OFFSET_Z_LSB), calibrationData[10]);
+ this->m_bus->put(this->m_address, convert(Address::GYRO_OFFSET_Z_MSB), calibrationData[11]);
+
+ this->m_bus->put(this->m_address, convert(Address::MAG_OFFSET_X_LSB), calibrationData[12]);
+ this->m_bus->put(this->m_address, convert(Address::MAG_OFFSET_X_MSB), calibrationData[13]);
+ this->m_bus->put(this->m_address, convert(Address::MAG_OFFSET_Y_LSB), calibrationData[14]);
+ this->m_bus->put(this->m_address, convert(Address::MAG_OFFSET_Y_MSB), calibrationData[15]);
+ this->m_bus->put(this->m_address, convert(Address::MAG_OFFSET_Z_LSB), calibrationData[16]);
+ this->m_bus->put(this->m_address, convert(Address::MAG_OFFSET_Z_MSB), calibrationData[17]);
+
+ this->m_bus->put(this->m_address, convert(Address::ACCEL_RADIUS_LSB), calibrationData[18]);
+ this->m_bus->put(this->m_address, convert(Address::ACCEL_RADIUS_MSB), calibrationData[19]);
+
+ this->m_bus->put(this->m_address, convert(Address::MAG_RADIUS_LSB), calibrationData[20]);
+ this->m_bus->put(this->m_address, convert(Address::MAG_RADIUS_MSB), calibrationData[21]);
+
+ this->set_mode(lastMode);
+ }
+
+ void set_sensor_offsets (const Offsets &offsets) {
+ const OperationMode lastMode = this->_mode;
+ this->set_mode(OperationMode::CONFIG);
+ waitcnt(25 * MILLISECOND + CNT);
+
+ const auto bus = this->m_bus;
+ bus->put(this->m_address, convert(Address::ACCEL_OFFSET_X_LSB), static_cast(offsets.accel_x));
+ bus->put(this->m_address, convert(Address::ACCEL_OFFSET_X_MSB), static_cast(offsets.accel_x >> 8));
+ bus->put(this->m_address, convert(Address::ACCEL_OFFSET_Y_LSB), static_cast(offsets.accel_y));
+ bus->put(this->m_address, convert(Address::ACCEL_OFFSET_Y_MSB), static_cast(offsets.accel_y >> 8));
+ bus->put(this->m_address, convert(Address::ACCEL_OFFSET_Z_LSB), static_cast(offsets.accel_z));
+ bus->put(this->m_address, convert(Address::ACCEL_OFFSET_Z_MSB), static_cast(offsets.accel_z >> 8));
+
+ bus->put(this->m_address, convert(Address::GYRO_OFFSET_X_LSB), static_cast(offsets.gyro_x));
+ bus->put(this->m_address, convert(Address::GYRO_OFFSET_X_MSB), static_cast(offsets.gyro_x >> 8));
+ bus->put(this->m_address, convert(Address::GYRO_OFFSET_Y_LSB), static_cast(offsets.gyro_y));
+ bus->put(this->m_address, convert(Address::GYRO_OFFSET_Y_MSB), static_cast(offsets.gyro_y >> 8));
+ bus->put(this->m_address, convert(Address::GYRO_OFFSET_Z_LSB), static_cast(offsets.gyro_z));
+ bus->put(this->m_address, convert(Address::GYRO_OFFSET_Z_MSB), static_cast(offsets.gyro_z >> 8));
+
+ bus->put(this->m_address, convert(Address::MAG_OFFSET_X_LSB), static_cast(offsets.mag_x));
+ bus->put(this->m_address, convert(Address::MAG_OFFSET_X_MSB), static_cast(offsets.mag_x >> 8));
+ bus->put(this->m_address, convert(Address::MAG_OFFSET_Y_LSB), static_cast(offsets.mag_y));
+ bus->put(this->m_address, convert(Address::MAG_OFFSET_Y_MSB), static_cast(offsets.mag_y >> 8));
+ bus->put(this->m_address, convert(Address::MAG_OFFSET_Z_LSB), static_cast(offsets.mag_z));
+ bus->put(this->m_address, convert(Address::MAG_OFFSET_Z_MSB), static_cast(offsets.mag_z >> 8));
+
+ bus->put(this->m_address, convert(Address::ACCEL_RADIUS_LSB), static_cast(offsets.accel_radius));
+ bus->put(this->m_address,
+ convert(Address::ACCEL_RADIUS_MSB),
+ static_cast(offsets.accel_radius >> 8));
+
+ bus->put(this->m_address, convert(Address::MAG_RADIUS_LSB), static_cast(offsets.mag_radius));
+ bus->put(this->m_address, convert(Address::MAG_RADIUS_MSB), static_cast(offsets.mag_radius >> 8));
+
+ this->set_mode(lastMode);
+ }
+
+ bool is_fully_calibrated () const {
+ uint8_t system, gyro, accel, mag;
+ this->get_calibration(&system, &gyro, &accel, &mag);
+ return !(system < 3 || gyro < 3 || accel < 3 || mag < 3);
+ }
+
+ private:
+ const I2CMaster *m_bus;
+ const uint8_t m_address;
+ OperationMode _mode;
+};
+
+}
diff --git a/PropWare/serial/i2c/i2cmaster.h b/PropWare/serial/i2c/i2cmaster.h
index ef7abc28..f93513c0 100644
--- a/PropWare/serial/i2c/i2cmaster.h
+++ b/PropWare/serial/i2c/i2cmaster.h
@@ -26,6 +26,7 @@
#pragma once
+#include
#include
namespace PropWare {
@@ -272,7 +273,7 @@ class I2CMaster {
bool ping (const uint8_t device) const {
this->start();
bool result = this->send_byte(device);
- this->stop();
+ //this->stop();
return result;
}
diff --git a/PropWare/utility/ahrs/matrix.h b/PropWare/utility/ahrs/matrix.h
new file mode 100644
index 00000000..cb2cbd56
--- /dev/null
+++ b/PropWare/utility/ahrs/matrix.h
@@ -0,0 +1,189 @@
+/**
+ * @file PropWare/utility/imu/matrix.h
+ *
+ * @author David Zemon (Only modified for use in PropWare)
+ *
+ * @copyright
+ * 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 .
+ */
+
+#pragma once
+
+#include
+#include
+
+#include
+
+namespace PropWare {
+
+namespace imu {
+
+template
+class Matrix {
+ public:
+ Matrix () {
+ memset(m_matrix, 0, N * N * sizeof(double));
+ }
+
+ Matrix (const Matrix &m) {
+ for (size_t ij = 0; ij < N * N; ++ij)
+ this->m_matrix[ij] = m.m_matrix[ij];
+ }
+
+ Matrix &operator= (const Matrix &m) {
+ for (size_t ij = 0; ij < N * N; ++ij)
+ this->m_matrix[ij] = m.m_matrix[ij];
+ return *this;
+ }
+
+ Vector row_to_vector (const size_t i) const {
+ Vector ret;
+ for (size_t j = 0; j < N; j++)
+ ret[j] = this->cell(i, j);
+ return ret;
+ }
+
+ Vector col_to_vector (const size_t j) const {
+ Vector ret;
+ for (size_t i = 0; i < N; i++)
+ ret[i] = this->cell(i, j);
+ return ret;
+ }
+
+ void vector_to_row (const Vector &v, const size_t i) {
+ for (size_t j = 0; j < N; j++)
+ this->cell(i, j) = v[j];
+ }
+
+ void vector_to_col (const Vector &v, const size_t j) {
+ for (size_t i = 0; i < N; i++)
+ this->cell(i, j) = v[i];
+ }
+
+ double operator() (const size_t i, const size_t j) const {
+ return this->cell(i, j);
+ }
+ double &operator() (const size_t i, const size_t j) {
+ return this->cell(i, j);
+ }
+
+ double cell (const size_t i, const size_t j) const {
+ return this->m_matrix[i * N + j];
+ }
+ double &cell (const size_t i, const size_t j) {
+ return this->m_matrix[i * N + j];
+ }
+
+
+ Matrix operator+ (const Matrix &m) const {
+ Matrix ret;
+ for (size_t ij = 0; ij < N * N; ++ij)
+ ret.m_matrix[ij] = this->m_matrix[ij] + m.m_matrix[ij];
+ return ret;
+ }
+
+ Matrix operator- (const Matrix &m) const {
+ Matrix ret;
+ for (size_t ij = 0; ij < N * N; ++ij)
+ ret.m_matrix[ij] = this->m_matrix[ij] - m.m_matrix[ij];
+ return ret;
+ }
+
+ Matrix operator* (double scalar) const {
+ Matrix ret;
+ for (size_t ij = 0; ij < N * N; ++ij)
+ ret.m_matrix[ij] = this->m_matrix[ij] * scalar;
+ return ret;
+ }
+
+ Matrix operator* (const Matrix &m) const {
+ Matrix ret;
+ for (size_t i = 0; i < N; i++) {
+ Vector row = this->row_to_vector(i);
+ for (size_t j = 0; j < N; j++)
+ ret(i, j) = row.dot(m.col_to_vector(j));
+ }
+ return ret;
+ }
+
+ Matrix transpose () const {
+ Matrix ret;
+ for (size_t i = 0; i < N; i++)
+ for (size_t j = 0; j < N; j++)
+ ret(j, i) = this->cell(i, j);
+ return ret;
+ }
+
+ Matrix minor_matrix (const size_t row, const size_t col) const {
+ Matrix ret;
+ for (size_t i = 0, im = 0; i < N; i++) {
+ if (i == row)
+ continue;
+
+ for (size_t j = 0, jm = 0; j < N; j++)
+ if (j != col)
+ ret(im, jm++) = this->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 (size_t i = 0; i < N; ++i, sign = -sign)
+ det += sign * this->cell(0, i) * this->minor_matrix(0, i).determinant();
+ return det;
+ }
+
+ Matrix invert () const {
+ Matrix ret;
+ double det = this->determinant();
+
+ for (size_t i = 0; i < N; i++)
+ for (size_t j = 0; j < N; j++) {
+ ret(i, j) = this->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 (size_t i = 0; i < N; ++i)
+ tr += this->cell(i, i);
+ return tr;
+ }
+
+ private:
+ double m_matrix[N * N];
+};
+
+
+template<>
+inline double Matrix<1>::determinant () const {
+ return this->cell(0, 0);
+}
+
+};
+
+}
diff --git a/PropWare/utility/ahrs/quaternion.h b/PropWare/utility/ahrs/quaternion.h
new file mode 100644
index 00000000..c14e3c26
--- /dev/null
+++ b/PropWare/utility/ahrs/quaternion.h
@@ -0,0 +1,245 @@
+/*
+ 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 .
+*/
+
+
+#pragma once
+
+#include
+#include
+#include
+#include
+
+#include
+
+namespace PropWare {
+
+namespace imu {
+
+class Quaternion {
+ public:
+ Quaternion ()
+ : m_w(1.0),
+ m_x(0.0),
+ m_y(0.0),
+ m_z(0.0) {
+ }
+
+ Quaternion (const double w, const double x, const double y, const double z)
+ : m_w(w),
+ m_x(x),
+ m_y(y),
+ m_z(z) {
+ }
+
+ Quaternion (const double w, const Vector<3> &vec)
+ : m_w(w),
+ m_x(vec.x()),
+ m_y(vec.y()),
+ m_z(vec.z()) {
+ }
+
+ double w () const {
+ return this->m_w;
+ }
+ double x () const {
+ return this->m_x;
+ }
+ double y () const {
+ return this->m_y;
+ }
+ double z () const {
+ return this->m_z;
+ }
+
+ double magnitude () const {
+ return sqrt(
+ m_w * this->m_w
+ + this->m_x * this->m_x
+ + this->m_y * this->m_y
+ + this->m_z * this->m_z
+ );
+ }
+
+ void normalize () {
+ double mag = magnitude();
+ *this = this->scale(1 / mag);
+ }
+
+ Quaternion conjugate () const {
+ return Quaternion(m_w, -m_x, -m_y, -m_z);
+ }
+
+ void from_axis_angle (const Vector<3> &axes, const double theta) {
+ this->m_w = cos(theta / 2);
+ //only need to calculate sine of half theta once
+ const double sht = sin(theta / 2);
+ this->m_x = axes.x() * sht;
+ this->m_y = axes.y() * sht;
+ this->m_z = axes.z() * sht;
+ }
+
+ void from_matrix (const Matrix<3> &m) {
+ double tr = m.trace();
+
+ double S;
+ if (tr > 0) {
+ S = sqrt(tr + 1.0) * 2;
+ this->m_w = 0.25 * S;
+ this->m_x = (m(2, 1) - m(1, 2)) / S;
+ this->m_y = (m(0, 2) - m(2, 0)) / S;
+ this->m_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;
+ this->m_w = (m(2, 1) - m(1, 2)) / S;
+ this->m_x = 0.25 * S;
+ this->m_y = (m(0, 1) + m(1, 0)) / S;
+ this->m_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;
+ this->m_w = (m(0, 2) - m(2, 0)) / S;
+ this->m_x = (m(0, 1) + m(1, 0)) / S;
+ this->m_y = 0.25 * S;
+ this->m_z = (m(1, 2) + m(2, 1)) / S;
+ } else {
+ S = sqrt(1.0 + m(2, 2) - m(0, 0) - m(1, 1)) * 2;
+ this->m_w = (m(1, 0) - m(0, 1)) / S;
+ this->m_x = (m(0, 2) + m(2, 0)) / S;
+ this->m_y = (m(1, 2) + m(2, 1)) / S;
+ this->m_z = 0.25 * S;
+ }
+ }
+
+ void to_axis_angle (Vector<3> &axes, double &angle) const {
+ double sqw = sqrt(1 - this->m_w * this->m_w);
+ if (sqw == 0) //it's a singularity and divide by zero, avoid
+ return;
+
+ angle = 2 * acos(m_w);
+ axes.x() = this->m_x / sqw;
+ axes.y() = this->m_y / sqw;
+ axes.z() = this->m_z / sqw;
+ }
+
+ Matrix<3> to_matrix () const {
+ Matrix<3> ret;
+ ret.cell(0, 0) = 1 - 2 * this->m_y * this->m_y - 2 * this->m_z * this->m_z;
+ ret.cell(0, 1) = 2 * this->m_x * this->m_y - 2 * this->m_w * this->m_z;
+ ret.cell(0, 2) = 2 * this->m_x * this->m_z + 2 * this->m_w * this->m_y;
+
+ ret.cell(1, 0) = 2 * this->m_x * this->m_y + 2 * this->m_w * this->m_z;
+ ret.cell(1, 1) = 1 - 2 * this->m_x * this->m_x - 2 * this->m_z * this->m_z;
+ ret.cell(1, 2) = 2 * this->m_y * this->m_z - 2 * this->m_w * this->m_x;
+
+ ret.cell(2, 0) = 2 * this->m_x * this->m_z - 2 * this->m_w * this->m_y;
+ ret.cell(2, 1) = 2 * this->m_y * this->m_z + 2 * this->m_w * this->m_x;
+ ret.cell(2, 2) = 1 - 2 * this->m_x * this->m_x - 2 * this->m_y * this->m_y;
+ return ret;
+ }
+
+ /**
+ * @brief 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> to_euler () const {
+ const auto sqw = this->m_w * this->m_w;
+ const auto sqx = this->m_x * this->m_x;
+ const auto sqy = this->m_y * this->m_y;
+ const auto sqz = this->m_z * this->m_z;
+
+ return Vector<3>(
+ atan2(2.0 * (m_x * this->m_y + this->m_z * this->m_w), (sqx - sqy - sqz + sqw)),
+ asin(-2.0 * (m_x * this->m_z - this->m_y * this->m_w) / (sqx + sqy + sqz + sqw)),
+ atan2(2.0 * (m_y * this->m_z + this->m_x * this->m_w), (-sqx - sqy + sqz + sqw))
+ );
+ }
+
+ Vector<3> to_angular_velocity (double dt) const {
+ Vector<3> ret;
+ const Quaternion one(1.0, 0.0, 0.0, 0.0);
+ const 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> rotate_vector (const Vector<2> &v) const {
+ return this->rotate_vector(Vector<3>(v.x(), v.y()));
+ }
+
+ Vector<3> rotate_vector (const Vector<3> &v) const {
+ Vector<3> qv(m_x, this->m_y, this->m_z);
+ Vector<3> t = qv.cross(v) * 2.0;
+ return v + t * this->m_w + qv.cross(t);
+ }
+
+
+ Quaternion operator* (const Quaternion &q) const {
+ return Quaternion(
+ this->m_w * q.m_w - this->m_x * q.m_x - this->m_y * q.m_y - this->m_z * q.m_z,
+ this->m_w * q.m_x + this->m_x * q.m_w + this->m_y * q.m_z - this->m_z * q.m_y,
+ this->m_w * q.m_y - this->m_x * q.m_z + this->m_y * q.m_w + this->m_z * q.m_x,
+ this->m_w * q.m_z + this->m_x * q.m_y - this->m_y * q.m_x + this->m_z * q.m_w
+ );
+ }
+
+ Quaternion operator+ (const Quaternion &q) const {
+ return Quaternion(m_w + q.m_w, this->m_x + q.m_x, this->m_y + q.m_y, this->m_z + q.m_z);
+ }
+
+ Quaternion operator- (const Quaternion &q) const {
+ return Quaternion(m_w - q.m_w, this->m_x - q.m_x, this->m_y - q.m_y, this->m_z - q.m_z);
+ }
+
+ Quaternion operator/ (const double scalar) const {
+ return Quaternion(m_w / scalar, this->m_x / scalar, this->m_y / scalar, this->m_z / scalar);
+ }
+
+ Quaternion operator* (const double scalar) const {
+ return scale(scalar);
+ }
+
+ Quaternion scale (const double scalar) const {
+ return Quaternion(m_w * scalar, this->m_x * scalar, this->m_y * scalar, this->m_z * scalar);
+ }
+
+ private:
+ double m_w;
+ double m_x;
+ double m_y;
+ double m_z;
+};
+
+}
+
+}
diff --git a/PropWare/utility/ahrs/vector.h b/PropWare/utility/ahrs/vector.h
new file mode 100644
index 00000000..7033995f
--- /dev/null
+++ b/PropWare/utility/ahrs/vector.h
@@ -0,0 +1,225 @@
+/**
+ * @file PropWare/utility/imu/vector.h
+ *
+ * @author David Zemon (Only modified for use in PropWare)
+ *
+ * @copyright
+ * 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 .
+ */
+
+#pragma once
+
+#include
+#include
+#include
+
+namespace PropWare {
+
+namespace imu {
+
+template
+class Vector {
+ public:
+ Vector () {
+ memset(m_vector, 0, sizeof(double) * N);
+ }
+
+ Vector (double a) {
+ memset(m_vector, 0, sizeof(double) * N);
+
+ static_assert(1 <= N, "Size must be greater than or equal to 1");
+ this->m_vector[0] = a;
+ }
+
+ Vector (double a, double b) {
+ static_assert(2 <= N, "Size must be greater than or equal to 2");
+
+ memset(m_vector, 0, sizeof(double) * N);
+ this->m_vector[0] = a;
+ this->m_vector[1] = b;
+ }
+
+ Vector (double a, double b, double c) {
+ static_assert(3 <= N, "Size must be greater than or equal to 3");
+
+ memset(m_vector, 0, sizeof(double) * N);
+ this->m_vector[0] = a;
+ this->m_vector[1] = b;
+ this->m_vector[2] = c;
+ }
+
+ Vector (double a, double b, double c, double d) {
+ static_assert(4 <= N, "Size must be greater than or equal to 4");
+
+ memset(m_vector, 0, sizeof(double) * N);
+ this->m_vector[0] = a;
+ this->m_vector[1] = b;
+ this->m_vector[2] = c;
+ this->m_vector[3] = d;
+ }
+
+ Vector (const Vector &v) {
+ for (size_t x = 0; x < N; x++)
+ this->m_vector[x] = v.m_vector[x];
+ }
+
+ size_t n () const {
+ return N;
+ }
+
+ double magnitude () const {
+ double res = 0;
+ for (size_t i = 0; i < N; ++i)
+ res += this->m_vector[i] * this->m_vector[i];
+
+ return sqrt(res);
+ }
+
+ void normalize () {
+ double mag = this->magnitude();
+ if (isnan(mag) || mag == 0.0)
+ return;
+
+ for (size_t i = 0; i < N; ++i)
+ this->m_vector[i] /= mag;
+ }
+
+ double dot (const Vector &v) const {
+ double ret = 0;
+ for (size_t i = 0; i < N; ++i)
+ ret += this->m_vector[i] * v.m_vector[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 (size_t i = 0; i < N; ++i)
+ ret.m_vector[i] = this->m_vector[i] * scalar;
+ return ret;
+ }
+
+ Vector invert () const {
+ Vector ret;
+ for (size_t i = 0; i < N; ++i)
+ ret.m_vector[i] = -m_vector[i];
+ return ret;
+ }
+
+ Vector &operator= (const Vector &v) {
+ for (size_t x = 0; x < N; x++)
+ this->m_vector[x] = v.m_vector[x];
+ return *this;
+ }
+
+ double &operator[] (const size_t n) {
+ return this->m_vector[n];
+ }
+
+ double operator[] (const size_t n) const {
+ return this->m_vector[n];
+ }
+
+ double &operator() (const size_t n) {
+ return this->m_vector[n];
+ }
+
+ double operator() (const size_t n) const {
+ return this->m_vector[n];
+ }
+
+ Vector operator+ (const Vector &v) const {
+ Vector ret;
+ for (size_t i = 0; i < N; ++i)
+ ret.m_vector[i] = this->m_vector[i] + v.m_vector[i];
+ return ret;
+ }
+
+ Vector operator- (const Vector &v) const {
+ Vector ret;
+ for (size_t i = 0; i < N; ++i)
+ ret.m_vector[i] = this->m_vector[i] - v.m_vector[i];
+ return ret;
+ }
+
+ Vector operator* (double scalar) const {
+ return scale(scalar);
+ }
+
+ Vector operator/ (double scalar) const {
+ Vector ret;
+ for (size_t i = 0; i < N; ++i)
+ ret.m_vector[i] = this->m_vector[i] / scalar;
+ return ret;
+ }
+
+ void to_degrees () {
+ for (size_t i = 0; i < N; ++i)
+ this->m_vector[i] *= 180 / M_PI;
+ }
+
+ void to_radians () {
+ for (size_t i = 0; i < N; ++i)
+ this->m_vector[i] *= M_PI / 180;
+ }
+
+ double &x () {
+ return this->m_vector[0];
+ }
+ double &y () {
+ return this->m_vector[1];
+ }
+ double &z () {
+ return this->m_vector[2];
+ }
+ double x () const {
+ return this->m_vector[0];
+ }
+ double y () const {
+ return this->m_vector[1];
+ }
+ double z () const {
+ return this->m_vector[2];
+ }
+
+
+ private:
+ double m_vector[N];
+};
+
+
+template<>
+inline Vector<3> Vector<3>::cross (const Vector &v) const {
+ return Vector(
+ this->m_vector[1] * v.m_vector[2] - this->m_vector[2] * v.m_vector[1],
+ this->m_vector[2] * v.m_vector[0] - this->m_vector[0] * v.m_vector[2],
+ this->m_vector[0] * v.m_vector[1] - this->m_vector[1] * v.m_vector[0]
+ );
+}
+
+}
+
+}