From 1b9e6d6da734f201c7c90757b38ca05d71a0f1b0 Mon Sep 17 00:00:00 2001 From: David Zemon Date: Sat, 23 Dec 2017 07:18:50 -0600 Subject: [PATCH] [#133] WIP --- Examples/PropWare_BNO055/BNO055_Demo.cpp | 77 ++- PropWare/CMakeLists.txt | 3 + PropWare/sensor/fusion/bno055.h | 835 ++++++++++++++++------- PropWare/serial/i2c/i2cmaster.h | 3 +- PropWare/utility/ahrs/matrix.h | 189 +++++ PropWare/utility/ahrs/quaternion.h | 245 +++++++ PropWare/utility/ahrs/vector.h | 225 ++++++ 7 files changed, 1338 insertions(+), 239 deletions(-) create mode 100644 PropWare/utility/ahrs/matrix.h create mode 100644 PropWare/utility/ahrs/quaternion.h create mode 100644 PropWare/utility/ahrs/vector.h diff --git a/Examples/PropWare_BNO055/BNO055_Demo.cpp b/Examples/PropWare_BNO055/BNO055_Demo.cpp index 85ad9513..57197d6a 100644 --- a/Examples/PropWare_BNO055/BNO055_Demo.cpp +++ b/Examples/PropWare_BNO055/BNO055_Demo.cpp @@ -24,9 +24,37 @@ */ #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 @@ -35,10 +63,53 @@ using PropWare::BNO055; * @include PropWare_BNO055/CMakeLists.txt */ int main () { - const I2CMaster i2cBus; - const BNO055 sensor(i2cBus); + 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(); - return 0; + 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/PropWare/CMakeLists.txt b/PropWare/CMakeLists.txt index b4af7921..70d0100e 100644 --- a/PropWare/CMakeLists.txt +++ b/PropWare/CMakeLists.txt @@ -55,6 +55,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/fusion/bno055.h b/PropWare/sensor/fusion/bno055.h index ed03a866..d7d9631a 100644 --- a/PropWare/sensor/fusion/bno055.h +++ b/PropWare/sensor/fusion/bno055.h @@ -1,7 +1,7 @@ /** * @file PropWare/sensor/fusion/bno055.h * - * @author David Zemon + * @author David Zemon (Only modified for use in PropWare) * * @copyright * The MIT License (MIT)
@@ -26,201 +26,205 @@ #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 ADDRESS_A = 0x28; - static const uint8_t ADDRESS_B = 0x28; + static const uint8_t DEVICE_ADDRESS_A = 0x28 << 1; + static const uint8_t DEVICE_ADDRESS_B = 0x29 << 1; static const uint8_t ID = 0xA0; - struct Offsets { - 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; + 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 { - /* 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 - } Address; - typedef enum { POWER_MODE_NORMAL = 0X00, POWER_MODE_LOWPOWER = 0X01, POWER_MODE_SUSPEND = 0X02 } PowerMode; - 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 - } OperationMode; + 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, @@ -234,31 +238,23 @@ class BNO055 { } AxisRemapConfiguration; 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 + P0 = 0x04, + P1 = 0x00, // default + P2 = 0x06, + P3 = 0x02, + P4 = 0x03, + P5 = 0x01, + P6 = 0x07, + P7 = 0x05 } AxisRemapSign; - typedef struct { - uint8_t accel_rev; - uint8_t mag_rev; - uint8_t gyro_rev; - uint16_t sw_rev; - uint8_t bl_rev; - } RevisionInfo; - 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 + 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 { @@ -268,38 +264,87 @@ class BNO055 { 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: - BNO055 (const uint8_t address = ADDRESS_A, const I2CMaster &bus = pwI2c) + 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) { } - PropWare::ErrorCode begin (OperationMode mode = OPERATION_MODE_NDOF) const { + 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 = pwI2c.get(this->m_address, BNO055_CHIP_ID_ADDR); + auto id = this->get_byte(Address::CHIP_ID); if (id != ID) { waitcnt(SECOND + CNT); // hold on for boot - id = pwI2c.get(this->m_address, BNO055_CHIP_ID_ADDR); + 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) */ - setMode(OPERATION_MODE_CONFIG); + if (!this->set_mode(OperationMode::CONFIG)) { + pwOut << "fail 1\n"; + return FAILED_TO_BOOT; + } /* Reset */ - this->m_bus->put(this->m_address, BNO055_SYS_TRIGGER_ADDR, 0x20); - while (this->m_bus->get(this->m_address, BNO055_CHIP_ID_ADDR) != ID) { - waitcnt(10 * MILLISECOND + CNT); + 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 */ - this->m_bus->put(this->m_address, BNO055_PWR_MODE_ADDR, POWER_MODE_NORMAL); + 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); - this->m_bus->put(this->m_address, BNO055_PAGE_ID_ADDR, 0); + 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 */ /* @@ -308,51 +353,371 @@ class BNO055 { (0 << 2) | // Euler = Degrees (1 << 1) | // Gyro = Rads (0 << 0); // Accelerometer = m/s^2 - write8(BNO055_UNIT_SEL_ADDR, unitsel); + write8(Address::UNIT_SEL, unitsel); */ /* Configure axis mapping (see section 3.4) */ /* - write8(BNO055_AXIS_MAP_CONFIG_ADDR, REMAP_CONFIG_P2); // P0-P7, Default is P1 + write8(Address::AXIS_MAP_CONFIG, REMAP_CONFIG_P2); // P0-P7, Default is P1 delay(10); - write8(BNO055_AXIS_MAP_SIGN_ADDR, REMAP_SIGN_P2); // P0-P7, Default is P1 + write8(Address::AXIS_MAP_SIGN, REMAP_SIGN_P2); // P0-P7, Default is P1 delay(10); */ - this->m_bus->put(this->m_address, BNO055_SYS_TRIGGER_ADDR, 0x0); + 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) */ - setMode(mode); + if (!this->set_mode(mode)) { + pwOut << "fail 6\n"; + return FAILED_TO_BOOT; + } waitcnt(20 * MILLISECOND + CNT); return NO_ERROR; } - void setMode (OperationMode mode) const; - void getRevInfo (RevisionInfo *) const; - void displayRevInfo (void) const; - void setExtCrystalUse (bool usextal) const; - void getSystemStatus (uint8_t *system_status, - uint8_t *self_test_result, - uint8_t *system_error) const; - void displaySystemStatus (void) const; - void getCalibration (uint8_t *system, uint8_t *gyro, uint8_t *accel, uint8_t *mag) const; + /** + * @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; + } - imu::Vector<3> getVector (VectorType vector_type) const; - imu::Quaternion getQuat (void) const; - int8_t getTemp (void) const; + 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 getSensorOffsets (uint8_t *calibData) const; - bool getSensorOffsets (Offsets &offsets_type) const; - void setSensorOffsets (const uint8_t *calibData) const; - void setSensorOffsets (const Offsets &offsets_type) const; - bool isFullyCalibrated (void) const; + 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] + ); +} + +} + +}