From 0e6364f3f6046741c50118c7c62fdbec3cff13da Mon Sep 17 00:00:00 2001 From: maruyama0630 <138867152+maruyama0630@users.noreply.github.com> Date: Mon, 18 Mar 2024 22:01:49 +0900 Subject: [PATCH] Add files via upload --- Main/Main/Adafruit_BNO055.cpp | 867 +++++++++++++++++++++++++++++++++ Main/Main/Adafruit_BNO055.h | 329 +++++++++++++ Main/Main/Main.ino | 547 +++++++++++++++++++++ Main/Main/utility/imumaths.h | 31 ++ Main/Main/utility/matrix.h | 185 +++++++ Main/Main/utility/quaternion.h | 214 ++++++++ Main/Main/utility/vector.h | 184 +++++++ 7 files changed, 2357 insertions(+) create mode 100644 Main/Main/Adafruit_BNO055.cpp create mode 100644 Main/Main/Adafruit_BNO055.h create mode 100644 Main/Main/Main.ino create mode 100644 Main/Main/utility/imumaths.h create mode 100644 Main/Main/utility/matrix.h create mode 100644 Main/Main/utility/quaternion.h create mode 100644 Main/Main/utility/vector.h diff --git a/Main/Main/Adafruit_BNO055.cpp b/Main/Main/Adafruit_BNO055.cpp new file mode 100644 index 0000000..cbe61c8 --- /dev/null +++ b/Main/Main/Adafruit_BNO055.cpp @@ -0,0 +1,867 @@ +/*! + * @file Adafruit_BNO055.cpp + * + * @mainpage Adafruit BNO055 Orientation Sensor + * + * @section intro_sec Introduction + * + * This is a library for the BNO055 orientation sensor + * + * Designed specifically to work with the Adafruit BNO055 9-DOF Breakout. + * + * Pick one up today in the adafruit shop! + * ------> https://www.adafruit.com/product/2472 + * + * 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! + * + * @section author Author + * + * K.Townsend (Adafruit Industries) + * + * @section license License + * + * MIT license, all text above must be included in any redistribution + */ + +#include "Arduino.h" + +#include +#include + +#include "Adafruit_BNO055.h" + +/*! + * @brief Instantiates a new Adafruit_BNO055 class + * @param sensorID + * sensor ID + * @param address + * i2c address + * @param theWire + * Wire object + */ +Adafruit_BNO055::Adafruit_BNO055(int32_t sensorID, uint8_t address, + TwoWire *theWire) { +// BNO055 clock stretches for 500us or more! +#ifdef ESP8266 + theWire->setClockStretchLimit(1000); // Allow for 1000us of clock stretching +#endif + + _sensorID = sensorID; + i2c_dev = new Adafruit_I2CDevice(address, theWire); +} + +/*! + * @brief Sets up the HW + * @param mode + * mode values + * [OPERATION_MODE_CONFIG, + * OPERATION_MODE_ACCONLY, + * OPERATION_MODE_MAGONLY, + * OPERATION_MODE_GYRONLY, + * OPERATION_MODE_ACCMAG, + * OPERATION_MODE_ACCGYRO, + * OPERATION_MODE_MAGGYRO, + * OPERATION_MODE_AMG, + * OPERATION_MODE_IMUPLUS, + * OPERATION_MODE_COMPASS, + * OPERATION_MODE_M4G, + * OPERATION_MODE_NDOF_FMC_OFF, + * OPERATION_MODE_NDOF] + * @return true if process is successful + */ +bool Adafruit_BNO055::begin(adafruit_bno055_opmode_t mode) { + // Start without a detection + i2c_dev->begin(false); + +#if defined(TARGET_RP2040) + // philhower core seems to work with this speed? + i2c_dev->setSpeed(50000); +#endif + + // can take 850 ms to boot! + int timeout = 850; // in ms + while (timeout > 0) { + if (i2c_dev->begin()) { + break; + } + // wasnt detected... we'll retry! + delay(10); + timeout -= 10; + } + if (timeout <= 0) + return false; + + /* Make sure we have the right device */ + uint8_t id = read8(BNO055_CHIP_ID_ADDR); + if (id != BNO055_ID) { + delay(1000); // hold on for boot + id = read8(BNO055_CHIP_ID_ADDR); + if (id != BNO055_ID) { + return false; // still not? ok bail + } + } + + /* Switch to config mode (just in case since this is the default) */ + setMode(OPERATION_MODE_CONFIG); + + /* Reset */ + write8(BNO055_SYS_TRIGGER_ADDR, 0x20); + /* Delay incrased to 30ms due to power issues https://tinyurl.com/y375z699 */ + delay(30); + while (read8(BNO055_CHIP_ID_ADDR) != BNO055_ID) { + delay(10); + } + delay(50); + + /* Set to normal power mode */ + write8(BNO055_PWR_MODE_ADDR, POWER_MODE_NORMAL); + delay(10); + + write8(BNO055_PAGE_ID_ADDR, 0); + + /* Set the output units */ + /* + uint8_t unitsel = (0 << 7) | // Orientation = Android + (0 << 4) | // Temperature = Celsius + (0 << 2) | // Euler = Degrees + (1 << 1) | // Gyro = Rads + (0 << 0); // Accelerometer = m/s^2 + write8(BNO055_UNIT_SEL_ADDR, unitsel); + */ + + /* Configure axis mapping (see section 3.4) */ + /* + write8(BNO055_AXIS_MAP_CONFIG_ADDR, REMAP_CONFIG_P2); // P0-P7, Default is P1 + delay(10); + write8(BNO055_AXIS_MAP_SIGN_ADDR, REMAP_SIGN_P2); // P0-P7, Default is P1 + delay(10); + */ + + write8(BNO055_SYS_TRIGGER_ADDR, 0x0); + delay(10); + /* Set the requested operating mode (see section 3.3) */ + setMode(mode); + delay(20); + + return true; +} + +/*! + * @brief Puts the chip in the specified operating mode + * @param mode + * mode values + * [OPERATION_MODE_CONFIG, + * OPERATION_MODE_ACCONLY, + * OPERATION_MODE_MAGONLY, + * OPERATION_MODE_GYRONLY, + * OPERATION_MODE_ACCMAG, + * OPERATION_MODE_ACCGYRO, + * OPERATION_MODE_MAGGYRO, + * OPERATION_MODE_AMG, + * OPERATION_MODE_IMUPLUS, + * OPERATION_MODE_COMPASS, + * OPERATION_MODE_M4G, + * OPERATION_MODE_NDOF_FMC_OFF, + * OPERATION_MODE_NDOF] + */ +void Adafruit_BNO055::setMode(adafruit_bno055_opmode_t mode) { + _mode = mode; + write8(BNO055_OPR_MODE_ADDR, _mode); + delay(30); +} + +/*! + * @brief Gets the current operating mode of the chip + * @return operating_mode in integer which can be mapped in Section 3.3 + * for example: a return of 12 (0X0C) => NDOF + */ +adafruit_bno055_opmode_t Adafruit_BNO055::getMode() { + return (adafruit_bno055_opmode_t)read8(BNO055_OPR_MODE_ADDR); +} + +/*! + * @brief Changes the chip's axis remap + * @param remapcode + * remap code possible values + * [REMAP_CONFIG_P0 + * REMAP_CONFIG_P1 (default) + * REMAP_CONFIG_P2 + * REMAP_CONFIG_P3 + * REMAP_CONFIG_P4 + * REMAP_CONFIG_P5 + * REMAP_CONFIG_P6 + * REMAP_CONFIG_P7] + */ +void Adafruit_BNO055::setAxisRemap( + adafruit_bno055_axis_remap_config_t remapcode) { + adafruit_bno055_opmode_t modeback = _mode; + + setMode(OPERATION_MODE_CONFIG); + delay(25); + write8(BNO055_AXIS_MAP_CONFIG_ADDR, remapcode); + delay(10); + /* Set the requested operating mode (see section 3.3) */ + setMode(modeback); + delay(20); +} + +/*! + * @brief Changes the chip's axis signs + * @param remapsign + * remap sign possible values + * [REMAP_SIGN_P0 + * REMAP_SIGN_P1 (default) + * REMAP_SIGN_P2 + * REMAP_SIGN_P3 + * REMAP_SIGN_P4 + * REMAP_SIGN_P5 + * REMAP_SIGN_P6 + * REMAP_SIGN_P7] + */ +void Adafruit_BNO055::setAxisSign(adafruit_bno055_axis_remap_sign_t remapsign) { + adafruit_bno055_opmode_t modeback = _mode; + + setMode(OPERATION_MODE_CONFIG); + delay(25); + write8(BNO055_AXIS_MAP_SIGN_ADDR, remapsign); + delay(10); + /* Set the requested operating mode (see section 3.3) */ + setMode(modeback); + delay(20); +} + +/*! + * @brief Use the external 32.768KHz crystal + * @param usextal + * use external crystal boolean + */ +void Adafruit_BNO055::setExtCrystalUse(boolean usextal) { + adafruit_bno055_opmode_t modeback = _mode; + + /* Switch to config mode (just in case since this is the default) */ + setMode(OPERATION_MODE_CONFIG); + delay(25); + write8(BNO055_PAGE_ID_ADDR, 0); + if (usextal) { + write8(BNO055_SYS_TRIGGER_ADDR, 0x80); + } else { + write8(BNO055_SYS_TRIGGER_ADDR, 0x00); + } + delay(10); + /* Set the requested operating mode (see section 3.3) */ + setMode(modeback); + delay(20); +} + +/*! + * @brief Gets the latest system status info + * @param system_status + * system status info + * @param self_test_result + * self test result + * @param system_error + * system error info + */ +void Adafruit_BNO055::getSystemStatus(uint8_t *system_status, + uint8_t *self_test_result, + uint8_t *system_error) { + write8(BNO055_PAGE_ID_ADDR, 0); + + /* System Status (see section 4.3.58) + 0 = Idle + 1 = System Error + 2 = Initializing Peripherals + 3 = System Iniitalization + 4 = Executing Self-Test + 5 = Sensor fusio algorithm running + 6 = System running without fusion algorithms + */ + + if (system_status != 0) + *system_status = read8(BNO055_SYS_STAT_ADDR); + + /* Self Test Results + 1 = test passed, 0 = test failed + + Bit 0 = Accelerometer self test + Bit 1 = Magnetometer self test + Bit 2 = Gyroscope self test + Bit 3 = MCU self test + + 0x0F = all good! + */ + + if (self_test_result != 0) + *self_test_result = read8(BNO055_SELFTEST_RESULT_ADDR); + + /* System Error (see section 4.3.59) + 0 = No error + 1 = Peripheral initialization error + 2 = System initialization error + 3 = Self test result failed + 4 = Register map value out of range + 5 = Register map address out of range + 6 = Register map write error + 7 = BNO low power mode not available for selected operat ion mode + 8 = Accelerometer power mode not available + 9 = Fusion algorithm configuration error + A = Sensor configuration error + */ + + if (system_error != 0) + *system_error = read8(BNO055_SYS_ERR_ADDR); + + delay(200); +} + +/*! + * @brief Gets the chip revision numbers + * @param info + * revision info + */ +void Adafruit_BNO055::getRevInfo(adafruit_bno055_rev_info_t *info) { + uint8_t a, b; + + memset(info, 0, sizeof(adafruit_bno055_rev_info_t)); + + /* Check the accelerometer revision */ + info->accel_rev = read8(BNO055_ACCEL_REV_ID_ADDR); + + /* Check the magnetometer revision */ + info->mag_rev = read8(BNO055_MAG_REV_ID_ADDR); + + /* Check the gyroscope revision */ + info->gyro_rev = read8(BNO055_GYRO_REV_ID_ADDR); + + /* Check the SW revision */ + info->bl_rev = read8(BNO055_BL_REV_ID_ADDR); + + a = read8(BNO055_SW_REV_ID_LSB_ADDR); + b = read8(BNO055_SW_REV_ID_MSB_ADDR); + info->sw_rev = (((uint16_t)b) << 8) | ((uint16_t)a); +} + +/*! + * @brief Gets current calibration state. Each value should be a uint8_t + * pointer and it will be set to 0 if not calibrated and 3 if + * fully calibrated. + * See section 34.3.54 + * @param sys + * Current system calibration status, depends on status of all sensors, + * read-only + * @param gyro + * Current calibration status of Gyroscope, read-only + * @param accel + * Current calibration status of Accelerometer, read-only + * @param mag + * Current calibration status of Magnetometer, read-only + */ +void Adafruit_BNO055::getCalibration(uint8_t *sys, uint8_t *gyro, + uint8_t *accel, uint8_t *mag) { + uint8_t calData = read8(BNO055_CALIB_STAT_ADDR); + if (sys != NULL) { + *sys = (calData >> 6) & 0x03; + } + if (gyro != NULL) { + *gyro = (calData >> 4) & 0x03; + } + if (accel != NULL) { + *accel = (calData >> 2) & 0x03; + } + if (mag != NULL) { + *mag = calData & 0x03; + } +} + +/*! + * @brief Gets the temperature in degrees celsius + * @return temperature in degrees celsius + */ +int8_t Adafruit_BNO055::getTemp() { + int8_t temp = (int8_t)(read8(BNO055_TEMP_ADDR)); + return temp; +} + +/*! + * @brief Gets a vector reading from the specified source + * @param vector_type + * possible vector type values + * [VECTOR_ACCELEROMETER + * VECTOR_MAGNETOMETER + * VECTOR_GYROSCOPE + * VECTOR_EULER + * VECTOR_LINEARACCEL + * VECTOR_GRAVITY] + * @return vector from specified source + */ +imu::Vector<3> Adafruit_BNO055::getVector(adafruit_vector_type_t vector_type) { + imu::Vector<3> xyz; + uint8_t buffer[6]; + memset(buffer, 0, 6); + + int16_t x, y, z; + x = y = z = 0; + + /* Read vector data (6 bytes) */ + readLen((adafruit_bno055_reg_t)vector_type, buffer, 6); + + x = ((int16_t)buffer[0]) | (((int16_t)buffer[1]) << 8); + y = ((int16_t)buffer[2]) | (((int16_t)buffer[3]) << 8); + z = ((int16_t)buffer[4]) | (((int16_t)buffer[5]) << 8); + + /*! + * Convert the value to an appropriate range (section 3.6.4) + * and assign the value to the Vector type + */ + switch (vector_type) { + case VECTOR_MAGNETOMETER: + /* 1uT = 16 LSB */ + xyz[0] = ((double)x) / 16.0; + xyz[1] = ((double)y) / 16.0; + xyz[2] = ((double)z) / 16.0; + break; + case VECTOR_GYROSCOPE: + /* 1dps = 16 LSB */ + xyz[0] = ((double)x) / 16.0; + xyz[1] = ((double)y) / 16.0; + xyz[2] = ((double)z) / 16.0; + break; + case VECTOR_EULER: + /* 1 degree = 16 LSB */ + xyz[0] = ((double)x) / 16.0; + xyz[1] = ((double)y) / 16.0; + xyz[2] = ((double)z) / 16.0; + break; + case VECTOR_ACCELEROMETER: + /* 1m/s^2 = 100 LSB */ + xyz[0] = ((double)x) / 100.0; + xyz[1] = ((double)y) / 100.0; + xyz[2] = ((double)z) / 100.0; + break; + case VECTOR_LINEARACCEL: + /* 1m/s^2 = 100 LSB */ + xyz[0] = ((double)x) / 100.0; + xyz[1] = ((double)y) / 100.0; + xyz[2] = ((double)z) / 100.0; + break; + case VECTOR_GRAVITY: + /* 1m/s^2 = 100 LSB */ + xyz[0] = ((double)x) / 100.0; + xyz[1] = ((double)y) / 100.0; + xyz[2] = ((double)z) / 100.0; + break; + } + + return xyz; +} + +/*! + * @brief Gets a quaternion reading from the specified source + * @return quaternion reading + */ +imu::Quaternion Adafruit_BNO055::getQuat() { + uint8_t buffer[8]; + memset(buffer, 0, 8); + + int16_t x, y, z, w; + x = y = z = w = 0; + + /* Read quat data (8 bytes) */ + readLen(BNO055_QUATERNION_DATA_W_LSB_ADDR, buffer, 8); + w = (((uint16_t)buffer[1]) << 8) | ((uint16_t)buffer[0]); + x = (((uint16_t)buffer[3]) << 8) | ((uint16_t)buffer[2]); + y = (((uint16_t)buffer[5]) << 8) | ((uint16_t)buffer[4]); + z = (((uint16_t)buffer[7]) << 8) | ((uint16_t)buffer[6]); + + /*! + * Assign to Quaternion + * See + * https://cdn-shop.adafruit.com/datasheets/BST_BNO055_DS000_12.pdf + * 3.6.5.5 Orientation (Quaternion) + */ + const double scale = (1.0 / (1 << 14)); + imu::Quaternion quat(scale * w, scale * x, scale * y, scale * z); + return quat; +} + +/*! + * @brief Provides the sensor_t data for this sensor + * @param sensor + * Sensor description + */ +void Adafruit_BNO055::getSensor(sensor_t *sensor) { + /* Clear the sensor_t object */ + memset(sensor, 0, sizeof(sensor_t)); + + /* Insert the sensor name in the fixed length char array */ + strncpy(sensor->name, "BNO055", sizeof(sensor->name) - 1); + sensor->name[sizeof(sensor->name) - 1] = 0; + sensor->version = 1; + sensor->sensor_id = _sensorID; + sensor->type = SENSOR_TYPE_ORIENTATION; + sensor->min_delay = 0; + sensor->max_value = 0.0F; + sensor->min_value = 0.0F; + sensor->resolution = 0.01F; +} + +/*! + * @brief Reads the sensor and returns the data as a sensors_event_t + * @param event + * Event description + * @return always returns true + */ +bool Adafruit_BNO055::getEvent(sensors_event_t *event) { + /* Clear the event */ + memset(event, 0, sizeof(sensors_event_t)); + + event->version = sizeof(sensors_event_t); + event->sensor_id = _sensorID; + event->type = SENSOR_TYPE_ORIENTATION; + event->timestamp = millis(); + + /* Get a Euler angle sample for orientation */ + imu::Vector<3> euler = getVector(Adafruit_BNO055::VECTOR_EULER); + event->orientation.x = euler.x(); + event->orientation.y = euler.y(); + event->orientation.z = euler.z(); + + return true; +} + +/*! + * @brief Reads the sensor and returns the data as a sensors_event_t + * @param event + * Event description + * @param vec_type + * specify the type of reading + * @return always returns true + */ +bool Adafruit_BNO055::getEvent(sensors_event_t *event, + adafruit_vector_type_t vec_type) { + /* Clear the event */ + memset(event, 0, sizeof(sensors_event_t)); + + event->version = sizeof(sensors_event_t); + event->sensor_id = _sensorID; + event->timestamp = millis(); + + // read the data according to vec_type + imu::Vector<3> vec; + if (vec_type == Adafruit_BNO055::VECTOR_LINEARACCEL) { + event->type = SENSOR_TYPE_LINEAR_ACCELERATION; + vec = getVector(Adafruit_BNO055::VECTOR_LINEARACCEL); + + event->acceleration.x = vec.x(); + event->acceleration.y = vec.y(); + event->acceleration.z = vec.z(); + } else if (vec_type == Adafruit_BNO055::VECTOR_ACCELEROMETER) { + event->type = SENSOR_TYPE_ACCELEROMETER; + vec = getVector(Adafruit_BNO055::VECTOR_ACCELEROMETER); + + event->acceleration.x = vec.x(); + event->acceleration.y = vec.y(); + event->acceleration.z = vec.z(); + } else if (vec_type == Adafruit_BNO055::VECTOR_GRAVITY) { + event->type = SENSOR_TYPE_GRAVITY; + vec = getVector(Adafruit_BNO055::VECTOR_GRAVITY); + + event->acceleration.x = vec.x(); + event->acceleration.y = vec.y(); + event->acceleration.z = vec.z(); + } else if (vec_type == Adafruit_BNO055::VECTOR_EULER) { + event->type = SENSOR_TYPE_ORIENTATION; + vec = getVector(Adafruit_BNO055::VECTOR_EULER); + + event->orientation.x = vec.x(); + event->orientation.y = vec.y(); + event->orientation.z = vec.z(); + } else if (vec_type == Adafruit_BNO055::VECTOR_GYROSCOPE) { + event->type = SENSOR_TYPE_GYROSCOPE; + vec = getVector(Adafruit_BNO055::VECTOR_GYROSCOPE); + + event->gyro.x = vec.x() * SENSORS_DPS_TO_RADS; + event->gyro.y = vec.y() * SENSORS_DPS_TO_RADS; + event->gyro.z = vec.z() * SENSORS_DPS_TO_RADS; + } else if (vec_type == Adafruit_BNO055::VECTOR_MAGNETOMETER) { + event->type = SENSOR_TYPE_MAGNETIC_FIELD; + vec = getVector(Adafruit_BNO055::VECTOR_MAGNETOMETER); + + event->magnetic.x = vec.x(); + event->magnetic.y = vec.y(); + event->magnetic.z = vec.z(); + } + + return true; +} + +/*! + * @brief Reads the sensor's offset registers into a byte array + * @param calibData + * Calibration offset (buffer size should be 22) + * @return true if read is successful + */ +bool Adafruit_BNO055::getSensorOffsets(uint8_t *calibData) { + if (isFullyCalibrated()) { + adafruit_bno055_opmode_t lastMode = _mode; + setMode(OPERATION_MODE_CONFIG); + + readLen(ACCEL_OFFSET_X_LSB_ADDR, calibData, NUM_BNO055_OFFSET_REGISTERS); + + setMode(lastMode); + return true; + } + return false; +} + +/*! + * @brief Reads the sensor's offset registers into an offset struct + * @param offsets_type + * type of offsets + * @return true if read is successful + */ +bool Adafruit_BNO055::getSensorOffsets( + adafruit_bno055_offsets_t &offsets_type) { + if (isFullyCalibrated()) { + adafruit_bno055_opmode_t lastMode = _mode; + setMode(OPERATION_MODE_CONFIG); + delay(25); + + /* Accel offset range depends on the G-range: + +/-2g = +/- 2000 mg + +/-4g = +/- 4000 mg + +/-8g = +/- 8000 mg + +/-1§g = +/- 16000 mg */ + offsets_type.accel_offset_x = (read8(ACCEL_OFFSET_X_MSB_ADDR) << 8) | + (read8(ACCEL_OFFSET_X_LSB_ADDR)); + offsets_type.accel_offset_y = (read8(ACCEL_OFFSET_Y_MSB_ADDR) << 8) | + (read8(ACCEL_OFFSET_Y_LSB_ADDR)); + offsets_type.accel_offset_z = (read8(ACCEL_OFFSET_Z_MSB_ADDR) << 8) | + (read8(ACCEL_OFFSET_Z_LSB_ADDR)); + + /* Magnetometer offset range = +/- 6400 LSB where 1uT = 16 LSB */ + offsets_type.mag_offset_x = + (read8(MAG_OFFSET_X_MSB_ADDR) << 8) | (read8(MAG_OFFSET_X_LSB_ADDR)); + offsets_type.mag_offset_y = + (read8(MAG_OFFSET_Y_MSB_ADDR) << 8) | (read8(MAG_OFFSET_Y_LSB_ADDR)); + offsets_type.mag_offset_z = + (read8(MAG_OFFSET_Z_MSB_ADDR) << 8) | (read8(MAG_OFFSET_Z_LSB_ADDR)); + + /* Gyro offset range depends on the DPS range: + 2000 dps = +/- 32000 LSB + 1000 dps = +/- 16000 LSB + 500 dps = +/- 8000 LSB + 250 dps = +/- 4000 LSB + 125 dps = +/- 2000 LSB + ... where 1 DPS = 16 LSB */ + offsets_type.gyro_offset_x = + (read8(GYRO_OFFSET_X_MSB_ADDR) << 8) | (read8(GYRO_OFFSET_X_LSB_ADDR)); + offsets_type.gyro_offset_y = + (read8(GYRO_OFFSET_Y_MSB_ADDR) << 8) | (read8(GYRO_OFFSET_Y_LSB_ADDR)); + offsets_type.gyro_offset_z = + (read8(GYRO_OFFSET_Z_MSB_ADDR) << 8) | (read8(GYRO_OFFSET_Z_LSB_ADDR)); + + /* Accelerometer radius = +/- 1000 LSB */ + offsets_type.accel_radius = + (read8(ACCEL_RADIUS_MSB_ADDR) << 8) | (read8(ACCEL_RADIUS_LSB_ADDR)); + + /* Magnetometer radius = +/- 960 LSB */ + offsets_type.mag_radius = + (read8(MAG_RADIUS_MSB_ADDR) << 8) | (read8(MAG_RADIUS_LSB_ADDR)); + + setMode(lastMode); + return true; + } + return false; +} + +/*! + * @brief Writes an array of calibration values to the sensor's offset + * @param calibData + * calibration data + */ +void Adafruit_BNO055::setSensorOffsets(const uint8_t *calibData) { + adafruit_bno055_opmode_t lastMode = _mode; + setMode(OPERATION_MODE_CONFIG); + delay(25); + + /* Note: Configuration will take place only when user writes to the last + byte of each config data pair (ex. ACCEL_OFFSET_Z_MSB_ADDR, etc.). + Therefore the last byte must be written whenever the user wants to + changes the configuration. */ + + /* A writeLen() would make this much cleaner */ + write8(ACCEL_OFFSET_X_LSB_ADDR, calibData[0]); + write8(ACCEL_OFFSET_X_MSB_ADDR, calibData[1]); + write8(ACCEL_OFFSET_Y_LSB_ADDR, calibData[2]); + write8(ACCEL_OFFSET_Y_MSB_ADDR, calibData[3]); + write8(ACCEL_OFFSET_Z_LSB_ADDR, calibData[4]); + write8(ACCEL_OFFSET_Z_MSB_ADDR, calibData[5]); + + write8(MAG_OFFSET_X_LSB_ADDR, calibData[6]); + write8(MAG_OFFSET_X_MSB_ADDR, calibData[7]); + write8(MAG_OFFSET_Y_LSB_ADDR, calibData[8]); + write8(MAG_OFFSET_Y_MSB_ADDR, calibData[9]); + write8(MAG_OFFSET_Z_LSB_ADDR, calibData[10]); + write8(MAG_OFFSET_Z_MSB_ADDR, calibData[11]); + + write8(GYRO_OFFSET_X_LSB_ADDR, calibData[12]); + write8(GYRO_OFFSET_X_MSB_ADDR, calibData[13]); + write8(GYRO_OFFSET_Y_LSB_ADDR, calibData[14]); + write8(GYRO_OFFSET_Y_MSB_ADDR, calibData[15]); + write8(GYRO_OFFSET_Z_LSB_ADDR, calibData[16]); + write8(GYRO_OFFSET_Z_MSB_ADDR, calibData[17]); + + write8(ACCEL_RADIUS_LSB_ADDR, calibData[18]); + write8(ACCEL_RADIUS_MSB_ADDR, calibData[19]); + + write8(MAG_RADIUS_LSB_ADDR, calibData[20]); + write8(MAG_RADIUS_MSB_ADDR, calibData[21]); + + setMode(lastMode); +} + +/*! + * @brief Writes to the sensor's offset registers from an offset struct + * @param offsets_type + * accel_offset_x = acceleration offset x + * accel_offset_y = acceleration offset y + * accel_offset_z = acceleration offset z + * + * mag_offset_x = magnetometer offset x + * mag_offset_y = magnetometer offset y + * mag_offset_z = magnetometer offset z + * + * gyro_offset_x = gyroscrope offset x + * gyro_offset_y = gyroscrope offset y + * gyro_offset_z = gyroscrope offset z + */ +void Adafruit_BNO055::setSensorOffsets( + const adafruit_bno055_offsets_t &offsets_type) { + adafruit_bno055_opmode_t lastMode = _mode; + setMode(OPERATION_MODE_CONFIG); + delay(25); + + /* Note: Configuration will take place only when user writes to the last + byte of each config data pair (ex. ACCEL_OFFSET_Z_MSB_ADDR, etc.). + Therefore the last byte must be written whenever the user wants to + changes the configuration. */ + + write8(ACCEL_OFFSET_X_LSB_ADDR, (offsets_type.accel_offset_x) & 0x0FF); + write8(ACCEL_OFFSET_X_MSB_ADDR, (offsets_type.accel_offset_x >> 8) & 0x0FF); + write8(ACCEL_OFFSET_Y_LSB_ADDR, (offsets_type.accel_offset_y) & 0x0FF); + write8(ACCEL_OFFSET_Y_MSB_ADDR, (offsets_type.accel_offset_y >> 8) & 0x0FF); + write8(ACCEL_OFFSET_Z_LSB_ADDR, (offsets_type.accel_offset_z) & 0x0FF); + write8(ACCEL_OFFSET_Z_MSB_ADDR, (offsets_type.accel_offset_z >> 8) & 0x0FF); + + write8(MAG_OFFSET_X_LSB_ADDR, (offsets_type.mag_offset_x) & 0x0FF); + write8(MAG_OFFSET_X_MSB_ADDR, (offsets_type.mag_offset_x >> 8) & 0x0FF); + write8(MAG_OFFSET_Y_LSB_ADDR, (offsets_type.mag_offset_y) & 0x0FF); + write8(MAG_OFFSET_Y_MSB_ADDR, (offsets_type.mag_offset_y >> 8) & 0x0FF); + write8(MAG_OFFSET_Z_LSB_ADDR, (offsets_type.mag_offset_z) & 0x0FF); + write8(MAG_OFFSET_Z_MSB_ADDR, (offsets_type.mag_offset_z >> 8) & 0x0FF); + + write8(GYRO_OFFSET_X_LSB_ADDR, (offsets_type.gyro_offset_x) & 0x0FF); + write8(GYRO_OFFSET_X_MSB_ADDR, (offsets_type.gyro_offset_x >> 8) & 0x0FF); + write8(GYRO_OFFSET_Y_LSB_ADDR, (offsets_type.gyro_offset_y) & 0x0FF); + write8(GYRO_OFFSET_Y_MSB_ADDR, (offsets_type.gyro_offset_y >> 8) & 0x0FF); + write8(GYRO_OFFSET_Z_LSB_ADDR, (offsets_type.gyro_offset_z) & 0x0FF); + write8(GYRO_OFFSET_Z_MSB_ADDR, (offsets_type.gyro_offset_z >> 8) & 0x0FF); + + write8(ACCEL_RADIUS_LSB_ADDR, (offsets_type.accel_radius) & 0x0FF); + write8(ACCEL_RADIUS_MSB_ADDR, (offsets_type.accel_radius >> 8) & 0x0FF); + + write8(MAG_RADIUS_LSB_ADDR, (offsets_type.mag_radius) & 0x0FF); + write8(MAG_RADIUS_MSB_ADDR, (offsets_type.mag_radius >> 8) & 0x0FF); + + setMode(lastMode); +} + +/*! + * @brief Checks of all cal status values are set to 3 (fully calibrated) + * @return status of calibration + */ +bool Adafruit_BNO055::isFullyCalibrated() { + uint8_t system, gyro, accel, mag; + getCalibration(&system, &gyro, &accel, &mag); + + switch (_mode) { + case OPERATION_MODE_ACCONLY: + return (accel == 3); + case OPERATION_MODE_MAGONLY: + return (mag == 3); + case OPERATION_MODE_GYRONLY: + case OPERATION_MODE_M4G: /* No magnetometer calibration required. */ + return (gyro == 3); + case OPERATION_MODE_ACCMAG: + case OPERATION_MODE_COMPASS: + return (accel == 3 && mag == 3); + case OPERATION_MODE_ACCGYRO: + case OPERATION_MODE_IMUPLUS: + return (accel == 3 && gyro == 3); + case OPERATION_MODE_MAGGYRO: + return (mag == 3 && gyro == 3); + default: + return (system == 3 && gyro == 3 && accel == 3 && mag == 3); + } +} + +/*! + * @brief Enter Suspend mode (i.e., sleep) + */ +void Adafruit_BNO055::enterSuspendMode() { + adafruit_bno055_opmode_t modeback = _mode; + + /* Switch to config mode (just in case since this is the default) */ + setMode(OPERATION_MODE_CONFIG); + delay(25); + write8(BNO055_PWR_MODE_ADDR, 0x02); + /* Set the requested operating mode (see section 3.3) */ + setMode(modeback); + delay(20); +} + +/*! + * @brief Enter Normal mode (i.e., wake) + */ +void Adafruit_BNO055::enterNormalMode() { + adafruit_bno055_opmode_t modeback = _mode; + + /* Switch to config mode (just in case since this is the default) */ + setMode(OPERATION_MODE_CONFIG); + delay(25); + write8(BNO055_PWR_MODE_ADDR, 0x00); + /* Set the requested operating mode (see section 3.3) */ + setMode(modeback); + delay(20); +} + +/*! + * @brief Writes an 8 bit value over I2C + */ +bool Adafruit_BNO055::write8(adafruit_bno055_reg_t reg, byte value) { + uint8_t buffer[2] = {(uint8_t)reg, (uint8_t)value}; + return i2c_dev->write(buffer, 2); +} + +/*! + * @brief Reads an 8 bit value over I2C + */ +byte Adafruit_BNO055::read8(adafruit_bno055_reg_t reg) { + uint8_t buffer[1] = {reg}; + i2c_dev->write_then_read(buffer, 1, buffer, 1); + return (byte)buffer[0]; +} + +/*! + * @brief Reads the specified number of bytes over I2C + */ +bool Adafruit_BNO055::readLen(adafruit_bno055_reg_t reg, byte *buffer, + uint8_t len) { + uint8_t reg_buf[1] = {(uint8_t)reg}; + return i2c_dev->write_then_read(reg_buf, 1, buffer, len); +} diff --git a/Main/Main/Adafruit_BNO055.h b/Main/Main/Adafruit_BNO055.h new file mode 100644 index 0000000..fe55ba7 --- /dev/null +++ b/Main/Main/Adafruit_BNO055.h @@ -0,0 +1,329 @@ +/*! + * @file Adafruit_BNO055.h + * + * 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! + * ------> https://www.adafruit.com/product/2472 + * + * 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! + * + * K.Townsend (Adafruit Industries) + * + * MIT license, all text above must be included in any redistribution + */ + +#ifndef __ADAFRUIT_BNO055_H__ +#define __ADAFRUIT_BNO055_H__ + +#include "Arduino.h" + +#include "utility/imumaths.h" +#include +#include + +/** BNO055 Address A **/ +#define BNO055_ADDRESS_A (0x28) +/** BNO055 Address B **/ +#define BNO055_ADDRESS_B (0x29) +/** BNO055 ID **/ +#define BNO055_ID (0xA0) + +/** Offsets registers **/ +#define NUM_BNO055_OFFSET_REGISTERS (22) + +/** A structure to represent offsets **/ +typedef struct { + int16_t accel_offset_x; /**< x acceleration offset */ + int16_t accel_offset_y; /**< y acceleration offset */ + int16_t accel_offset_z; /**< z acceleration offset */ + + int16_t mag_offset_x; /**< x magnetometer offset */ + int16_t mag_offset_y; /**< y magnetometer offset */ + int16_t mag_offset_z; /**< z magnetometer offset */ + + int16_t gyro_offset_x; /**< x gyroscrope offset */ + int16_t gyro_offset_y; /**< y gyroscrope offset */ + int16_t gyro_offset_z; /**< z gyroscrope offset */ + + int16_t accel_radius; /**< acceleration radius */ + + int16_t mag_radius; /**< magnetometer radius */ +} adafruit_bno055_offsets_t; + +/** Operation mode settings **/ +typedef enum { + OPERATION_MODE_CONFIG = 0X00, + OPERATION_MODE_ACCONLY = 0X01, + OPERATION_MODE_MAGONLY = 0X02, + OPERATION_MODE_GYRONLY = 0X03, + OPERATION_MODE_ACCMAG = 0X04, + OPERATION_MODE_ACCGYRO = 0X05, + OPERATION_MODE_MAGGYRO = 0X06, + OPERATION_MODE_AMG = 0X07, + OPERATION_MODE_IMUPLUS = 0X08, + OPERATION_MODE_COMPASS = 0X09, + OPERATION_MODE_M4G = 0X0A, + OPERATION_MODE_NDOF_FMC_OFF = 0X0B, + OPERATION_MODE_NDOF = 0X0C +} adafruit_bno055_opmode_t; + +/*! + * @brief Class that stores state and functions for interacting with + * BNO055 Sensor + */ +class Adafruit_BNO055 : public Adafruit_Sensor { +public: + /** BNO055 Registers **/ + 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, + + /* Mode registers */ + BNO055_OPR_MODE_ADDR = 0X3D, + BNO055_PWR_MODE_ADDR = 0X3E, + + BNO055_SYS_TRIGGER_ADDR = 0X3F, + BNO055_TEMP_SOURCE_ADDR = 0X40, + + /* Axis remap registers */ + BNO055_AXIS_MAP_CONFIG_ADDR = 0X41, + BNO055_AXIS_MAP_SIGN_ADDR = 0X42, + + /* SIC registers */ + BNO055_SIC_MATRIX_0_LSB_ADDR = 0X43, + BNO055_SIC_MATRIX_0_MSB_ADDR = 0X44, + BNO055_SIC_MATRIX_1_LSB_ADDR = 0X45, + BNO055_SIC_MATRIX_1_MSB_ADDR = 0X46, + BNO055_SIC_MATRIX_2_LSB_ADDR = 0X47, + BNO055_SIC_MATRIX_2_MSB_ADDR = 0X48, + BNO055_SIC_MATRIX_3_LSB_ADDR = 0X49, + BNO055_SIC_MATRIX_3_MSB_ADDR = 0X4A, + BNO055_SIC_MATRIX_4_LSB_ADDR = 0X4B, + BNO055_SIC_MATRIX_4_MSB_ADDR = 0X4C, + BNO055_SIC_MATRIX_5_LSB_ADDR = 0X4D, + BNO055_SIC_MATRIX_5_MSB_ADDR = 0X4E, + BNO055_SIC_MATRIX_6_LSB_ADDR = 0X4F, + BNO055_SIC_MATRIX_6_MSB_ADDR = 0X50, + BNO055_SIC_MATRIX_7_LSB_ADDR = 0X51, + BNO055_SIC_MATRIX_7_MSB_ADDR = 0X52, + BNO055_SIC_MATRIX_8_LSB_ADDR = 0X53, + BNO055_SIC_MATRIX_8_MSB_ADDR = 0X54, + + /* Accelerometer Offset registers */ + ACCEL_OFFSET_X_LSB_ADDR = 0X55, + ACCEL_OFFSET_X_MSB_ADDR = 0X56, + ACCEL_OFFSET_Y_LSB_ADDR = 0X57, + ACCEL_OFFSET_Y_MSB_ADDR = 0X58, + ACCEL_OFFSET_Z_LSB_ADDR = 0X59, + ACCEL_OFFSET_Z_MSB_ADDR = 0X5A, + + /* Magnetometer Offset registers */ + MAG_OFFSET_X_LSB_ADDR = 0X5B, + MAG_OFFSET_X_MSB_ADDR = 0X5C, + MAG_OFFSET_Y_LSB_ADDR = 0X5D, + MAG_OFFSET_Y_MSB_ADDR = 0X5E, + MAG_OFFSET_Z_LSB_ADDR = 0X5F, + MAG_OFFSET_Z_MSB_ADDR = 0X60, + + /* Gyroscope Offset register s*/ + GYRO_OFFSET_X_LSB_ADDR = 0X61, + GYRO_OFFSET_X_MSB_ADDR = 0X62, + GYRO_OFFSET_Y_LSB_ADDR = 0X63, + GYRO_OFFSET_Y_MSB_ADDR = 0X64, + GYRO_OFFSET_Z_LSB_ADDR = 0X65, + GYRO_OFFSET_Z_MSB_ADDR = 0X66, + + /* Radius registers */ + ACCEL_RADIUS_LSB_ADDR = 0X67, + ACCEL_RADIUS_MSB_ADDR = 0X68, + MAG_RADIUS_LSB_ADDR = 0X69, + MAG_RADIUS_MSB_ADDR = 0X6A + } adafruit_bno055_reg_t; + + /** BNO055 power settings */ + typedef enum { + POWER_MODE_NORMAL = 0X00, + POWER_MODE_LOWPOWER = 0X01, + POWER_MODE_SUSPEND = 0X02 + } adafruit_bno055_powermode_t; + + /** Remap settings **/ + typedef enum { + REMAP_CONFIG_P0 = 0x21, + REMAP_CONFIG_P1 = 0x24, // default + REMAP_CONFIG_P2 = 0x24, + REMAP_CONFIG_P3 = 0x21, + REMAP_CONFIG_P4 = 0x24, + REMAP_CONFIG_P5 = 0x21, + REMAP_CONFIG_P6 = 0x21, + REMAP_CONFIG_P7 = 0x24 + } adafruit_bno055_axis_remap_config_t; + + /** Remap Signs **/ + typedef enum { + REMAP_SIGN_P0 = 0x04, + REMAP_SIGN_P1 = 0x00, // default + REMAP_SIGN_P2 = 0x06, + REMAP_SIGN_P3 = 0x02, + REMAP_SIGN_P4 = 0x03, + REMAP_SIGN_P5 = 0x01, + REMAP_SIGN_P6 = 0x07, + REMAP_SIGN_P7 = 0x05 + } adafruit_bno055_axis_remap_sign_t; + + /** A structure to represent revisions **/ + typedef struct { + uint8_t accel_rev; /**< acceleration rev */ + uint8_t mag_rev; /**< magnetometer rev */ + uint8_t gyro_rev; /**< gyroscrope rev */ + uint16_t sw_rev; /**< SW rev */ + uint8_t bl_rev; /**< bootloader rev */ + } adafruit_bno055_rev_info_t; + + /** Vector Mappings **/ + typedef enum { + VECTOR_ACCELEROMETER = BNO055_ACCEL_DATA_X_LSB_ADDR, + VECTOR_MAGNETOMETER = BNO055_MAG_DATA_X_LSB_ADDR, + VECTOR_GYROSCOPE = BNO055_GYRO_DATA_X_LSB_ADDR, + VECTOR_EULER = BNO055_EULER_H_LSB_ADDR, + VECTOR_LINEARACCEL = BNO055_LINEAR_ACCEL_DATA_X_LSB_ADDR, + VECTOR_GRAVITY = BNO055_GRAVITY_DATA_X_LSB_ADDR + } adafruit_vector_type_t; + + Adafruit_BNO055(int32_t sensorID = -1, uint8_t address = BNO055_ADDRESS_A, + TwoWire *theWire = &Wire); + + bool begin(adafruit_bno055_opmode_t mode = OPERATION_MODE_NDOF); + void setMode(adafruit_bno055_opmode_t mode); + adafruit_bno055_opmode_t getMode(); + void setAxisRemap(adafruit_bno055_axis_remap_config_t remapcode); + void setAxisSign(adafruit_bno055_axis_remap_sign_t remapsign); + void getRevInfo(adafruit_bno055_rev_info_t *); + void setExtCrystalUse(boolean usextal); + void getSystemStatus(uint8_t *system_status, uint8_t *self_test_result, + uint8_t *system_error); + void getCalibration(uint8_t *system, uint8_t *gyro, uint8_t *accel, + uint8_t *mag); + + imu::Vector<3> getVector(adafruit_vector_type_t vector_type); + imu::Quaternion getQuat(); + int8_t getTemp(); + + /* Adafruit_Sensor implementation */ + bool getEvent(sensors_event_t *); + bool getEvent(sensors_event_t *, adafruit_vector_type_t); + void getSensor(sensor_t *); + + /* Functions to deal with raw calibration data */ + bool getSensorOffsets(uint8_t *calibData); + bool getSensorOffsets(adafruit_bno055_offsets_t &offsets_type); + void setSensorOffsets(const uint8_t *calibData); + void setSensorOffsets(const adafruit_bno055_offsets_t &offsets_type); + bool isFullyCalibrated(); + + /* Power managments functions */ + void enterSuspendMode(); + void enterNormalMode(); + + byte read8(adafruit_bno055_reg_t); + bool readLen(adafruit_bno055_reg_t, byte *buffer, uint8_t len); + bool write8(adafruit_bno055_reg_t, byte value); + +private: + + Adafruit_I2CDevice *i2c_dev = NULL; ///< Pointer to I2C bus interface + + int32_t _sensorID; + adafruit_bno055_opmode_t _mode; +}; + +#endif diff --git a/Main/Main/Main.ino b/Main/Main/Main.ino new file mode 100644 index 0000000..d33aa80 --- /dev/null +++ b/Main/Main/Main.ino @@ -0,0 +1,547 @@ +// global data +float data_bno_accel_x_mss = 0; +float data_bno_accel_y_mss = 0; +float data_bno_accel_z_mss = 0; +float data_bme_pressure_hPa = 0; +float data_bme_temperature_degC = 0; +float data_bme_altitude_m = 0; +uint32_t data_gnss_latitude_udeg = 0; +uint32_t data_gnss_longitude_udeg = 0; +float data_bat_v = 0; +float data_ext_v = 0; +bool data_key_sw_active = false; +int emst = 0; +bool accelopen = false, altitudeopen = false; + + +// pinout +const pin_size_t MIF_TX = 2; +const pin_size_t MIF_RX = 3; +const pin_size_t BNO_SDA = 4; +const pin_size_t BNO_SCL = 5; +const pin_size_t VALVE_TX = 6; +const pin_size_t VALVE_RX = 7; +const pin_size_t BME_SDA = 10; +const pin_size_t BME_SCL = 11; +const pin_size_t E220_TX = 12; +const pin_size_t E220_RX = 13; +const pin_size_t GNSS_TX = 14; +const pin_size_t GNSS_RX = 15; +const pin_size_t KEY_SW = 20; +const pin_size_t EXT_V = 26; +const pin_size_t BAT_V = 27; + +// Servo +#include +const pin_size_t SERVO_1 = 21; +const pin_size_t SERVO_2 = 22; +Servo servo1; +Servo servo2; +int servo_close_time = 0; + +// KeySW + + +// BNO055 +#include // 必要なヘッダーファイルをインクルード +#include +#include +#include "Adafruit_BNO055.h" +#include "utility/imumaths.h" +Adafruit_BNO055 bno = Adafruit_BNO055(55, 0x28, &Wire); +sensors_event_t accelerometerData; + +sensors_event_t* event; +bool acceljudge_gettingout = false; //加速度による離床判定 +bool acceljudge_open = false; //加速度による開放判定 +int accelcount = 0; //閾値を5回満たすカウントをする +int phase_now = 0; //現在のフェーズを区別(0=CHEAK,1=READY,2=FLIGHT,3=OPENED) +int Openjudge = 1; //開放禁止コマンド用状態表示用(0=NOT_OPEN,1=OPEN) +double BNO[10]; //BNOデータ取得、格納に用いる +unsigned long time_data_ms = 0; //離床判定タイマー(燃焼終了検知) +bool mecotime_data_judge_ms = false; //燃焼終了検知 +bool apogeetime_data_judge_ms = false; //頂点到達検知 + +//BNO設定 +/* Set the delay between fresh samples */ +uint16_t BNO055_SAMPLERATE_DELAY_MS = 100; + +// Check I2C device address and correct line below (by default address is 0x29 or 0x28) +// id, address + +// BME280 +//BME280のライブラリーを取り込みます。 +#include +//1013.25は地球の海面上の大気圧の平均値(1気圧)です。 +#define SEALEVELPRESSURE_HPA (1013.25) + +Adafruit_BME280 bme; +unsigned long delayTime; +float temperature; //気温 +float altitude; //高度 +bool altitudejudge_gettingout = false; //高度による離床判定 +bool altitudejudge_open = false; //高度による開放判定 +int altitudecount = 0; +double BME[10]; //BMEデータ取得、格納に用いる + +// SAM-M8Q +#include +TinyGPSPlus gps; +SerialPIO Serial_GNSS(GNSS_TX, GNSS_RX, 512); +// https://arduino-pico.readthedocs.io/en/latest/piouart.html + +// ES920LR +#define Serial_ES920 Serial2 +// https://moyoi-memo.hatenablog.com/entry/2022/02/15/112100 +String downlink = ""; +String uplink = ""; +String uplink_string = ""; + +// W25Q128 + + +// Valve +char valve_mode = '/'; +SerialPIO Serial_Valve(VALVE_TX, VALVE_RX, 32); + +// MissionInterface +SerialPIO Serial_MIF(MIF_TX, MIF_RX, 256); + +// setup()ではdelay()使用可 +void setup() { + pinMode(LED_BUILTIN, OUTPUT); + digitalWrite(LED_BUILTIN, HIGH); + delay(500); + // デバッグ出力 + Serial.begin(115200); + + Wire.setSDA(BNO_SDA); + Wire.setSCL(BNO_SCL); + while (!bno.begin()) { + Serial.print("BNO055 ERR"); + delay(100); + } + + bno.setMode(OPERATION_MODE_CONFIG); + delay(25); + uint8_t savePageID = bno.read8(Adafruit_BNO055::BNO055_PAGE_ID_ADDR); + bno.write8(Adafruit_BNO055::BNO055_PAGE_ID_ADDR, 0X01); + bno.write8(Adafruit_BNO055::BNO055_ACCEL_DATA_X_LSB_ADDR, 0X17); // page2なのでホントはACC_DATA_X_LSBではなくACC_DATA_X_LSBにアクセス + delay(10); + bno.write8(Adafruit_BNO055::BNO055_PAGE_ID_ADDR, savePageID); + bno.setMode(OPERATION_MODE_ACCONLY); + delay(20); + + Wire1.setSDA(BME_SDA); + Wire1.setSCL(BME_SCL); + while (!bme.begin(0x76, &Wire1)) { + Serial.print("BME280 ERR"); + delay(100); + } + bme.setSampling( + Adafruit_BME280::MODE_NORMAL, + Adafruit_BME280::SAMPLING_X1, + Adafruit_BME280::SAMPLING_X2, + Adafruit_BME280::SAMPLING_NONE, + Adafruit_BME280::FILTER_X16, + Adafruit_BME280::STANDBY_MS_0_5); + + analogReadResolution(12); + // pinMode(KEY_SW, INPUT); + + Serial_ES920.setTX(8); + Serial_ES920.setRX(9); + Serial_ES920.setFIFOSize(64); + Serial_ES920.begin(115200); + while (Serial_ES920.available()) { + Serial_ES920.read(); + } + + Serial_GNSS.begin(9600); + Serial_Valve.begin(115200); + Serial_MIF.begin(115200); + + //servo + servo1.attach(SERVO_1); + servo2.attach(SERVO_2); +} + +// loop()と，ここから呼び出される関数ではdelay()使用禁止 +void loop() { + static uint32_t time_100Hz = 0; + if (millis() - time_100Hz >= 10) { + time_100Hz += 10; + + static int count_10Hz = 0; + count_10Hz++; + if (count_10Hz > 10) { + count_10Hz = 0; + + // 10Hzで実行する処理 + getevent(); + data_bat_v = analogRead(BAT_V) * 3.3 * 11 / (1 << 12); + data_ext_v = analogRead(EXT_V) * 3.3 * 11 / (1 << 12); + + // フライト = 回路としてOPEN = LOW + // バッテリー駆動でなくUSB駆動の場合常にLOWなので除外 + + data_key_sw_active = (digitalRead(KEY_SW) == LOW) && data_bat_v > 1; + static bool last_data_key_sw_active = false; + if (data_key_sw_active && !last_data_key_sw_active && phase_now == 0) { + phase_now = 1; + } + // if (!data_key_sw_active && last_data_key_sw_active) { + // if (opener.open_judge.prohibitOpen) { + // gocheck(); + // } else { + // gocheck(); + // opener.clear_prohibitOpen(); + // } + // } + last_data_key_sw_active = data_key_sw_active; + + // デバッグ出力 + /* + Serial.print("BNO055, "); + Serial.print(data_bno_accel_x_mss); + Serial.print(", "); + Serial.print(data_bno_accel_y_mss); + Serial.print(", "); + Serial.print(data_bno_accel_z_mss); + Serial.print(", "); + + Serial.print("BNO055, "); + Serial.print(data_bme_pressure_hPa); + Serial.print(", "); + Serial.print(data_bme_temperature_degC); + Serial.print(", "); + Serial.print(data_bme_altitude_m); + Serial.print(", "); + + Serial.print("GNSS, "); + Serial.print(data_gnss_latitude_udeg); + Serial.print(", "); + Serial.print(data_gnss_latitude_udeg); + Serial.print(", "); + */ + // if (need_response_usb) { + // Serial.println("response:" + response); + // need_response_usb = false; + // } else { + // Serial.println(downlink); + // } + } + + //100Hz + // BME280から100Hzで測定 + if (altitudejudge_gettingout == false) { //READYフェーズでのみ離床判定可能 + if (altitudecount > 2) { //2回連続で閾値を超えたときの処理 + altitudejudge_gettingout = true; //離床判定・フェーズ移行 + altitudecount = 0; //開放判定に向けた初期化 + } else { + if (calculateMediumBME() < -360) { + altitudecount++; + Serial.print(1); + } else { + altitudecount = 0; //初期化 + } + } + } + if ((altitudejudge_open == false) && (phase_now == 2)) { //READYフェーズのみ開放判定可能 + if (altitudecount > 2) { //2回連続で閾値を下回ったときの処理 + altitudejudge_open = true; //開放判定・フェーズ移行 + } + if (calculateMediumBME() > -350) { //開放判定閾値 + altitudecount++; + } else { + altitudecount = 0; //初期化 + } + } + } + + // 常に実行する処理 + + // コマンドアップリンク + //ES920LR設定用 + // if (Serial.available()) { + // downlink = Serial.readStringUntil('\n'); + // Serial.println(downlink); + // } + if (Serial.available()) { + char c = (char)Serial.read(); + Serial_ES920.write(c); + } + + // if (Serial_ES920.available()) { //動作検証用(使わない) + // uplink_string = Serial_ES920.read(); + // uplink_string.trim(); + // Serial.println(uplink_string); + // } + + uplink = ""; + if (Serial_ES920.available()) { + uplink = Serial_ES920.readStringUntil('\n'); + } + // if (uplink != "") { //動作検証用(使わない) + // Serial.print("uplink:"); + // Serial.println(uplink); + // } + //CHEACK・NOTOPEN・READYコマンド受信 + //READYコマンド(CHECKphaseであることが条件) + if (phase_now == 0) { + if (uplink.substring(0, uplink.length() - 1) == "READY") { //READYコマンド + phase_now = 1; // "READY"が入力された場合、phase_nowに１を代入する + Serial.println("READY_transitioned"); //動作確認用 + Serial.println(phase_now); + } + } + if (uplink.substring(0, uplink.length() - 1) == "emst") { + emst = 1; + Serial.println("emergency"); //動作確認用 + Serial.println("NOTopened"); + } + if (uplink.substring(0, uplink.length() - 1) == "CHECK") { //CHECKコマンド + gocheck(); // "CHECK"が入力された場合、phaseに0を代入 + Serial.println("CHECK_transitioned"); //動作確認用 + } + + + //離床判定 + if (phase_now == 1 && (altitudejudge_gettingout || acceljudge_gettingout)) { //READYフェーズに移行かつ、離床判定後(加速度or気圧で判定)である場合 + //高度による離床判定 + //10Hz処理に記述 + + //加速度による離床判定 + //10Hz処理に記述 + + + //高度・加速度による離床判定 + if (acceljudge_gettingout) { //加速度による離床判定 + phase_now = 2; //フェーズ移行 + Serial.println("FLIGHT_accel_transitioned"); //シリアルモニタに状態移行を表示 + time_data_ms = millis(); //離床判定タイマー + } else if (altitudejudge_gettingout /*高度による離床判定条件*/) { //高度による離床判定 + phase_now = 2; //フェーズ移行 + Serial.println("FLIGHT_altitude_transitioned"); //シリアルモニタに状態移行を表示 + time_data_ms = millis(); //離床判定タイマー + } + } + + //開放判定 + if (phase_now == 2) { + //高度による開放判定 + //100Hz処理に記述する + + //加速度による開放判定 + //100Hz処理に記述 + + //高度・加速度による開放判定 + //加速度での開放判定 + if (millis() - time_data_ms > 5000 /*とりあえず燃焼時間5秒設定*/) { //時間による燃焼終了検知 + mecotime_data_judge_ms = true; + } + + if ((acceljudge_open) && (phase_now == 2) && (mecotime_data_judge_ms)) { + accelopen = true; + } + //高度による開放判定 + if (millis() - time_data_ms > 12000 /*とりあえず燃焼時間12秒設定*/) { //時間による頂点到達検知 + apogeetime_data_judge_ms = true; + } + if ((altitudejudge_open) || (apogeetime_data_judge_ms)) { + altitudeopen = true; + } + if (((accelopen) && (altitudeopen) && (emst == 0))) { //高度と加速度の閾値超過連続回数がともに5回以上かつ開放禁止コマンドが入力されていないとき + phase_now = 3; //フェーズ移行 + servo1.write(93); + servo2.write(93); + servo_close_time = millis(); + } + Serial.println("OPENED_transitioned"); + } +} + +// SAM-M8QのUARTを常に読み出し +while (Serial_GNSS.available()) { + gps.encode(Serial_GNSS.read()); + if (gps.location.isUpdated()) { + data_gnss_latitude_udeg = gps.location.lat() * 1000000; + data_gnss_longitude_udeg = gps.location.lng() * 1000000; + } +} +if (millis() - servo_close_time >= 5000) { + servo1.write(30); + servo2.write(30); +} + + + +// テレメトリ生成 +downlink = ""; +if (phase_now == 0) { + downlink += 'C'; +} else if (phase_now == 1) { + downlink += 'R'; +} else if (phase_now == 2) { + downlink += 'F'; +} else if (phase_now == 3) { + downlink += 'O'; +} + +if (emst == 0) { + downlink += '/'; // Emergency +} else { + downlink += 'E'; // Emergency +} +//時間による頂点到達 +if (phase_now >= 2) { + if (apogeetime_data_judge_ms) { + downlink += 'T'; // apogeeTime + } else { + downlink += '/'; //apogeeTime + } +} +//高度による頂点到達 +if (altitudejudge_open) { + downlink += 'P'; // Pressure +} else { + downlink += '/'; +} + +//時間による燃焼終了 +if (phase_now >= 2) { + if (mecotime_data_judge_ms) { + downlink += 'T'; // mecoTime + } else { + downlink += '/'; // mecoTime + } +} else { + downlink += '/'; +} +//加速度による燃焼終了 +if (acceljudge_open) { + downlink += 'A'; +} else { + downlink += '/'; +} + +//離床条件 +if (acceljudge_gettingout) { //高度と加速度のどっちで離床したかの判定 + downlink += 'A'; +} else if (altitudejudge_gettingout) { + downlink += 'P'; +} else { + downlink += '/'; +} + + +data_bme_altitude_m = bme.readAltitude(SEALEVELPRESSURE_HPA); +downlink += valve_mode; +downlink += String(data_key_sw_active ? 'K' : '/') + ','; // Key +downlink += String(data_bno_accel_z_mss, 1) + ','; +downlink += String(data_bme_temperature_degC, 1) + ','; +downlink += String(data_bme_altitude_m, 1) + ','; +downlink += String(data_gnss_latitude_udeg % 1000000) + ','; +downlink += String(data_gnss_longitude_udeg % 1000000) + ','; +downlink += String(data_bat_v, 1) + ','; +downlink += String(static_cast(data_ext_v)); +Serial.println(data_bno_accel_z_mss); + +// テレメトリダウンリンク +const uint32_t downlink_rate_ms = 2500; +static uint32_t last_downlink_ms = 0; +if (millis() - last_downlink_ms > downlink_rate_ms) { + last_downlink_ms = millis(); + Serial_ES920.println(downlink); + //Serial.println(downlink); +} +} + +//BNO055の100Hzで測定する際に用いる関数 +//BNOからセンサ値が0でない値のみを返す関数 +double getBNO(sensors_event_t* event) { + bno.getEvent(&accelerometerData, Adafruit_BNO055::VECTOR_ACCELEROMETER); + if (event->type == SENSOR_TYPE_ACCELEROMETER) { + data_bno_accel_x_mss = event->acceleration.x; + data_bno_accel_y_mss = event->acceleration.y; + data_bno_accel_z_mss = event->acceleration.z; + + } else { + Serial.print("Unk:"); + } + // Serial.print(data_bno_accel_z_mss ); + // Serial.print(","); + return data_bno_accel_z_mss; +} +//BNOの中央値算出 +double calculateMedianBNO(sensors_event_t* event) { + double BNO[10]; + for (int i = 0; i < 10; i++) { + BNO[i] = getBNO(event); + } + // 中央値を計算するために配列をソートする + std::sort(BNO, BNO + 10); + double median = BNO[4]; // 中央値 + return median; +} + +//BMEデータ中央値計算(気圧値) +double calculateMediumBME() { + double BME[4]; + double tmp = 0, medium = 0, Medium = 0, MEDIUM; + for (int i = 0; i < 4; i++) { + data_bme_temperature_degC = bme.readTemperature(); + BME[i] = bme.readAltitude(SEALEVELPRESSURE_HPA); + } + // 中央値を計算するために配列をソートする + std::sort(BME, BME + 4); + medium = BME[2]; // 中央値 + Medium = MEDIUM; + MEDIUM = (Medium - medium) / 0.5; + Serial.print(data_bme_altitude_m); + Serial.print(","); + return MEDIUM; +} + + +void gocheck() { + acceljudge_gettingout = false; //加速度による離床判定 + acceljudge_open = false; //加速度による開放判定 + accelcount = 0; //閾値を5回満たすカウントをする + altitudejudge_gettingout = false; //高度による離床判定 + altitudejudge_open = false; //高度による開放判定 + altitudecount = 0; //閾値を5回満たすカウントをする + phase_now = 0; //現在のフェーズを区別(0=CHEAK,1=READY,2=FLIGHT,3=OPENED) + emst = 0; //開放禁止コマンド用状態表示用(0=NOT_OPEN,1=OPEN) + apogeetime_data_judge_ms = false; + mecotime_data_judge_ms = false; + time_data_ms = 100000000; + return; +} +void getevent() { + // 10Hzで実行する処理 + // BNO055から100Hzで測定 + if (acceljudge_gettingout == false) { //READYフェーズでのみ離床判定可能 + if (calculateMedianBNO(&accelerometerData) <= -9) { //中央値が閾値を超えた回数カウント + accelcount++; + } else { + accelcount = 0; //初期化 + } + + if (accelcount > 5) { //5回連続で閾値を超えたときの処理 + acceljudge_gettingout = true; //離床判定・フェーズ移行 + accelcount = 0; //開放判定に向けた初期化 + } + } + if ((acceljudge_open == false) && (phase_now == 2)) { //READYフェーズのみ開放判定可能 + if (calculateMedianBNO(&accelerometerData) > -7) { //中央値が閾値を下回った回数カウント + accelcount++; + } else { + accelcount = 0; //初期化 + } + if (accelcount > 5) { //5回連続で閾値を下回ったときの処理 + acceljudge_open = true; //開放判定・フェーズ移行 + } + } + return; +} diff --git a/Main/Main/utility/imumaths.h b/Main/Main/utility/imumaths.h new file mode 100644 index 0000000..0daefb0 --- /dev/null +++ b/Main/Main/utility/imumaths.h @@ -0,0 +1,31 @@ + +// Inertial Measurement Unit Maths Library +// +// Copyright 2013-2021 Sam Cowen +// +// 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. + +#ifndef IMUMATH_H +#define IMUMATH_H + +#include "matrix.h" +#include "quaternion.h" +#include "vector.h" + +#endif diff --git a/Main/Main/utility/matrix.h b/Main/Main/utility/matrix.h new file mode 100644 index 0000000..7190586 --- /dev/null +++ b/Main/Main/utility/matrix.h @@ -0,0 +1,185 @@ +// Inertial Measurement Unit Maths Library +// +// Copyright 2013-2021 Sam Cowen +// Bug fixes and cleanups by Gé Vissers (gvissers@gmail.com) +// +// 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. + +#ifndef IMUMATH_MATRIX_HPP +#define IMUMATH_MATRIX_HPP + +#include +#include + +#include "vector.h" + +namespace imu { + +template class Matrix { +public: + Matrix() { memset(_cell_data, 0, N * N * sizeof(double)); } + + Matrix(const Matrix &m) { + for (int ij = 0; ij < N * N; ++ij) { + _cell_data[ij] = m._cell_data[ij]; + } + } + + ~Matrix() {} + + Matrix &operator=(const Matrix &m) { + for (int ij = 0; ij < N * N; ++ij) { + _cell_data[ij] = m._cell_data[ij]; + } + return *this; + } + + Vector row_to_vector(int i) const { + Vector ret; + for (int j = 0; j < N; j++) { + ret[j] = cell(i, j); + } + return ret; + } + + Vector col_to_vector(int j) const { + Vector ret; + for (int i = 0; i < N; i++) { + ret[i] = cell(i, j); + } + return ret; + } + + void vector_to_row(const Vector &v, int i) { + for (int j = 0; j < N; j++) { + cell(i, j) = v[j]; + } + } + + void vector_to_col(const Vector &v, int j) { + for (int i = 0; i < N; i++) { + cell(i, j) = v[i]; + } + } + + double operator()(int i, int j) const { return cell(i, j); } + double &operator()(int i, int j) { return cell(i, j); } + + double cell(int i, int j) const { return _cell_data[i * N + j]; } + double &cell(int i, int j) { return _cell_data[i * N + j]; } + + Matrix operator+(const Matrix &m) const { + Matrix ret; + for (int ij = 0; ij < N * N; ++ij) { + ret._cell_data[ij] = _cell_data[ij] + m._cell_data[ij]; + } + return ret; + } + + Matrix operator-(const Matrix &m) const { + Matrix ret; + for (int ij = 0; ij < N * N; ++ij) { + ret._cell_data[ij] = _cell_data[ij] - m._cell_data[ij]; + } + return ret; + } + + Matrix operator*(double scalar) const { + Matrix ret; + for (int ij = 0; ij < N * N; ++ij) { + ret._cell_data[ij] = _cell_data[ij] * scalar; + } + return ret; + } + + Matrix operator*(const Matrix &m) const { + Matrix ret; + for (int i = 0; i < N; i++) { + Vector row = row_to_vector(i); + for (int j = 0; j < N; j++) { + ret(i, j) = row.dot(m.col_to_vector(j)); + } + } + return ret; + } + + Matrix transpose() const { + Matrix ret; + for (int i = 0; i < N; i++) { + for (int j = 0; j < N; j++) { + ret(j, i) = cell(i, j); + } + } + return ret; + } + + Matrix minor_matrix(int row, int col) const { + Matrix ret; + for (int i = 0, im = 0; i < N; i++) { + if (i == row) + continue; + + for (int j = 0, jm = 0; j < N; j++) { + if (j != col) { + ret(im, jm++) = cell(i, j); + } + } + im++; + } + return ret; + } + + double determinant() const { + // specialization for N == 1 given below this class + double det = 0.0, sign = 1.0; + for (int i = 0; i < N; ++i, sign = -sign) + det += sign * cell(0, i) * minor_matrix(0, i).determinant(); + return det; + } + + Matrix invert() const { + Matrix ret; + double det = determinant(); + + for (int i = 0; i < N; i++) { + for (int j = 0; j < N; j++) { + ret(i, j) = minor_matrix(j, i).determinant() / det; + if ((i + j) % 2 == 1) + ret(i, j) = -ret(i, j); + } + } + return ret; + } + + double trace() const { + double tr = 0.0; + for (int i = 0; i < N; ++i) + tr += cell(i, i); + return tr; + } + +private: + double _cell_data[N * N]; +}; + +template <> inline double Matrix<1>::determinant() const { return cell(0, 0); } + +}; // namespace imu + +#endif diff --git a/Main/Main/utility/quaternion.h b/Main/Main/utility/quaternion.h new file mode 100644 index 0000000..1aa471d --- /dev/null +++ b/Main/Main/utility/quaternion.h @@ -0,0 +1,214 @@ +// Inertial Measurement Unit Maths Library +// +// Copyright 2013-2021 Sam Cowen +// Bug fixes and cleanups by Gé Vissers (gvissers@gmail.com) +// +// 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. + +#ifndef IMUMATH_QUATERNION_HPP +#define IMUMATH_QUATERNION_HPP + +#include +#include +#include +#include + +#include "matrix.h" + +namespace imu { + +class Quaternion { +public: + Quaternion() : _w(1.0), _x(0.0), _y(0.0), _z(0.0) {} + + Quaternion(double w, double x, double y, double z) + : _w(w), _x(x), _y(y), _z(z) {} + + Quaternion(double w, Vector<3> vec) + : _w(w), _x(vec.x()), _y(vec.y()), _z(vec.z()) {} + + double &w() { return _w; } + double &x() { return _x; } + double &y() { return _y; } + double &z() { return _z; } + + double w() const { return _w; } + double x() const { return _x; } + double y() const { return _y; } + double z() const { return _z; } + + double magnitude() const { + return sqrt(_w * _w + _x * _x + _y * _y + _z * _z); + } + + void normalize() { + double mag = magnitude(); + *this = this->scale(1 / mag); + } + + Quaternion conjugate() const { return Quaternion(_w, -_x, -_y, -_z); } + + void fromAxisAngle(const Vector<3> &axis, double theta) { + _w = cos(theta / 2); + // only need to calculate sine of half theta once + double sht = sin(theta / 2); + _x = axis.x() * sht; + _y = axis.y() * sht; + _z = axis.z() * sht; + } + + void fromMatrix(const Matrix<3> &m) { + double tr = m.trace(); + + double S; + if (tr > 0) { + S = sqrt(tr + 1.0) * 2; + _w = 0.25 * S; + _x = (m(2, 1) - m(1, 2)) / S; + _y = (m(0, 2) - m(2, 0)) / S; + _z = (m(1, 0) - m(0, 1)) / S; + } else if (m(0, 0) > m(1, 1) && m(0, 0) > m(2, 2)) { + S = sqrt(1.0 + m(0, 0) - m(1, 1) - m(2, 2)) * 2; + _w = (m(2, 1) - m(1, 2)) / S; + _x = 0.25 * S; + _y = (m(0, 1) + m(1, 0)) / S; + _z = (m(0, 2) + m(2, 0)) / S; + } else if (m(1, 1) > m(2, 2)) { + S = sqrt(1.0 + m(1, 1) - m(0, 0) - m(2, 2)) * 2; + _w = (m(0, 2) - m(2, 0)) / S; + _x = (m(0, 1) + m(1, 0)) / S; + _y = 0.25 * S; + _z = (m(1, 2) + m(2, 1)) / S; + } else { + S = sqrt(1.0 + m(2, 2) - m(0, 0) - m(1, 1)) * 2; + _w = (m(1, 0) - m(0, 1)) / S; + _x = (m(0, 2) + m(2, 0)) / S; + _y = (m(1, 2) + m(2, 1)) / S; + _z = 0.25 * S; + } + } + + void toAxisAngle(Vector<3> &axis, double &angle) const { + double sqw = sqrt(1 - _w * _w); + if (sqw == 0) // it's a singularity and divide by zero, avoid + return; + + angle = 2 * acos(_w); + axis.x() = _x / sqw; + axis.y() = _y / sqw; + axis.z() = _z / sqw; + } + + Matrix<3> toMatrix() const { + Matrix<3> ret; + ret.cell(0, 0) = 1 - 2 * _y * _y - 2 * _z * _z; + ret.cell(0, 1) = 2 * _x * _y - 2 * _w * _z; + ret.cell(0, 2) = 2 * _x * _z + 2 * _w * _y; + + ret.cell(1, 0) = 2 * _x * _y + 2 * _w * _z; + ret.cell(1, 1) = 1 - 2 * _x * _x - 2 * _z * _z; + ret.cell(1, 2) = 2 * _y * _z - 2 * _w * _x; + + ret.cell(2, 0) = 2 * _x * _z - 2 * _w * _y; + ret.cell(2, 1) = 2 * _y * _z + 2 * _w * _x; + ret.cell(2, 2) = 1 - 2 * _x * _x - 2 * _y * _y; + return ret; + } + + // Returns euler angles that represent the quaternion. Angles are + // returned in rotation order and right-handed about the specified + // axes: + // + // v[0] is applied 1st about z (ie, roll) + // v[1] is applied 2nd about y (ie, pitch) + // v[2] is applied 3rd about x (ie, yaw) + // + // Note that this means result.x() is not a rotation about x; + // similarly for result.z(). + // + Vector<3> toEuler() const { + Vector<3> ret; + double sqw = _w * _w; + double sqx = _x * _x; + double sqy = _y * _y; + double sqz = _z * _z; + + ret.x() = atan2(2.0 * (_x * _y + _z * _w), (sqx - sqy - sqz + sqw)); + ret.y() = asin(-2.0 * (_x * _z - _y * _w) / (sqx + sqy + sqz + sqw)); + ret.z() = atan2(2.0 * (_y * _z + _x * _w), (-sqx - sqy + sqz + sqw)); + + return ret; + } + + Vector<3> toAngularVelocity(double dt) const { + Vector<3> ret; + Quaternion one(1.0, 0.0, 0.0, 0.0); + Quaternion delta = one - *this; + Quaternion r = (delta / dt); + r = r * 2; + r = r * one; + + ret.x() = r.x(); + ret.y() = r.y(); + ret.z() = r.z(); + return ret; + } + + Vector<3> rotateVector(const Vector<2> &v) const { + return rotateVector(Vector<3>(v.x(), v.y())); + } + + Vector<3> rotateVector(const Vector<3> &v) const { + Vector<3> qv(_x, _y, _z); + Vector<3> t = qv.cross(v) * 2.0; + return v + t * _w + qv.cross(t); + } + + Quaternion operator*(const Quaternion &q) const { + return Quaternion(_w * q._w - _x * q._x - _y * q._y - _z * q._z, + _w * q._x + _x * q._w + _y * q._z - _z * q._y, + _w * q._y - _x * q._z + _y * q._w + _z * q._x, + _w * q._z + _x * q._y - _y * q._x + _z * q._w); + } + + Quaternion operator+(const Quaternion &q) const { + return Quaternion(_w + q._w, _x + q._x, _y + q._y, _z + q._z); + } + + Quaternion operator-(const Quaternion &q) const { + return Quaternion(_w - q._w, _x - q._x, _y - q._y, _z - q._z); + } + + Quaternion operator/(double scalar) const { + return Quaternion(_w / scalar, _x / scalar, _y / scalar, _z / scalar); + } + + Quaternion operator*(double scalar) const { return scale(scalar); } + + Quaternion scale(double scalar) const { + return Quaternion(_w * scalar, _x * scalar, _y * scalar, _z * scalar); + } + +private: + double _w, _x, _y, _z; +}; + +} // namespace imu + +#endif diff --git a/Main/Main/utility/vector.h b/Main/Main/utility/vector.h new file mode 100644 index 0000000..e4c3727 --- /dev/null +++ b/Main/Main/utility/vector.h @@ -0,0 +1,184 @@ +// Inertial Measurement Unit Maths Library +// +// Copyright 2013-2021 Sam Cowen +// Bug fixes and cleanups by Gé Vissers (gvissers@gmail.com) +// +// 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. + +#ifndef IMUMATH_VECTOR_HPP +#define IMUMATH_VECTOR_HPP + +#include +#include +#include + +namespace imu { + +template class Vector { +public: + Vector() { memset(p_vec, 0, sizeof(double) * N); } + + Vector(double a) { + memset(p_vec, 0, sizeof(double) * N); + p_vec[0] = a; + } + + Vector(double a, double b) { + memset(p_vec, 0, sizeof(double) * N); + p_vec[0] = a; + p_vec[1] = b; + } + + Vector(double a, double b, double c) { + memset(p_vec, 0, sizeof(double) * N); + p_vec[0] = a; + p_vec[1] = b; + p_vec[2] = c; + } + + Vector(double a, double b, double c, double d) { + memset(p_vec, 0, sizeof(double) * N); + p_vec[0] = a; + p_vec[1] = b; + p_vec[2] = c; + p_vec[3] = d; + } + + Vector(const Vector &v) { + for (int x = 0; x < N; x++) + p_vec[x] = v.p_vec[x]; + } + + ~Vector() {} + + uint8_t n() { return N; } + + double magnitude() const { + double res = 0; + for (int i = 0; i < N; i++) + res += p_vec[i] * p_vec[i]; + + return sqrt(res); + } + + void normalize() { + double mag = magnitude(); + if (isnan(mag) || mag == 0.0) + return; + + for (int i = 0; i < N; i++) + p_vec[i] /= mag; + } + + double dot(const Vector &v) const { + double ret = 0; + for (int i = 0; i < N; i++) + ret += p_vec[i] * v.p_vec[i]; + + return ret; + } + + // The cross product is only valid for vectors with 3 dimensions, + // with the exception of higher dimensional stuff that is beyond + // the intended scope of this library. + // Only a definition for N==3 is given below this class, using + // cross() with another value for N will result in a link error. + Vector cross(const Vector &v) const; + + Vector scale(double scalar) const { + Vector ret; + for (int i = 0; i < N; i++) + ret.p_vec[i] = p_vec[i] * scalar; + return ret; + } + + Vector invert() const { + Vector ret; + for (int i = 0; i < N; i++) + ret.p_vec[i] = -p_vec[i]; + return ret; + } + + Vector &operator=(const Vector &v) { + for (int x = 0; x < N; x++) + p_vec[x] = v.p_vec[x]; + return *this; + } + + double &operator[](int n) { return p_vec[n]; } + + double operator[](int n) const { return p_vec[n]; } + + double &operator()(int n) { return p_vec[n]; } + + double operator()(int n) const { return p_vec[n]; } + + Vector operator+(const Vector &v) const { + Vector ret; + for (int i = 0; i < N; i++) + ret.p_vec[i] = p_vec[i] + v.p_vec[i]; + return ret; + } + + Vector operator-(const Vector &v) const { + Vector ret; + for (int i = 0; i < N; i++) + ret.p_vec[i] = p_vec[i] - v.p_vec[i]; + return ret; + } + + Vector operator*(double scalar) const { return scale(scalar); } + + Vector operator/(double scalar) const { + Vector ret; + for (int i = 0; i < N; i++) + ret.p_vec[i] = p_vec[i] / scalar; + return ret; + } + + void toDegrees() { + for (int i = 0; i < N; i++) + p_vec[i] *= 57.2957795131; // 180/pi + } + + void toRadians() { + for (int i = 0; i < N; i++) + p_vec[i] *= 0.01745329251; // pi/180 + } + + double &x() { return p_vec[0]; } + double &y() { return p_vec[1]; } + double &z() { return p_vec[2]; } + double x() const { return p_vec[0]; } + double y() const { return p_vec[1]; } + double z() const { return p_vec[2]; } + +private: + double p_vec[N]; +}; + +template <> inline Vector<3> Vector<3>::cross(const Vector &v) const { + return Vector(p_vec[1] * v.p_vec[2] - p_vec[2] * v.p_vec[1], + p_vec[2] * v.p_vec[0] - p_vec[0] * v.p_vec[2], + p_vec[0] * v.p_vec[1] - p_vec[1] * v.p_vec[0]); +} + +} // namespace imu + +#endif