From df42f2036e37147e706a43ffeffc329b50c756ed Mon Sep 17 00:00:00 2001 From: jojos38 Date: Sun, 15 Sep 2024 15:34:35 +0200 Subject: [PATCH 01/10] Initial skeleton --- lib/bmi323/bmi323_lib.cpp | 887 ++++++++++++++++++++++++ lib/bmi323/bmi323_lib.h | 459 ++++++++++++ lib/bmm350/bmm350.cpp | 702 +++++++++++++++++++ lib/bmm350/bmm350.h | 442 ++++++++++++ src/consts.h | 2 + src/defines.h | 24 +- src/sensors/SensorManager.cpp | 2 + src/sensors/softfusion/drivers/bmi323.h | 279 ++++++++ 8 files changed, 2794 insertions(+), 3 deletions(-) create mode 100644 lib/bmi323/bmi323_lib.cpp create mode 100644 lib/bmi323/bmi323_lib.h create mode 100644 lib/bmm350/bmm350.cpp create mode 100644 lib/bmm350/bmm350.h create mode 100644 src/sensors/softfusion/drivers/bmi323.h diff --git a/lib/bmi323/bmi323_lib.cpp b/lib/bmi323/bmi323_lib.cpp new file mode 100644 index 000000000..a03dedb45 --- /dev/null +++ b/lib/bmi323/bmi323_lib.cpp @@ -0,0 +1,887 @@ +/* + SlimeVR Code is placed under the MIT license + Copyright (c) 2024 jojos38 + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + THE SOFTWARE. +*/ + +#include "bmi323_lib.h" + +/** + * @brief Entry point for bmi323 sensor. It reads and validates the chip-id of the sensor. + */ +int8_t BMI323_LIB::initI2C() { + this->chipId = 0; + + // Reset the sensor before initializing + int8_t result = this->softReset(); + if (result != SUCCESS) return result; + + // Read chip-id of the BMI323 + uint8_t chipId[2] = { 0 }; + result = this->getRegisters(REGISTER_CHIP_ID, chipId, 2); + if (result != SUCCESS) return result; + + // Validate the chip id + if (chipId[0] != CHIP_ID) { + return ERR_DEVICE_NOT_FOUND; + } + + // Set the chip id + this->chipId = chipId[0]; + + // Set the accel bit width + if (((chipId[1] & REV_ID_MASK) >> REV_ID_POS) == ENABLE) { + this->accelBitWidth = ACCEL_DP_OFF_XYZ_14_BIT_MASK; + } + else { + this->accelBitWidth = ACCEL_DP_OFF_XYZ_13_BIT_MASK; + } + + return SUCCESS; +} + +/** + * @brief Writes the available sensor specific commands to the sensor. + */ +int8_t BMI323_LIB::setCommandRegister(uint16_t command) { + // Define register data + uint8_t registerData[2] = { + (uint8_t)(command & SET_LOW_BYTE), + (uint8_t)((command & SET_HIGH_BYTE) >> 8) + }; + + // Set the command in the command register + int8_t result = this->setRegisters(REGISTER_COMMAND, registerData, 2); + + return result; +} + +/** + * @brief Writes data to the given register address of bmi323 sensor. + */ +int8_t BMI323_LIB::setRegisters(uint8_t registerAddress, const uint8_t *data, uint16_t length) { + // Check that the data is not null + if (data == nullptr) return ERR_NULL_PTR; + + // Write the data to the register + this->interfaceResult = this->write(registerAddress, data, length); + this->delayMicroseconds(2); + + // Check that the write was successful + if (this->interfaceResult != SUCCESS_INTERFACE_RESULT) return ERR_INTERFACE_RESULT; + + return SUCCESS_INTERFACE_RESULT; +} + +/** + * @brief Reads the data from the given register address of bmi323 + * sensor. + * + * @note For most of the registers auto address increment applies, with the + * exception of a few special registers, which trap the address. For e.g., + * Register address - 0x03. + */ +int8_t BMI323_LIB::getRegisters(uint8_t registerAddress, uint8_t *data, uint16_t length) { + if (data == nullptr) return ERR_NULL_PTR; + + uint8_t tempBuffer[READ_BUFFER_LENGTH]; + + // Read the data from the register + this->interfaceResult = this->read(registerAddress, tempBuffer, length + DUMMY_BYTE); + this->delayMicroseconds(2); + + // Check that the read was successful + if (this->interfaceResult != SUCCESS_INTERFACE_RESULT) return ERR_INTERFACE_RESULT; + + // Copy the data to the data buffer + uint16_t index = 0; + while (index < length) { + data[index] = tempBuffer[index + DUMMY_BYTE]; + index++; + } + + return SUCCESS_INTERFACE_RESULT; +} + +/** + * @brief Resets bmi323 sensor. All registers are overwritten with their default values. + */ +int8_t BMI323_LIB::softReset() { + int8_t result; + + // Reset bmi323 + result = this->setCommandRegister(COMMAND_SOFT_RESET); + this->delayMicroseconds(DELAY_SOFT_RESET); + if (result != SUCCESS) return result; + + // Enable feature engine + uint8_t featureData[2] = { 0x2c, 0x01 }; + result = this->setRegisters(REGISTER_FEATURE_IO2, featureData, 2); + if (result != SUCCESS) return result; + + // Enable feature status bit + uint8_t featureIOStatus[2] = { ENABLE, 0 }; + result = this->setRegisters(REGISTER_FEATURE_IO_STATUS, featureIOStatus, 2); + if (result != SUCCESS) return result; + + // Enable feature engine bit + uint8_t featureEngineEnable[2] = { ENABLE, 0 }; + result = this->setRegisters(REGISTER_FEATURE_CTRL, featureEngineEnable, 2); + if (result != SUCCESS) return result; + + + // Checking the status bit for feature engine enable + uint8_t registerData[2] = { 0 }; + uint8_t loop = 1; + while (loop <= 10) { + this->delayMicroseconds(100000); + result = this->getRegisters(REGISTER_FEATURE_IO1, registerData, 2); + if (result == SUCCESS) { + if (registerData[0] & FEATURE_ENGINE_ENABLE_MASK) { + result = SUCCESS; + break; + } else { + result = E_FEATURE_ENGINE_STATUS; + } + } + + loop++; + } + + return result; +} + +/** + * @brief Sets offset dgain for the sensor which stores self-calibrated values for gyro. + */ +int8_t BMI323_LIB::setGyroOffsetGain(uint16_t *gyroOffset, uint8_t *gyroGain) { + if (gyroOffset == nullptr || gyroGain == nullptr) return ERR_NULL_PTR; + + // Gyroscope offset + uint16_t gyroOffsetX, gyroOffsetY, gyroOffsetZ; + gyroOffsetX = BMI323_LIB::setBitPos0(0, GYRO_DP_OFF_MASK, gyroOffset[0]); + gyroOffsetY = BMI323_LIB::setBitPos0(0, GYRO_DP_OFF_MASK, gyroOffset[1]); + gyroOffsetZ = BMI323_LIB::setBitPos0(0, GYRO_DP_OFF_MASK, gyroOffset[2]); + + // Gyroscope gain + uint8_t gyroGainX, gyroGainY, gyroGainZ; + gyroGainX = (uint8_t)BMI323_LIB::setBitPos0(0, GYRO_DP_DGAIN_MASK, gyroGain[0]); + gyroGainY = (uint8_t)BMI323_LIB::setBitPos0(0, GYRO_DP_DGAIN_MASK, gyroGain[1]); + gyroGainZ = (uint8_t)BMI323_LIB::setBitPos0(0, GYRO_DP_DGAIN_MASK, gyroGain[2]); + + /// Prepare registers + uint8_t gyroOffsetGain[12] = { 0 }; + // X + gyroOffsetGain[0] = (uint8_t)(gyroOffsetX & SET_LOW_BYTE); + gyroOffsetGain[1] = (gyroOffsetX & SET_HIGH_BYTE) >> 8; + gyroOffsetGain[2] = gyroGainX; + // Y + gyroOffsetGain[4] = (uint8_t)(gyroOffsetY & SET_LOW_BYTE); + gyroOffsetGain[5] = (gyroOffsetY & SET_HIGH_BYTE) >> 8; + gyroOffsetGain[6] = gyroGainY; + // Z + gyroOffsetGain[8] = (uint8_t)(gyroOffsetZ & SET_LOW_BYTE); + gyroOffsetGain[9] = (gyroOffsetZ & SET_HIGH_BYTE) >> 8; + gyroOffsetGain[10] = gyroGainZ; + + // Set offset and gain + uint8_t result = this->setRegisters(REGISTER_GYRO_DP_OFF_X, gyroOffsetGain, 12); + if (result != SUCCESS) return result; + + return SUCCESS; +} + +uint16_t BMI323_LIB::setBitPos0(uint16_t registerData, uint16_t bitMask, uint16_t data) { + return (registerData & ~(bitMask)) | (data & bitMask); +} + +uint16_t BMI323_LIB::setBits(uint16_t registerData, uint16_t bitMask, uint16_t bitPos, uint16_t data) { + return (registerData & ~(bitMask)) | ((data << bitPos) & bitMask); +} + +/** + * @brief Sets the FIFO configuration in the sensor. + */ +int8_t BMI323_LIB::setFifoConfig(uint16_t config, uint8_t enable) { + // Get existing FIFO config + uint8_t data[2] = { 0 }; + int8_t result = this->getRegisters(REGISTER_FIFO_CONF, data, 2); + if (result != SUCCESS) return result; + + // Prepare the new confiuration + uint16_t fifoConfig = config & FIFO_CONFIG_MASK; + if (enable == ENABLE) { + data[0] = data[0] | (uint8_t)fifoConfig; + data[1] = data[1] | (uint8_t)(fifoConfig >> 8); + } else { + data[0] = data[0] & (~fifoConfig); + data[1] = data[1] & (~(fifoConfig >> 8)); + } + + // Apply the new configuration + result = this->setRegisters(REGISTER_FIFO_CONF, data, 2); + if (result != SUCCESS) return result; + + return SUCCESS; +} + +int8_t BMI323_LIB::setAccelConfig(AccelConfig *config) { + if (config == nullptr) return ERR_NULL_PTR; + + return this->setAccelConfig( + config->odr, + config->bandwidth, + config->accelMode, + config->range, + config->avgNum + ); +} + +/** + * @brief Sets accelerometer configurations like ODR, accel mode, bandwidth, average samples and range. + */ +int8_t BMI323_LIB::setAccelConfig(uint8_t odr, uint8_t bandwidth, uint8_t accelMode, uint8_t range, uint8_t avgNum) { + int8_t result; + + // Validate bandwidth and averaging samples + result = this->validateBwAvgAccelMode(&bandwidth, &accelMode, &avgNum); + if (result != SUCCESS) return ERR_ACCEL_INVALID_CFG; + + // Validate ODR and range + result = this->validateAccelOdrRange(&odr, &range); + if (result != SUCCESS) return ERR_ACCEL_INVALID_CFG; + + // Validate odr and avgNum in low power mode + if (accelMode == ACCEL_MODE_LOW_PWR) { + result = this->validateAccelOdrAvg(odr, avgNum); + if (result != SUCCESS) return ERR_ACCEL_INVALID_CFG; + } + + // Valide odr in normal and high power mode (Gyro must be in low power mode to go below 6.25Hz) + if (accelMode == ACCEL_MODE_NORMAL || accelMode == ACCEL_MODE_HIGH_PERF) { + // Accel cannot work at low rate when it's in normal or high power mode + if (odr >= ACCEL_ODR_0_78HZ && odr <= ACCEL_ODR_6_25HZ) { + return ERR_ACCEL_INVALID_CFG; + } + } + + // Prepare register data + uint16_t odrRegister = BMI323_LIB::setBitPos0(0, ACCEL_ODR_MASK, odr); + uint16_t rangeRegister = this->setBits(0, ACCEL_RANGE_MASK, ACCEL_RANGE_POS, range); + uint16_t bandwidthRegister = this->setBits(0, ACCEL_BW_MASK, ACCEL_BW_POS, bandwidth); + uint16_t avgNumRegister = this->setBits(0, ACCEL_AVG_NUM_MASK, ACCEL_AVG_NUM_POS, avgNum); + uint16_t accelModeRegister = this->setBits(0, ACCEL_MODE_MASK, ACCEL_MODE_POS, accelMode); + + uint8_t registerData[2] = { + (uint8_t)(odrRegister | rangeRegister | bandwidthRegister), + (uint8_t)((avgNumRegister | accelModeRegister) >> 8) + }; + + // Set configurations for accel + result = this->setRegisters(REGISTER_ACCEL_CONF, registerData, 2); + if (result != SUCCESS) return result; + + return SUCCESS; +} + +/** + * @brief Used to validate the boundary conditions. + */ +int8_t BMI323_LIB::checkBoundaryVal(uint8_t *val, uint8_t min, uint8_t max) { + if (val == nullptr) return ERR_NULL_PTR; + + // Check if value is below minimum value + if (*val < min) { + // Auto correct the invalid value to minimum value + *val = min; + } + + // Check if value is above maximum value + if (*val > max) { + // Auto correct the invalid value to maximum value + *val = max; + } + + return SUCCESS; +} + +/** + * @brief Validates bandwidth and accel mode of the accelerometer set by the user. + */ +int8_t BMI323_LIB::validateBwAvgAccelMode(uint8_t *bandwidth, uint8_t *accelMode, uint8_t *avgNum) { + if (bandwidth == nullptr || accelMode == nullptr || avgNum == nullptr) return ERR_NULL_PTR; + + int8_t result; + + // Validate and auto-correct accel mode + result = BMI323_LIB::checkBoundaryVal(accelMode, ACCEL_MODE_DISABLE, ACCEL_MODE_HIGH_PERF); + if (result != SUCCESS) return result; + + // Validate for averaging number of samples + result = BMI323_LIB::checkBoundaryVal(avgNum, ACCEL_AVG_1, ACCEL_AVG_64); + if (result != SUCCESS) return result; + + // Validate bandwidth + result = BMI323_LIB::checkBoundaryVal(bandwidth, ACCEL_BANDWIDTH_ODR_HALF, ACCEL_BANDWIDTH_ODR_QUARTER); + if (result != SUCCESS) return result; + + return SUCCESS; +} + +/** + * @brief Validates ODR and range of the accelerometer set by the user. + */ +int8_t BMI323_LIB::validateAccelOdrRange(uint8_t *odr, uint8_t *range) { + if (odr == nullptr || range == nullptr) return ERR_NULL_PTR; + + int8_t result; + + // Validate and auto correct ODR + result = BMI323_LIB::checkBoundaryVal(odr, ACCEL_ODR_0_78HZ, ACCEL_ODR_6400HZ); + if (result != SUCCESS) return result; + + // Validate and auto correct Range + result = BMI323_LIB::checkBoundaryVal(range, ACCEL_RANGE_2G, ACCEL_RANGE_16G); + if (result != SUCCESS) return result; + + return SUCCESS; +} + +/** + * Used to validate ODR and AVG combinations for accel + */ +int8_t BMI323_LIB::validateAccelOdrAvg(uint8_t accelOdr, uint8_t accelAvg) { + float odr = 0.0, avg = 0.0; + + switch (accelOdr) { + case ACCEL_ODR_0_78HZ: + odr = 0.78125; + break; + case ACCEL_ODR_1_56HZ: + odr = 1.5625; + break; + case ACCEL_ODR_3_125HZ: + odr = 3.125; + break; + case ACCEL_ODR_6_25HZ: + odr = 6.25; + break; + case ACCEL_ODR_12_5HZ: + odr = 12.5; + break; + case ACCEL_ODR_25HZ: + odr = 25.0; + break; + case ACCEL_ODR_50HZ: + odr = 50.0; + break; + case ACCEL_ODR_100HZ: + odr = 100.0; + break; + case ACCEL_ODR_200HZ: + odr = 200.0; + break; + case ACCEL_ODR_400HZ: + odr = 400.0; + break; + default: + break; + } + + switch (accelAvg) { + case ACCEL_AVG_1: + avg = 1.0; + break; + case ACCEL_AVG_2: + avg = 2.0; + break; + case ACCEL_AVG_4: + avg = 4.0; + break; + case ACCEL_AVG_8: + avg = 8.0; + break; + case ACCEL_AVG_16: + avg = 16.0; + break; + case ACCEL_AVG_32: + avg = 32.0; + break; + case ACCEL_AVG_64: + avg = 64.0; + break; + default: + break; + } + + return BMI323_LIB::accelSkippedSamplesCheck(odr, avg); +} + +/*! + * @brief Used to check skipped samples for accel + */ +int8_t BMI323_LIB::accelSkippedSamplesCheck(float odr, float avg) { + // Check if odr and avg are valid + if (odr <= 0.0 || avg <= 0.0) return ERR_ACCEL_INVALID_CFG; + + // Check if skipped samples is above 0 + float skippedSamples = (float)(6400.0 / odr) - avg; + if (skippedSamples <= 0.0) return ERR_ACCEL_INVALID_CFG; + + return SUCCESS; +} + +/*! + * Sets gyroscope configurations like ODR, bandwidth, gyro mode, average samples and dps range. + */ +int8_t BMI323_LIB::setGyroConfig(uint8_t odr, uint8_t bandwidth, uint8_t gyroMode, uint8_t range, uint8_t avgNum) { + int8_t result; + + // Validate bandwidth, average samples and mode */ + result = BMI323_LIB::validateBwAvgGyroMode(&bandwidth, &gyroMode, &avgNum); + if (result != SUCCESS) return ERR_GYRO_INVALID_CFG; + + // Validate odr and range + result = BMI323_LIB::validateGyroOdrRange(&odr, &range); + if (result != SUCCESS) return ERR_GYRO_INVALID_CFG; + + // Validate odr and avgNum in low power mode + if (gyroMode == GYRO_MODE_LOW_PWR) { + result = BMI323_LIB::validateGyroOdrAvg(odr, avgNum); + if (result != SUCCESS) return ERR_GYRO_INVALID_CFG; + } + + // Prepare register data + uint16_t odrRegister = BMI323_LIB::setBitPos0(0, GYRO_ODR_MASK, odr); + uint16_t rangeRegister = BMI323_LIB::setBits(0, GYRO_RANGE_MASK, GYRO_RANGE_POS, range); + uint16_t bandwidthRegister = BMI323_LIB::setBits(0, GYRO_BW_MASK, GYRO_BW_POS, bandwidth); + uint16_t avgNumRegister = BMI323_LIB::setBits(0, GYRO_AVG_NUM_MASK, GYRO_AVG_NUM_POS, avgNum); + uint16_t gyroModeRegister = BMI323_LIB::setBits(0, GYRO_MODE_MASK, GYRO_MODE_POS, gyroMode); + + uint8_t registerData[2] = { + (uint8_t)(odrRegister | rangeRegister | bandwidthRegister), + (uint8_t)((avgNumRegister | gyroModeRegister) >> 8) + }; + + // Set gyro configurations + result = this->setRegisters(REGISTER_GYRO_CONF, registerData, 2); + if (result != SUCCESS) return result; + + return SUCCESS; +} + +/*! + * @brief Validates bandwidth, average samples and gyr mode of the gyroscope set by the user. + */ +int8_t BMI323_LIB::validateBwAvgGyroMode(uint8_t *bandwidth, uint8_t *gyroMode, uint8_t *avgNum) { + if (bandwidth == nullptr || gyroMode == nullptr || avgNum == nullptr) return ERR_NULL_PTR; + + int8_t result; + + // Validate and auto-correct gyro mode + result = BMI323_LIB::checkBoundaryVal(gyroMode, GYRO_MODE_DISABLE, GYRO_MODE_HIGH_PERF); + if (result != SUCCESS) return result; + + // Validate for averaging mode + result = BMI323_LIB::checkBoundaryVal(avgNum, GYRO_AVG_1, GYRO_AVG_64); + if (result != SUCCESS) return result; + + // Validate for bandwidth + result = BMI323_LIB::checkBoundaryVal(bandwidth, GYRO_BANDWIDTH_ODR_HALF, GYRO_BANDWIDTH_ODR_QUARTER); + if (result != SUCCESS) return result; + + return SUCCESS; +} + +/*! + * @brief Validates ODR and range of the gyroscope set by + * the user. + */ +int8_t BMI323_LIB::validateGyroOdrRange(uint8_t *odr, uint8_t *range) { + if (odr == nullptr || range == nullptr) return ERR_NULL_PTR; + + int8_t result; + + // Validate and auto correct ODR + result = BMI323_LIB::checkBoundaryVal(odr, GYRO_ODR_0_78HZ, GYRO_ODR_6400HZ); + if (result != SUCCESS) return result; + + // Validate and auto correct Range + result = BMI323_LIB::checkBoundaryVal(range, GYRO_RANGE_125DPS, GYRO_RANGE_2000DPS); + if (result != SUCCESS) return result; + + return SUCCESS; +} + +/*! + * @brief Used to validate ODR and AVG combinations for gyro + */ +int8_t BMI323_LIB::validateGyroOdrAvg(uint8_t gyroOdr, uint8_t gyroAvg) { + + float odr = 0.0, avg = 0.0; + + switch (gyroOdr) + { + case GYRO_ODR_0_78HZ: + odr = 0.78125; + break; + case GYRO_ODR_1_56HZ: + odr = 1.5625; + break; + case GYRO_ODR_3_125HZ: + odr = 3.125; + break; + case GYRO_ODR_6_25HZ: + odr = 6.25; + break; + case GYRO_ODR_12_5HZ: + odr = 12.5; + break; + case GYRO_ODR_25HZ: + odr = 25.0; + break; + case GYRO_ODR_50HZ: + odr = 50.0; + break; + case GYRO_ODR_100HZ: + odr = 100.0; + break; + case GYRO_ODR_200HZ: + odr = 200.0; + break; + case GYRO_ODR_400HZ: + odr = 400.0; + break; + default: + break; + } + + switch (gyroAvg) + { + case GYRO_AVG_1: + avg = 1.0; + break; + case GYRO_AVG_2: + avg = 2.0; + break; + case GYRO_AVG_4: + avg = 4.0; + break; + case GYRO_AVG_8: + avg = 8.0; + break; + case GYRO_AVG_16: + avg = 16.0; + break; + case GYRO_AVG_32: + avg = 32.0; + break; + case GYRO_AVG_64: + avg = 64.0; + break; + default: + break; + } + + return BMI323_LIB::gyroSkippedSamplesCheck(odr, avg); +} + +/*! + * @brief Used to check skipped samples for gyro + */ +int8_t BMI323_LIB::gyroSkippedSamplesCheck(float odr, float avg) { + // Check of odr and avg are valid + if (odr <= 0.0 || avg <= 0.0) return ERR_GYRO_INVALID_CFG; + + // Check if skipped samples is above 0 + float skippedSamples = (float)(6400.0 / odr) - avg; + if (skippedSamples <= 0.0) return ERR_GYRO_INVALID_CFG; + + return SUCCESS; +} + +/*! + * @brief This API is used to perform the self-calibration for either sensitivity or offset or both. + */ +int8_t BMI323_LIB::performGyroCalibration(uint8_t calibrationType, uint8_t applyCalibration, SelfCalibResult *calibrationResult) { + if (calibrationResult == nullptr) return ERR_NULL_PTR; + + int8_t result; + + struct AccelConfig previousConfig; + + result = getAccelConfig(&previousConfig); + if (result != SUCCESS) return result; + + result = setAccelConfig( + ACCEL_ODR_100HZ, + previousConfig.bandwidth, + ACCEL_MODE_HIGH_PERF, + ACCEL_RANGE_8G, + previousConfig.avgNum + ); + if (result != SUCCESS) return result; + + // Disable alternate accel and gyro mode + // rslt = disable_alt_conf_acc_gyr_mode(dev); // TODO Too lazy to implement for now + + // Get and set the self-calibration mode given by the user in the self-calibration dma register. + result = this->getSetCalibrationDMA(CALIBRATION_OFFSET | CALIBRATION_SENSITIVITY, applyCalibration); + if (result != SUCCESS) return result; + + // Trigger the self-calibration command + result = this->setCommandRegister(COMMAND_SELF_CALIB_TRIGGER); + if (result != SUCCESS) return result; + + // Get the self-calibration status and result + result = this->getGyroCalibrationResult(calibrationResult); + if (result != SUCCESS) return result; + + // Restore accel configurations + result = this->setAccelConfig(&previousConfig); + if (result != SUCCESS) return result; + + return SUCCESS; +} + +/*! + * @brief This internal API gets accelerometer configurations like ODR, + * bandwidth, accel mode, average samples and gravity range. + */ +int8_t BMI323_LIB::getAccelConfig(struct AccelConfig *config) { + if (config == nullptr) return ERR_NULL_PTR; + + int8_t result; + + uint8_t dataArray[2] = { 0 }; + result = this->getRegisters(REGISTER_ACCEL_CONF, dataArray, 2); + if (result != SUCCESS) return result; + + uint16_t registerData = dataArray[0]; + + /* Get accelerometer ODR */ + config->odr = BMI323_LIB::getBitPos0(registerData, ACCEL_ODR_MASK); + + /* Get accelerometer range */ + config->range = BMI323_LIB::getBits(registerData, ACCEL_RANGE_MASK, ACCEL_RANGE_POS); + + /* Get accelerometer bandwidth */ + config->bandwidth = BMI323_LIB::getBits(registerData, ACCEL_BW_MASK, ACCEL_BW_POS); + registerData = (uint16_t)dataArray[1] << 8; + + /* Get accelerometer average samples */ + config->avgNum = BMI323_LIB::getBits(registerData, ACCEL_AVG_NUM_MASK, ACCEL_AVG_NUM_POS); + + /* Get accel mode */ + config->accelMode = BMI323_LIB::getBits(registerData, ACCEL_MODE_MASK, ACCEL_MODE_POS); + + return SUCCESS; +} + +uint16_t BMI323_LIB::getBitPos0(uint16_t registerData, uint16_t bitMask) { + return registerData & bitMask; +} + +uint16_t BMI323_LIB::getBits(uint16_t registerData, uint16_t bitMask, uint16_t bitPos) { + return (registerData & bitMask) >> bitPos; +} + +/** + * @brief This internal API gets and sets the self-calibration mode given + * by the user in the self-calibration dma register. + */ +int8_t BMI323_LIB::getSetCalibrationDMA(uint8_t calibrationType, uint8_t applyCalibration) { + int8_t result; + + // Array to set the base address of self-calibration feature + uint8_t calibrationBaseAddress[2] = { BASE_ADDR_GYRO_CALIBRATION_SELECT, 0 }; + + result = this->setRegisters(REGISTER_FEATURE_DATA_ADDRESS, calibrationBaseAddress, 2); + if (result != SUCCESS) return result; + + uint8_t registerData[2]; + result = this->getRegisters(REGISTER_FEATURE_DATA_TX, registerData, 2); + if (result != SUCCESS) return result; + + // The value of apply correction is appended with the selection given by the user + registerData[0] = (applyCalibration | calibrationType); + + result = this->setRegisters(REGISTER_FEATURE_DATA_ADDRESS, calibrationBaseAddress, 2); + if (result != SUCCESS) return result; + + result = this->setRegisters(REGISTER_FEATURE_DATA_TX, registerData, 2); + if (result != SUCCESS) return result; + + return SUCCESS; +} + +/** + * @brief Used to get the status of gyro self-calibration and the result of the event + */ +int8_t BMI323_LIB::getGyroCalibrationResult(struct SelfCalibResult *calibrationResult) { + int8_t result; + + calibrationResult->calibrationErrorResult = 0; + + uint8_t idx; + uint8_t limit = 25; + for (idx = 0; idx < limit; idx++) { + /* A delay of 120ms is required to read the error status register */ + this->delayMicroseconds(120000); + + uint8_t data_array[2]; + result = this->getRegisters(REGISTER_FEATURE_IO1, data_array, 2); + if (result != SUCCESS) return result; + + uint8_t calibrationStatus = (data_array[0] & CALIB_ST_STATUS_MASK) >> CALIB_ST_COMPLETE_POS; + uint8_t featureEngineErrRegLsb, featureEngineErrRegMsb; + if ((calibrationStatus == 1) && (result == SUCCESS)) { + calibrationResult->gyroCalibrationResult = (data_array[0] & GYRO_CALIB_RESULT_MASK) >> GYRO_CALIB_RESULT_POS; + } + + result = this->getFeatureEngineErrorStatus(&featureEngineErrRegLsb, &featureEngineErrRegMsb); + if (result != SUCCESS) return result; + calibrationResult->calibrationErrorResult = featureEngineErrRegLsb; + } + + return SUCCESS; +} + +/*! + * @brief Reads the feature engine error status from the sensor. + */ +int8_t BMI323_LIB::getFeatureEngineErrorStatus(uint8_t *featureEngineErrRegLsb, uint8_t *featureEngineErrRegMsb) { + if (featureEngineErrRegLsb == nullptr || featureEngineErrRegMsb == nullptr) return ERR_NULL_PTR; + + int8_t result; + + // Array variable to get error status from register + uint8_t data[2]; + + // Read the feature engine error codes + result = this->getRegisters(REGISTER_FEATURE_IO1, data, 2); + if (result != SUCCESS) return result; + + *featureEngineErrRegLsb = data[0]; + *featureEngineErrRegMsb = data[1]; + + return SUCCESS; +} + +/*! + * @brief Gets the length of FIFO data available in the sensor in bytes + */ +int8_t BMI323_LIB::getFifoLength(uint16_t *fifoAvailableLength) { + if (fifoAvailableLength == nullptr) return ERR_NULL_PTR; + + int8_t result; + + // Array to store FIFO data length + uint8_t data[LENGTH_FIFO_DATA] = { 0 }; + + // Read FIFO length + result = this->getRegisters(REGISTER_FIFO_FILL_LEVEL, data, 2); + if (result != SUCCESS) return result; + + // Get the MSB byte of FIFO length + data[0] = BMI323_LIB::getBitPos0(data[0], FIFO_FILL_LEVEL_MASK); + uint16_t registerData; + registerData = ((uint16_t)data[1] << 8); + registerData = BMI323_LIB::getBitPos0(registerData, FIFO_FILL_LEVEL_MASK); + + // Get total FIFO length + *fifoAvailableLength = (uint16_t)(registerData | data[0]) * 2; + if (*fifoAvailableLength == 0) return WARN_FIFO_EMPTY; + + return SUCCESS; +} + +/*! + * @brief Reads the FIFO data + */ +int8_t BMI323_LIB::readFifoData(uint8_t *data, const uint16_t length) { + if (data == nullptr) return ERR_NULL_PTR; + if (length == 0) return ERR_INTERFACE_RESULT; + + uint8_t configData[2] = { 0 }; + this->getRegisters(REGISTER_FIFO_CONF, configData, 2); + + return this->read(REGISTER_FIFO_DATA, data, (uint32_t)length); +} + +/*! + * @brief This API gets offset dgain for the sensor which stores self-calibrated values for gyro. + */ +int8_t BMI323_LIB::getGyroOffsetGain(uint16_t *offset, uint8_t* gain) { + if (offset == nullptr || gain == nullptr) return ERR_NULL_PTR; + + int8_t result; + + // Get temperature user offset for gyro + uint8_t registerData[12]; + result = this->getRegisters(REGISTER_GYRO_DP_OFF_X, registerData, 12); + if (result != SUCCESS) return result; + + // Offset register + uint16_t offsetX = (uint16_t)(((uint16_t)registerData[1] << 8) | registerData[0]); + uint16_t offsetY = (uint16_t)(((uint16_t)registerData[5] << 8) | registerData[4]); + uint16_t offsetZ = (uint16_t)(((uint16_t)registerData[9] << 8) | registerData[8]); + + // Gain register + uint8_t gainX = registerData[2]; + uint8_t gainY = registerData[6]; + uint8_t gainZ = registerData[10]; + + // Offset + offset[0] = BMI323_LIB::getBitPos0(offsetX, GYRO_DP_OFF_MASK); + offset[1] = BMI323_LIB::getBitPos0(offsetY, GYRO_DP_OFF_MASK); + offset[2] = BMI323_LIB::getBitPos0(offsetZ, GYRO_DP_OFF_MASK); + + // Gain + gain[0] = BMI323_LIB::getBitPos0(gainX, GYRO_DP_DGAIN_MASK); + gain[1] = BMI323_LIB::getBitPos0(gainY, GYRO_DP_DGAIN_MASK); + gain[2] = BMI323_LIB::getBitPos0(gainZ, GYRO_DP_DGAIN_MASK); + + return SUCCESS; +} + +/*! + * @brief Returns the sensor temperature in Celcius + */ +int8_t BMI323_LIB::getTemperatureCelcius(float *temperature) { + if (temperature == nullptr) return ERR_NULL_PTR; + + int8_t result; + + // Array to define data stored in register + uint8_t registerData[2] = { 0 }; + + // Read the sensor data + result = this->getRegisters(REGISTER_TEMP_DATA, registerData, 2); + if (result != SUCCESS) return result; + + // Parse register data + uint16_t rawTemperature = (uint16_t)(registerData[0] | ((uint16_t)registerData[1] << 8)); + + // Convert to Celcius + *temperature = (float)(((float)((int16_t)rawTemperature)) / 512.0) + 23.0; + + return SUCCESS; +} \ No newline at end of file diff --git a/lib/bmi323/bmi323_lib.h b/lib/bmi323/bmi323_lib.h new file mode 100644 index 000000000..ded06bd21 --- /dev/null +++ b/lib/bmi323/bmi323_lib.h @@ -0,0 +1,459 @@ +/* + SlimeVR Code is placed under the MIT license + Copyright (c) 2024 jojos38 + + 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 BMI323_LIB_H +#define BMI323_LIB_H + +#include + +typedef int8_t (*ReadFunction)(uint8_t, uint8_t*, uint32_t, const void*); +typedef int8_t (*WriteFunction)(uint8_t, const uint8_t*, uint32_t, const void *); +typedef void (*DelayFunction)(uint32_t, const void *); + +class BMI323_LIB { + + public: + /****************************************************************************************** + * PUBLIC STRUCTS + ******************************************************************************************/ + struct AccelConfig { + /*! Output data rate in Hz */ + uint8_t odr; + + /*! Bandwidth parameter */ + uint8_t bandwidth; + + /*! Filter accel mode */ + uint8_t accelMode; + + /*! Gravity range */ + uint8_t range; + + /*! Defines the number of samples to be averaged */ + uint8_t avgNum; + }; + + /*! + * @brief Structure to store self calibration result + */ + struct SelfCalibResult { + /*! Stores the self-calibration result */ + uint8_t gyroCalibrationResult; + + /*! Stores the self-calibration error codes result */ + uint8_t calibrationErrorResult; + }; + + /****************************************************************************************** + * PUBLIC CONSTANTS + ******************************************************************************************/ + // General + static const uint16_t CHIP_ID = UINT16_C(0x0043); + static const uint8_t ENABLE = UINT8_C(1); + static const uint8_t DISABLE = UINT8_C(0); + static const uint8_t ADDRESS_I2C_PRIMARY = UINT8_C(0x68); + static const uint8_t ADDRESS_I2C_SECONDARY = UINT8_C(0x69); + + // Success + static const int8_t SUCCESS = INT8_C(0); + static const int8_t SUCCESS_INTERFACE_RESULT = INT8_C(0); + + // Errors + static const int8_t ERR_NULL_PTR = INT8_C(-1); + static const int8_t ERR_INTERFACE_RESULT = INT8_C(-2); + static const int8_t ERR_DEVICE_NOT_FOUND = INT8_C(-3); + static const int8_t ERR_ACCEL_INVALID_CFG = INT8_C(-4); + static const int8_t ERR_GYRO_INVALID_CFG = INT8_C(-5); + + // Warnings + static const uint8_t WARN_FIFO_EMPTY = UINT8_C(1); + + // I²C + static const uint8_t READ_BUFFER_LENGTH = UINT8_C(128); + static const uint8_t DUMMY_BYTE = UINT8_C(2); + + // FIFO + static const uint16_t FIFO_TIME_EN = UINT16_C(0x0100); + static const uint16_t FIFO_ACCEL_EN = UINT16_C(0x0200); + static const uint16_t FIFO_GYRO_EN = UINT16_C(0x0400); + static const uint16_t FIFO_TEMP_EN = UINT16_C(0x0800); + static const uint16_t FIFO_ALL_EN = UINT16_C(0x0F00); + + // FIFO Dummy frames + static const uint16_t FIFO_GYRO_DUMMY_FRAME = UINT16_C(0x7F02); + static const uint16_t FIFO_ACCEL_DUMMY_FRAME = UINT16_C(0x7F01); + static const uint16_t FIFO_TEMP_DUMMY_FRAME = UINT16_C(0x8000); + + // FIFO Lengths + static const uint8_t LENGTH_FIFO_ACCEL = UINT8_C(6); + static const uint8_t LENGTH_FIFO_GYRO = UINT8_C(6); + static const uint8_t LENGTH_TEMPERATURE = UINT8_C(2); + static const uint8_t LENGTH_SENSOR_TIME = UINT8_C(2); + + // Calibration + static const uint8_t CALIBRATION_SENSITIVITY = UINT8_C(1); + static const uint8_t CALIBRATION_OFFSET = UINT8_C(2); + static const uint8_t CALIBRATION_APPLY_FALSE = UINT8_C(0); + static const uint8_t CALIBRATION_APPLY_TRUE = UINT8_C(4); + + /** + * Accelerometer power mode + * ACCEL_MODE_DISABLE: Accelerometer is disabled + * ACCEL_MODE_LOW_PWR: Data rate is limited from 12.5Hz to 400Hz + */ + static const uint8_t ACCEL_MODE_DISABLE = UINT8_C(0x00); + static const uint8_t ACCEL_MODE_LOW_PWR = UINT8_C(0x03); + static const uint8_t ACCEL_MODE_NORMAL = UINT8_C(0X04); + static const uint8_t ACCEL_MODE_HIGH_PERF = UINT8_C(0x07); + + /** + * Choose the number of values the accelerometer should average for the last reading. + * Averaging can help reduce noise and provide a smoother accelerometer output. + * ACCEL_AVG_1: No averaging (raw values) + * ACCEL_AVG_X: Averae over X values + */ + static const uint8_t ACCEL_AVG_1 = UINT8_C(0x00); + static const uint8_t ACCEL_AVG_2 = UINT8_C(0x01); + static const uint8_t ACCEL_AVG_4 = UINT8_C(0x02); + static const uint8_t ACCEL_AVG_8 = UINT8_C(0x03); + static const uint8_t ACCEL_AVG_16 = UINT8_C(0x04); + static const uint8_t ACCEL_AVG_32 = UINT8_C(0x05); + static const uint8_t ACCEL_AVG_64 = UINT8_C(0x06); + + /** + * Accel output data rate bandwidth + * + * ACCEL_BANDWIDTH_ODR_HALF: Moderate filtering, tolerates some high-frequency noise. + * Provides a balance between filtering and responsiveness. + * Use when you need a reasonably smooth signal while preserving some high-frequency components. + * + * ACCEL_BANDWIDTH_ODR_QUARTER: Heavily filtered, reduced high-frequency noise. + * Offers higher noise reduction at the cost of reduced bandwidth. + * Choose for applications where filtering out high-frequency noise is critical, + * even if it means sacrificing some responsiveness. + */ + static const uint8_t ACCEL_BANDWIDTH_ODR_HALF = UINT8_C(0); + static const uint8_t ACCEL_BANDWIDTH_ODR_QUARTER = UINT8_C(1); + + /** + * Accelerometer data output rate in Hertz + * Numbers with underscores should be interpreted as comma + * ACCEL_ODR_0_78HZ: 0.78Hz + * ACCEL_ODR_100HZ: 100Hz + * etc... + */ + static const uint8_t ACCEL_ODR_0_78HZ = UINT8_C(0x01); + static const uint8_t ACCEL_ODR_1_56HZ = UINT8_C(0x02); + static const uint8_t ACCEL_ODR_3_125HZ = UINT8_C(0x03); + static const uint8_t ACCEL_ODR_6_25HZ = UINT8_C(0x04); + static const uint8_t ACCEL_ODR_12_5HZ = UINT8_C(0x05); + static const uint8_t ACCEL_ODR_25HZ = UINT8_C(0x06); + static const uint8_t ACCEL_ODR_50HZ = UINT8_C(0x07); + static const uint8_t ACCEL_ODR_100HZ = UINT8_C(0x08); + static const uint8_t ACCEL_ODR_200HZ = UINT8_C(0x09); + static const uint8_t ACCEL_ODR_400HZ = UINT8_C(0x0A); + static const uint8_t ACCEL_ODR_800HZ = UINT8_C(0x0B); + static const uint8_t ACCEL_ODR_1600HZ = UINT8_C(0x0C); + static const uint8_t ACCEL_ODR_3200HZ = UINT8_C(0x0D); + static const uint8_t ACCEL_ODR_6400HZ = UINT8_C(0x0E); + + /** + * The maximum accelerometer range in G + * Selecting a higher range allows the accelerometer to measure larger accelerations but may reduce sensitivity to smaller ones. + * Conversely, selecting a lower range provides higher sensitivity to smaller accelerations but may saturate at higher values. + */ + static const uint8_t ACCEL_RANGE_2G = UINT8_C(0x00); + static const uint8_t ACCEL_RANGE_4G = UINT8_C(0x01); + static const uint8_t ACCEL_RANGE_8G = UINT8_C(0x02); + static const uint8_t ACCEL_RANGE_16G = UINT8_C(0x03); + + /** + * Choose the number of values the gyroscope should average for the last reading. + * Averaging can help reduce noise and provide a smoother accelerometer output. + * GYRO_AVG_1: No averaging (raw values) + * GYRO_AVG_X: Averae over X values + */ + static const uint8_t GYRO_AVG_1 = UINT8_C(0x00); + static const uint8_t GYRO_AVG_2 = UINT8_C(0x01); + static const uint8_t GYRO_AVG_4 = UINT8_C(0x02); + static const uint8_t GYRO_AVG_8 = UINT8_C(0x03); + static const uint8_t GYRO_AVG_16 = UINT8_C(0x04); + static const uint8_t GYRO_AVG_32 = UINT8_C(0x05); + static const uint8_t GYRO_AVG_64 = UINT8_C(0x06); + + /** + * Gyroscope data output rate in Hertz + * Numbers with underscores should be interpreted as comma + * GYRO_ODR_0_78HZ: 0.78Hz + * GYRO_ODR_100HZ: 100Hz + * etc... + */ + static const uint8_t GYRO_ODR_0_78HZ = UINT8_C(0x01); + static const uint8_t GYRO_ODR_1_56HZ = UINT8_C(0x02); + static const uint8_t GYRO_ODR_3_125HZ = UINT8_C(0x03); + static const uint8_t GYRO_ODR_6_25HZ = UINT8_C(0x04); + static const uint8_t GYRO_ODR_12_5HZ = UINT8_C(0x05); + static const uint8_t GYRO_ODR_25HZ = UINT8_C(0x06); + static const uint8_t GYRO_ODR_50HZ = UINT8_C(0x07); + static const uint8_t GYRO_ODR_100HZ = UINT8_C(0x08); + static const uint8_t GYRO_ODR_200HZ = UINT8_C(0x09); + static const uint8_t GYRO_ODR_400HZ = UINT8_C(0x0A); + static const uint8_t GYRO_ODR_800HZ = UINT8_C(0x0B); + static const uint8_t GYRO_ODR_1600HZ = UINT8_C(0x0C); + static const uint8_t GYRO_ODR_3200HZ = UINT8_C(0x0D); + static const uint8_t GYRO_ODR_6400HZ = UINT8_C(0x0E); + + /** + * Gyroscope Range Options (DPS - Degrees Per Second): + * + * Set the maximum range of the gyroscope in degrees per second (DPS). The range + * determines the maximum rate of angular rotation that the gyroscope can measure + * accurately without saturation. + * + * Selecting a higher range allows the gyroscope to measure faster rotations but may reduce sensitivity to smaller ones. + * Conversely, selecting a lower range provides higher sensitivity to smaller rotations but may saturate at higher values. + * + * GYRO_RANGE_125DPS: Best for very slow rotations with no high frequencies. e.g track sun position. + * GYRO_RANGE_250DPS: Suitable for relatively slow or moderate rotational motion. e.g drone stabilization + * GYRO_RANGE_500DPS: Appropriate for applications involving faster rotational motion. e.g camera stabilizer. + * GYRO_RANGE_1000DPS: Ideal for capturing faster and dynamic motion. e.g robotics performing agile maneuvers. + * GYRO_RANGE_2000DPS: Recommended for rapid and high-frequency rotation. e.g ultra fast drones, ultra fast robots + */ + static const uint8_t GYRO_RANGE_125DPS = UINT8_C(0x00); + static const uint8_t GYRO_RANGE_250DPS = UINT8_C(0x01); + static const uint8_t GYRO_RANGE_500DPS = UINT8_C(0x02); + static const uint8_t GYRO_RANGE_1000DPS = UINT8_C(0x03); + static const uint8_t GYRO_RANGE_2000DPS = UINT8_C(0x04); + + /** + * Gyroscope power mode + * GYRO_MODE_DISABLE: Gyroscope is disabled + */ + static const uint8_t GYRO_MODE_DISABLE = UINT8_C(0x00); + static const uint8_t GYRO_MODE_SUSPEND = UINT8_C(0x01); + static const uint8_t GYRO_MODE_LOW_PWR = UINT8_C(0x03); + static const uint8_t GYRO_MODE_NORMAL = UINT8_C(0x04); + static const uint8_t GYRO_MODE_HIGH_PERF = UINT8_C(0x07); + + /** + * Gyroscope output data rate bandwidth + * + * GYRO_BANDWIDTH_ODR_HALF: Moderate filtering, tolerates some high-frequency noise. + * Provides a balance between filtering and responsiveness. + * Use when you need a reasonably smooth signal while preserving some high-frequency components. + * + * GYRO_BANDWIDTH_ODR_QUARTER: Heavily filtered, reduced high-frequency noise. + * Offers higher noise reduction at the cost of reduced bandwidth. + * Choose for applications where filtering out high-frequency noise is critical, + * even if it means sacrificing some responsiveness. + */ + static const uint8_t GYRO_BANDWIDTH_ODR_HALF = UINT8_C(0); + static const uint8_t GYRO_BANDWIDTH_ODR_QUARTER = UINT8_C(1); + + /****************************************************************************************** + * CONSTRUCTOR FUNCTIONS + ******************************************************************************************/ + BMI323_LIB(ReadFunction readFunc, WriteFunction writeFunc, DelayFunction delayFunc, const void* interfacePtr) : + readFunction(readFunc), + writeFunction(writeFunc), + delayFunction(delayFunc), + interfacePointer(interfacePtr) + {}; + ~BMI323_LIB() {}; + + int8_t read(uint8_t registerAddress, uint8_t* regData, uint32_t length) { + return readFunction(registerAddress, regData, length, this->interfacePointer); + } + + int8_t write(uint8_t registerAddress, const uint8_t* regData, uint32_t length) { + return writeFunction(registerAddress, regData, length, this->interfacePointer); + } + + void delayMicroseconds(uint32_t period) { + delayFunction(period, this->interfacePointer); + } + + /****************************************************************************************** + * PUBLIC FUNCTIONS + ******************************************************************************************/ + /** + * Initialize the + */ + int8_t initI2C(); + int8_t softReset(); + int8_t setGyroOffsetGain(uint16_t *gyroOffset, uint8_t *gyroGain); + int8_t setFifoConfig(uint16_t config, uint8_t enable); + int8_t getAccelConfig(struct AccelConfig *config); + int8_t performGyroCalibration(uint8_t calibrationType, uint8_t applyCalibration, SelfCalibResult *calibrationResult); + int8_t getGyroOffsetGain(uint16_t *offset, uint8_t* gain); + int8_t getFifoLength(uint16_t *fifoAvailableLength); + int8_t readFifoData(uint8_t *data, const uint16_t length); + int8_t getTemperatureCelcius(float *temperature); + int8_t setCommandRegister(uint16_t command); + int8_t setRegisters(uint8_t registerAddress, const uint8_t *data, uint16_t length); + int8_t getRegisters(uint8_t registerAddress, uint8_t *data, uint16_t length); + + /** + * @brief Set accelerometer configuration (Output data rate, bandwidth, mode, range and averaging) + * @param odr Any ACCEL_ODR value. Output data rate in hertz + * Defines how many accelerometer values you will be able to gather each second + * @param bandwidth Any ACCEL_BANDWIDTH value. + * For high-frequency vibrations, opt for "ACCEL_BANDWIDTH_ODR_HALF" to maintain responsiveness. + * For low-frequency motion or noise reduction, "ACCEL_BANDWIDTH_ODR_QUARTER" is more suitable. + * @param accelMode Any ACCEL_MODE value. + * Defines accelerometer mode + * @param range Any ACCEL_RANGE value. + * Defines the range of the accelerometer in G + * @param avgNum Any ACCEL_AVG value. + * Defines how many values should be averaged + */ + int8_t setAccelConfig(AccelConfig *config); + int8_t setAccelConfig(uint8_t odr, uint8_t bandwidth, uint8_t accelMode, uint8_t range, uint8_t avgNum); + int8_t setGyroConfig(uint8_t odr, uint8_t bandwidth, uint8_t gyroMode, uint8_t range, uint8_t avgNum); + + /****************************************************************************************** + * PUBLIC VARIABLES + ******************************************************************************************/ + // The chip id of the sensor + int8_t chipId; + // The result of the last I²C read or write operation (the return value of the user's function) + int8_t interfaceResult; + // The number of bits of the accelerometers, it's define by the chip revision id + uint16_t accelBitWidth; + + private: + /****************************************************************************************** + * PRIVATE CONSTANTS + ******************************************************************************************/ + // Commands + static const uint16_t COMMAND_SOFT_RESET = UINT16_C(0xDEAF); + static const uint16_t COMMAND_SELF_CALIB_TRIGGER = UINT16_C(0x0101); + + // Registers + static const uint8_t REGISTER_COMMAND = UINT8_C(0x7E); + static const uint8_t REGISTER_FEATURE_IO1 = UINT8_C(0x11); + static const uint8_t REGISTER_FEATURE_IO2 = UINT8_C(0x12); + static const uint8_t REGISTER_FEATURE_IO_STATUS = UINT8_C(0x14); + static const uint8_t REGISTER_FEATURE_CTRL = UINT8_C(0x40); + static const uint8_t REGISTER_CHIP_ID = UINT8_C(0x00); + static const uint8_t REGISTER_GYRO_DP_OFF_X = UINT8_C(0x66); + static const uint8_t REGISTER_FIFO_CONF = UINT8_C(0x36); + static const uint8_t REGISTER_FIFO_CTRL = UINT8_C(0x37); + static const uint8_t REGISTER_ACCEL_CONF = UINT8_C(0x20); + static const uint8_t REGISTER_GYRO_CONF = UINT8_C(0x21); + static const uint8_t REGISTER_FEATURE_DATA_ADDRESS = UINT8_C(0x41); + static const uint8_t REGISTER_FEATURE_DATA_TX = UINT8_C(0x42); + static const uint8_t REGISTER_FIFO_FILL_LEVEL = UINT8_C(0x15); + static const uint8_t REGISTER_FIFO_DATA = UINT8_C(0x16); + static const uint8_t REGISTER_TEMP_DATA = UINT8_C(0x09); + + // Delays + static const uint16_t DELAY_SOFT_RESET = UINT16_C(2000); + + // Feature engine + static const int8_t E_FEATURE_ENGINE_STATUS = INT8_C(-14); + static const uint16_t FEATURE_ENGINE_ENABLE_MASK = UINT16_C(0X0001); + + // Revision id + static const uint8_t REV_ID_POS = UINT8_C(4); + static const uint8_t REV_ID_MASK = UINT8_C(0xF0); + + // Set low and high byte + static const uint16_t SET_LOW_BYTE = UINT16_C(0x00FF); + static const uint16_t SET_HIGH_BYTE = UINT16_C(0xFF00); + + // Accel + static const uint16_t ACCEL_DP_OFF_XYZ_13_BIT_MASK = UINT16_C(0x1FFF); + static const uint16_t ACCEL_DP_OFF_XYZ_14_BIT_MASK = UINT16_C(0x3FFF); + static const uint16_t ACCEL_ODR_MASK = UINT16_C(0x000F); + static const uint16_t ACCEL_RANGE_MASK = UINT16_C(0x0070); + static const uint16_t ACCEL_RANGE_POS = UINT8_C(4); + static const uint16_t ACCEL_BW_MASK = UINT16_C(0x0080); + static const uint16_t ACCEL_BW_POS = UINT8_C(7); + static const uint16_t ACCEL_AVG_NUM_MASK = UINT16_C(0x0700); + static const uint16_t ACCEL_AVG_NUM_POS = UINT8_C(8); + static const uint16_t ACCEL_MODE_MASK = UINT16_C(0x7000); + static const uint16_t ACCEL_MODE_POS = UINT8_C(12); + + // Gyro + static const uint16_t GYRO_DP_OFF_MASK = UINT16_C(0x03FF); + static const uint16_t GYRO_DP_DGAIN_MASK = UINT16_C(0x007F); + static const uint16_t GYRO_ODR_MASK = UINT16_C(0x000F); + static const uint16_t GYRO_RANGE_MASK = UINT16_C(0x0070); + static const uint8_t GYRO_RANGE_POS = UINT8_C(4); + static const uint16_t GYRO_BW_MASK = UINT16_C(0x0080); + static const uint8_t GYRO_BW_POS = UINT8_C(7); + static const uint16_t GYRO_AVG_NUM_MASK = UINT16_C(0x0700); + static const uint8_t GYRO_AVG_NUM_POS = UINT8_C(8); + static const uint16_t GYRO_MODE_MASK = UINT16_C(0x7000); + static const uint8_t GYRO_MODE_POS = UINT8_C(12); + + static const uint8_t BASE_ADDR_GYRO_CALIBRATION_SELECT = UINT8_C(0x26); + + // Fifo + static const uint16_t FIFO_CONFIG_MASK = UINT16_C(0x0F01); + static const uint8_t LENGTH_FIFO_DATA = UINT8_C(2); + static const uint16_t FIFO_FILL_LEVEL_MASK = UINT16_C(0x07FF); + + // Calibration + static const uint8_t CALIB_ST_STATUS_MASK = UINT8_C(0x10); + static const uint8_t CALIB_ST_RESULT_MASK = UINT8_C(0x40); + static const uint16_t CALIB_ST_COMPLETE_MASK = UINT16_C(0x0010); + static const uint8_t CALIB_ST_COMPLETE_POS = UINT8_C(4); + static const uint16_t GYRO_CALIB_RESULT_MASK = UINT16_C(0x0020); + static const uint8_t GYRO_CALIB_RESULT_POS = UINT8_C(5); + + + + /****************************************************************************************** + * PRIVATE FUNCTIONS + ******************************************************************************************/ + static uint16_t setBitPos0(uint16_t registerData, uint16_t bitMask, uint16_t data); + static uint16_t getBitPos0(uint16_t registerData, uint16_t bitMask); + static uint16_t setBits(uint16_t registerData, uint16_t bitMask, uint16_t bitPos, uint16_t data); + static uint16_t getBits(uint16_t registerData, uint16_t bitMask, uint16_t bitPos); + static int8_t checkBoundaryVal(uint8_t *val, uint8_t min, uint8_t max); + + static int8_t validateBwAvgAccelMode(uint8_t *bandwidth, uint8_t *accelMode, uint8_t *avgNum); + static int8_t validateAccelOdrRange(uint8_t *odr, uint8_t *range); + static int8_t validateAccelOdrAvg(uint8_t accelOdr, uint8_t accelAvg); + static int8_t accelSkippedSamplesCheck(float odr, float avg); + + static int8_t validateBwAvgGyroMode(uint8_t *bandwidth, uint8_t *gyroMode, uint8_t *avgNum); + static int8_t validateGyroOdrRange(uint8_t *odr, uint8_t *range); + static int8_t validateGyroOdrAvg(uint8_t gyr_odr, uint8_t gyr_avg); + static int8_t gyroSkippedSamplesCheck(float odr, float avg); + + int8_t getSetCalibrationDMA(uint8_t calibrationType, uint8_t applyCalibration); + int8_t getFeatureEngineErrorStatus(uint8_t *featureEngineErrRegLsb, uint8_t *featureEngineErrRegMsb); + int8_t getGyroCalibrationResult(struct SelfCalibResult *calibrationResult); + + /****************************************************************************************** + * PRIVATE VARIABLES + ******************************************************************************************/ + ReadFunction readFunction; + WriteFunction writeFunction; + DelayFunction delayFunction; + const void *interfacePointer; +}; +#endif \ No newline at end of file diff --git a/lib/bmm350/bmm350.cpp b/lib/bmm350/bmm350.cpp new file mode 100644 index 000000000..b5040a0ff --- /dev/null +++ b/lib/bmm350/bmm350.cpp @@ -0,0 +1,702 @@ +/* + SlimeVR Code is placed under the MIT license + Copyright (c) 2024 jojos38 + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + THE SOFTWARE. +*/ + +#include "bmm350.h" + +/** + * @brief Entry point for bmm350 sensor. It reads and validates the chip-id of the sensor. + */ +int8_t BMM350::initI2C() { + int8_t result; + this->chipId = 0; + + // Assign axis_en with all axis enabled (BMM350_EN_XYZ_MSK) + this->enabledAxes = EN_XYZ_MASK; + + this->delayMicroseconds(START_UP_TIME_FROM_POR); + + // Set the soft reset command in the command register + uint8_t command = COMMAND_SOFTRESET; + result = this->setRegisters(REGISTER_CMD, &command, 1); + if (result != SUCCESS) return result; + + // Wait for the sensor to come back up + this->delayMicroseconds(SOFT_RESET_DELAY); + + // Chip ID of the sensor is read + uint8_t chipId = 0; + result = this->getRegisters(REGISTER_CHIP_ID, &chipId, 1); + if (result != SUCCESS) return result; + this->chipId = chipId; + + // Check the chip id + if (this->chipId != CHIP_ID) return ERR_DEVICE_NOT_FOUND; + + // Download OTP memory + result = this->otpDumpAfterBoot(); + if (result != SUCCESS) return result; + + // Power off OTP + command = OTP_CMD_PWR_OFF_OTP; + result = this->setRegisters(REGISTER_OTP_CMD_REG, &command, 1); + if (result != SUCCESS) return result; + + result = this->magneticResetAndWait(); + if (result != SUCCESS) return result; + + return SUCCESS; +} + +/** + * @brief Writes data to the given register address of BMM350 sensor. + */ +int8_t BMM350::setRegisters(uint8_t registerAddress, const uint8_t *data, uint16_t length) { + // Check that the data is not null + if (data == nullptr) return ERR_NULL_PTR; + + // Write the data to the register + this->interfaceResult = this->write(registerAddress, data, length); + this->delayMicroseconds(2); + + // Check that the write was successful + if (this->interfaceResult != SUCCESS_INTERFACE_RESULT) return ERR_INTERFACE_RESULT; + + return SUCCESS_INTERFACE_RESULT; +} + +/** + * @brief Reads the data from the given register address of BMM350 sensor. + */ +int8_t BMM350::getRegisters(uint8_t registerAddress, uint8_t *data, uint16_t length) { + if (data == nullptr) return ERR_NULL_PTR; + + uint8_t tempBuffer[READ_BUFFER_LENGTH]; + + // Read the data from the register + this->interfaceResult = this->read(registerAddress, tempBuffer, length + DUMMY_BYTE); + this->delayMicroseconds(2); + + // Check that the read was successful + if (this->interfaceResult != SUCCESS_INTERFACE_RESULT) return ERR_INTERFACE_RESULT; + + // Copy the data to the data buffer + uint16_t index = 0; + while (index < length) { + data[index] = tempBuffer[index + DUMMY_BYTE]; + index++; + } + + return SUCCESS_INTERFACE_RESULT; +} + +/** + * @brief Used to read OTP data after boot in user mode. + */ +int8_t BMM350::otpDumpAfterBoot() { + int8_t result; + + uint8_t index; + uint16_t otpWord = 0; + for (index = 0; index < OTP_DATA_LENGTH; index++) { + result = readOtpWord(index, &otpWord); + this->otpData[index] = otpWord; + } + + this->variantId = (this->otpData[30] & 0x7f00) >> 9; + + // Update magnetometer offset and sensitivity data. + this->updateMagOffSens(); + + return result; +} + +/** + * @brief Used to read OTP word + */ +int8_t BMM350::readOtpWord(uint8_t address, uint16_t *lsbMsb) { + if (lsbMsb == nullptr) return ERR_NULL_PTR; + + int8_t result; + + /* Set OTP command at specified address */ + uint8_t otpCommand = OTP_CMD_DIR_READ | (address & OTP_WORD_ADDR_MASK); + result = this->setRegisters(REGISTER_OTP_CMD_REG, &otpCommand, 1); + if (result != SUCCESS) return result; + + uint8_t otpStatus = 0; + uint8_t otpError = OTP_STATUS_NO_ERROR; + do { + this->delayMicroseconds(300); + + // Get OTP status + result = this->getRegisters(REGISTER_OTP_STATUS_REG, &otpStatus, 1); + otpError = otpStatus & OTP_STATUS_ERROR_MASK; + if (otpError != OTP_STATUS_NO_ERROR) { + break; + } + } while (!(otpStatus & OTP_STATUS_CMD_DONE) && result == SUCCESS); + + if (otpError != OTP_STATUS_NO_ERROR) { + switch (otpError) { + case OTP_STATUS_BOOT_ERR: + result = ERR_OTP_BOOT; + break; + case OTP_STATUS_PAGE_RD_ERR: + result = ERR_OTP_PAGE_RD; + break; + case OTP_STATUS_PAGE_PRG_ERR: + result =ERR_OTP_PAGE_PRG; + break; + case OTP_STATUS_SIGN_ERR: + result = ERR_OTP_SIGN; + break; + case OTP_STATUS_INV_CMD_ERR: + result = ERR_OTP_INV_CMD; + break; + default: + result = ERR_OTP_UNDEFINED; + break; + } + } + + + if (result == SUCCESS) { + // Get OTP MSB data + uint8_t msb = 0; + result = this->getRegisters(REGISTER_OTP_DATA_MSB_REG, &msb, 1); + if (result != SUCCESS) return result; + + /* Get OTP LSB data */ + uint8_t lsb = 0; + result = this->getRegisters(REGISTER_OTP_DATA_LSB_REG, &lsb, 1); + *lsbMsb = ((uint16_t)(msb << 8) | lsb) & 0xFFFF; + } + + return result; +} + +/** + * @brief Used to update magnetometer offset and sensitivity data. + */ +void BMM350::updateMagOffSens() { + uint16_t offXLsbMsb, offYLsbMsb, offZLsbMsb, tOff; + uint8_t sensX, sensY, sensZ, tSens; + uint8_t tcoX, tcoY, tcoZ; + uint8_t tcsX, tcsY, tcsZ; + uint8_t crossXY, crossYX, crossZX, crossZY; + + offXLsbMsb = this->otpData[MAG_OFFSET_X] & 0x0FFF; + offYLsbMsb = ((this->otpData[MAG_OFFSET_X] & 0xF000) >> 4) + (this->otpData[MAG_OFFSET_Y] & LSB_MASK); + offZLsbMsb = (this->otpData[MAG_OFFSET_Y] & 0x0F00) + (this->otpData[MAG_OFFSET_Z] & LSB_MASK); + tOff = this->otpData[TEMP_OFF_SENS] & LSB_MASK; + + this->magComp.dutOffsetCoef.offsetX = BMM350::fixSign(offXLsbMsb, SIGNED_12_BIT); + this->magComp.dutOffsetCoef.offsetY = BMM350::fixSign(offYLsbMsb, SIGNED_12_BIT); + this->magComp.dutOffsetCoef.offsetZ = BMM350::fixSign(offZLsbMsb, SIGNED_12_BIT); + this->magComp.dutOffsetCoef.tOffs = BMM350::fixSign(tOff, SIGNED_8_BIT) / 5.0f; + + sensX = (this->otpData[MAG_SENS_X] & MSB_MASK) >> 8; + sensY = (this->otpData[MAG_SENS_Y] & LSB_MASK); + sensZ = (this->otpData[MAG_SENS_Z] & MSB_MASK) >> 8; + tSens = (this->otpData[TEMP_OFF_SENS] & MSB_MASK) >> 8; + + this->magComp.dutSensitCoef.sensX = BMM350::fixSign(sensX, SIGNED_8_BIT) / 256.0f; + this->magComp.dutSensitCoef.sensY = (BMM350::fixSign(sensY, SIGNED_8_BIT) / 256.0f) + SENS_CORR_Y; + this->magComp.dutSensitCoef.sensZ = BMM350::fixSign(sensZ, SIGNED_8_BIT) / 256.0f; + this->magComp.dutSensitCoef.tSens = BMM350::fixSign(tSens, SIGNED_8_BIT) / 512.0f; + + tcoX = (this->otpData[MAG_TCO_X] & LSB_MASK); + tcoY = (this->otpData[MAG_TCO_Y] & LSB_MASK); + tcoZ = (this->otpData[MAG_TCO_Z] & LSB_MASK); + + this->magComp.dutTco.tcoX = BMM350::fixSign(tcoX, SIGNED_8_BIT) / 32.0f; + this->magComp.dutTco.tcoY = BMM350::fixSign(tcoY, SIGNED_8_BIT) / 32.0f; + this->magComp.dutTco.tcoZ = BMM350::fixSign(tcoZ, SIGNED_8_BIT) / 32.0f; + + tcsX = (this->otpData[MAG_TCS_X] & MSB_MASK) >> 8; + tcsY = (this->otpData[MAG_TCS_Y] & MSB_MASK) >> 8; + tcsZ = (this->otpData[MAG_TCS_Z] & MSB_MASK) >> 8; + + this->magComp.dutTcs.tcsX = BMM350::fixSign(tcsX, SIGNED_8_BIT) / 16384.0f; + this->magComp.dutTcs.tcsY = BMM350::fixSign(tcsY, SIGNED_8_BIT) / 16384.0f; + this->magComp.dutTcs.tcsZ = (BMM350::fixSign(tcsZ, SIGNED_8_BIT) / 16384.0f) - TCS_CORR_Z; + + this->magComp.dutT0 = (BMM350::fixSign(this->otpData[MAG_DUT_T_0], SIGNED_16_BIT) / 512.0f) + 23.0f; + + crossXY = (this->otpData[CROSS_X_Y] & LSB_MASK); + crossYX = (this->otpData[CROSS_Y_X] & MSB_MASK) >> 8; + crossZX = (this->otpData[CROSS_Z_X] & LSB_MASK); + crossZY = (this->otpData[CROSS_Z_Y] & MSB_MASK) >> 8; + + this->magComp.crossAxis.crossXY = BMM350::fixSign(crossXY, SIGNED_8_BIT) / 800.0f; + this->magComp.crossAxis.crossYX = BMM350::fixSign(crossYX, SIGNED_8_BIT) / 800.0f; + this->magComp.crossAxis.crossZX = BMM350::fixSign(crossZX, SIGNED_8_BIT) / 800.0f; + this->magComp.crossAxis.crossZY = BMM350::fixSign(crossZY, SIGNED_8_BIT) / 800.0f; +} + +/** + * @brief Converts the raw data from the IC data registers to signed integer + */ +int32_t BMM350::fixSign(uint32_t inval, int8_t bitsCount) { + int32_t power = 0; + int32_t retval; + + switch (bitsCount) { + case SIGNED_8_BIT: + power = 128; /* 2^7 */ + break; + case SIGNED_12_BIT: + power = 2048; /* 2^11 */ + break; + case SIGNED_16_BIT: + power = 32768; /* 2^15 */ + break; + case SIGNED_21_BIT: + power = 1048576; /* 2^20 */ + break; + case SIGNED_24_BIT: + power = 8388608; /* 2^23 */ + break; + default: + power = 0; + break; + } + + retval = (int32_t)inval; + if (retval >= power) { + retval = retval - (power * 2); + } + + return retval; +} + +/** + * @brief Used to perform the magnetic reset of the sensor + * which is necessary after a field shock (400mT field applied to sensor). + * It sends flux guide or bit reset to the device in suspend mode. + */ +int8_t BMM350::magneticResetAndWait() { + int8_t result; + + uint8_t pmuCmd = 0; + PmuCmdStatus0 pmuCmdStat0 = { 0 }; + uint8_t restoreNormal = DISABLE; + + result = this->getPmuCmdStatus0(&pmuCmdStat0); + if (result != SUCCESS) return result; + + // Check the power mode is normal before performing magnetic reset + if (pmuCmdStat0.pwrModeIsNormal == ENABLE) { + restoreNormal = ENABLE; + + // Reset can only be triggered in suspend + result = this->setPowermode(SUSPEND_MODE); + if (result != SUCCESS) return result; + } + + // Set BR to PMU_CMD register + pmuCmd = PMU_CMD_BR; + + result = this->setRegisters(REGISTER_PMU_CMD, &pmuCmd, 1); + if (result != SUCCESS) return result; + this->delayMicroseconds(BR_DELAY); + + // Verify if PMU_CMD_STATUS_0 register has BR set + result = this->getPmuCmdStatus0(&pmuCmdStat0); + if (result != SUCCESS) return result; + if (pmuCmdStat0.pmuCmdValue != PMU_CMD_STATUS_0_BR) return ERR_PMU_CMD_VALUE; + + // Set FGR to PMU_CMD register + pmuCmd = PMU_CMD_FGR; + + result = this->setRegisters(REGISTER_PMU_CMD, &pmuCmd, 1); + if (result != SUCCESS) return result; + this->delayMicroseconds(FGR_DELAY); + + // Verify if PMU_CMD_STATUS_0 register has FGR set + result = this->getPmuCmdStatus0(&pmuCmdStat0); + if (result != SUCCESS) return result; + + if (pmuCmdStat0.pmuCmdValue != PMU_CMD_STATUS_0_FGR) { + return ERR_PMU_CMD_VALUE; + } + + if (restoreNormal == ENABLE) { + result = this->setPowermode(NORMAL_MODE); + } + + return result; +} + +/** + * @brief Gets the PMU command status 0 value + */ +int8_t BMM350::getPmuCmdStatus0(struct PmuCmdStatus0 *pmuCmdStat0) { + if (pmuCmdStat0 == nullptr) return ERR_NULL_PTR; + + int8_t result; + + // Get PMU command status 0 data + uint8_t regData; + result = this->getRegisters(REGISTER_PMU_CMD_STATUS_0, ®Data, 1); + if (result != SUCCESS) return result; + + pmuCmdStat0->pmuCmdBusy = this->getBitPos0(regData, PMU_CMD_BUSY_MASK); + pmuCmdStat0->odrOvwr = this->getBits(regData, ODR_OVWR_MASK, ODR_OVWR_POS); + pmuCmdStat0->avrOvwr = this->getBits(regData, AVG_OVWR_MASK, AVG_OVWR_POS); + pmuCmdStat0->pwrModeIsNormal = this->getBits(regData, PWR_MODE_IS_NORMAL_MASK, PWR_MODE_IS_NORMAL_POS); + pmuCmdStat0->cmdIsIllegal = this->getBits(regData, CMD_IS_ILLEGAL_MASK, CMD_IS_ILLEGAL_POS); + pmuCmdStat0->pmuCmdValue = this->getBits(regData, PMU_CMD_VALUE_MASK, PMU_CMD_VALUE_POS); + + return SUCCESS; +} + +uint16_t BMM350::getBitPos0(uint16_t registerData, uint16_t bitMask) { + return registerData & bitMask; +} + +uint16_t BMM350::getBits(uint16_t registerData, uint16_t bitMask, uint16_t bitPos) { + return (registerData & bitMask) >> bitPos; +} + +uint16_t BMM350::setBits(uint16_t registerData, uint16_t bitMask, uint16_t bitPos, uint16_t data) { + return (registerData & ~(bitMask)) | ((data << bitPos) & bitMask); +} + +/** + * @brief This API is used to set the power mode of the sensor + */ +int8_t BMM350::setPowermode(enum Powermode powermode) { + + int8_t result; + + uint8_t last_pwr_mode; + uint8_t reg_data; + + + result = this->getRegisters(REGISTER_PMU_CMD, &last_pwr_mode, 1); + if (result != SUCCESS) return result; + + if (last_pwr_mode > PMU_CMD_NM_TC) return ERR_INVALID_CONFIG; + + if (last_pwr_mode == PMU_CMD_NM || last_pwr_mode == PMU_CMD_UPD_OAE) { + reg_data = PMU_CMD_SUS; + + // Set PMU command configuration + result = this->setRegisters(REGISTER_PMU_CMD, ®_data, 1); + if (result != SUCCESS) return result; + + this->delayMicroseconds(GOTO_SUSPEND_DELAY); + } + + result = this->switchPowermode(powermode); + if (result != SUCCESS) return result; + + return SUCCESS; +} + +/** + * @brief This internal API is used to switch from suspend mode to normal mode or forced mode. + */ +int8_t BMM350::switchPowermode(enum Powermode powermode) { + int8_t result; + + uint8_t regData = static_cast(powermode); + uint8_t getAvg; + + // Array to store suspend to forced mode delay + uint32_t susToForcedMode[4] = { + SUS_TO_FORCED_MODE_NO_AVG_DELAY, + SUS_TO_FORCED_MODE_AVG_2_DELAY, + SUS_TO_FORCED_MODE_AVG_4_DELAY, + SUS_TO_FORCED_MODE_AVG_8_DELAY + }; + + // Array to store suspend to forced mode fast delay + uint32_t susToForcedModeFast[4] = { + SUS_TO_FORCED_MODE_FAST_NO_AVG_DELAY, + SUS_TO_FORCED_MODE_FAST_AVG_2_DELAY, + SUS_TO_FORCED_MODE_FAST_AVG_4_DELAY, + SUS_TO_FORCED_MODE_FAST_AVG_8_DELAY + }; + + uint8_t avg = 0; + uint32_t delayUs = 0; + + // Set PMU command configuration to desired power mode + result = this->setRegisters(REGISTER_PMU_CMD, ®Data, 1); + if (result != SUCCESS) return result; + + + // Get average configuration + result = this->getRegisters(REGISTER_PMU_CMD_AGGR_SET, &getAvg, 1); + if (result != SUCCESS) return result; + + // Mask the average value + avg = ((getAvg & AVG_MASK) >> AVG_POS); + + // Check if desired power mode is normal mode + if (powermode == NORMAL_MODE) { + delayUs = SUSPEND_TO_NORMAL_DELAY; + } + + // Check if desired power mode is forced mode + if (powermode == FORCED_MODE) { + // Store delay based on averaging mode + delayUs = susToForcedMode[avg]; + } + + // Check if desired power mode is forced mode fast + if (powermode == FORCED_MODE_FAST) { + // Store delay based on averaging mode + delayUs = susToForcedModeFast[avg]; + } + + // Perform delay based on power mode + this->delayMicroseconds(delayUs); + + return result; +} + +/** + * @brief This API sets the ODR and averaging factor. + */ +int8_t BMM350::seOdrAvg(uint8_t odr, uint8_t performance) { + int8_t result; + + uint8_t performance_fix = performance; + + // Reduce the performance setting when too high for the chosen ODR + if (odr == ODR_400HZ && performance >= AVG_2) { + performance_fix = AVG_NO_AVG; + } + else if (odr == ODR_200HZ && performance >= AVG_4) { + performance_fix = AVG_2; + } + else if (odr == ODR_100HZ && performance >= AVG_8) { + performance_fix = AVG_4; + } + + // ODR is an enum taking the generated constants from the register map + uint8_t regData = 0; + regData = ((uint8_t)odr & ODR_MSK); + + // AVG / performance is an enum taking the generated constants from the register map + regData = BMM350::setBits(regData, AVG_MASK, AVG_POS, (uint8_t)performance_fix); + + // Set PMU command configurations for ODR and performance + result = this->setRegisters(REGISTER_PMU_CMD_AGGR_SET, ®Data, 1); + if (result != SUCCESS) return result; + + // Set PMU command configurations to update odr and average + regData = PMU_CMD_UPD_OAE; + + // Set PMU command configuration + result = this->setRegisters(REGISTER_PMU_CMD, ®Data, 1); + if (result != SUCCESS) return result; + + this->delayMicroseconds(UPD_OAE_DELAY); + + return result; +} + +/*! + * @brief Used to enable or disable the data ready interrupt + */ +int8_t BMM350::setInterruptEnabled(uint8_t enableDisable) { + int8_t result; + + // Get interrupt control configuration + uint8_t registerData = 0; + result = this->setRegisters(REGISTER_INT_CTRL, ®isterData, 1); + if (result != SUCCESS) return result; + + registerData = BMM350::setBits(registerData, DRDY_DATA_REG_EN_MASK, DRDY_DATA_REG_EN_POS, (uint8_t)enableDisable); + + // Finally transfer the interrupt configurations + result = this->setRegisters(REGISTER_INT_CTRL, ®isterData, 1); + if (result != SUCCESS) return result; + + return SUCCESS; +} + +/*! + * @brief This API is used to perform compensation for raw magnetometer and temperature data. + */ +int8_t BMM350::getCompensatedMagData(float *magData) { + if (magData == nullptr) return ERR_NULL_PTR; + + int8_t result; + + float outData[4] = { 0.0f }; + float dutOffsetCoef[3], dutSensitCoef[3], dutTco[3], dutTcs[3]; + float crAxCompX, crAxCompY, crAxCompZ; + + /* Reads raw magnetic x,y and z axis along with temperature */ + result = this->readOutRawData(outData); + if (result != SUCCESS) return result; + + /* Apply compensation to temperature reading */ + outData[3] = (1 + this->magComp.dutSensitCoef.tSens) * outData[3] + + this->magComp.dutOffsetCoef.tOffs; + + /* Store magnetic compensation structure to an array */ + dutOffsetCoef[0] = this->magComp.dutOffsetCoef.offsetX; + dutOffsetCoef[1] = this->magComp.dutOffsetCoef.offsetY; + dutOffsetCoef[2] = this->magComp.dutOffsetCoef.offsetZ; + + dutSensitCoef[0] = this->magComp.dutSensitCoef.sensX; + dutSensitCoef[1] = this->magComp.dutSensitCoef.sensY; + dutSensitCoef[2] = this->magComp.dutSensitCoef.sensZ; + + dutTco[0] = this->magComp.dutTco.tcoX; + dutTco[1] = this->magComp.dutTco.tcoY; + dutTco[2] = this->magComp.dutTco.tcoZ; + + dutTcs[0] = this->magComp.dutTcs.tcsX; + dutTcs[1] = this->magComp.dutTcs.tcsY; + dutTcs[2] = this->magComp.dutTcs.tcsZ; + + /* Compensate raw magnetic data */ + for (uint8_t index = 0; index < 3; index++) { + outData[index] *= 1 + dutSensitCoef[index]; + outData[index] += dutOffsetCoef[index]; + outData[index] += dutTco[index] * (outData[3] - this->magComp.dutT0); + outData[index] /= 1 + dutTcs[index] * (outData[3] - this->magComp.dutT0); + } + + crAxCompX = (outData[0] - this->magComp.crossAxis.crossXY * outData[1]) / + (1 - this->magComp.crossAxis.crossYX * this->magComp.crossAxis.crossXY); + crAxCompY = (outData[1] - this->magComp.crossAxis.crossYX * outData[0]) / + (1 - this->magComp.crossAxis.crossYX * this->magComp.crossAxis.crossXY); + crAxCompZ = + (outData[2] + + (outData[0] * + (this->magComp.crossAxis.crossYX * this->magComp.crossAxis.crossZY - + this->magComp.crossAxis.crossZX) - outData[1] * + (this->magComp.crossAxis.crossZY - this->magComp.crossAxis.crossYX * + this->magComp.crossAxis.crossZX)) / + (1 - this->magComp.crossAxis.crossYX * this->magComp.crossAxis.crossXY)); + + outData[0] = crAxCompX; + outData[1] = crAxCompY; + outData[2] = crAxCompZ; + + if ((this->enabledAxes & EN_X_MASK) == DISABLE) { + magData[0] = DISABLE; + } else { + magData[0] = outData[0]; + } + + if ((this->enabledAxes & EN_Y_MASK) == DISABLE) { + magData[1] = DISABLE; + } else { + magData[1] = outData[1]; + } + + if ((this->enabledAxes & EN_Z_MASK) == DISABLE) { + magData[2] = DISABLE; + } else { + magData[2] = outData[2]; + } + + magData[3] = outData[3]; + + return result; +} + + +/*! + * @brief This internal API is used to read raw magnetic x,y and z axis along with temperature + */ +int8_t BMM350::readOutRawData(float *outData) { + if (outData == nullptr) return ERR_NULL_PTR; + + int8_t result; + + float rawData[4] = { 0 }; + result = this->readUncompMagData(rawData); + if (result != SUCCESS) return result; + + outData[0] = (float)rawData[0] * 0.0070699787f; + outData[1] = (float)rawData[1] * 0.0070699787f; + outData[2] = (float)rawData[2] * 0.007174964082f; + outData[3] = (float)rawData[3] * 0.000981281852f; + + float temp = 0.0; + if (outData[3] > 0.0) { + temp = (float)(outData[3] - (1 * 25.49)); + } else if (outData[3] < 0.0) { + temp = (float)(outData[3] - (-1 * 25.49)); + } else { + temp = (float)(outData[3]); + } + + outData[3] = temp; + + return SUCCESS; +} + +/*! + * @brief This API is used to read uncompensated mag and temperature data. + */ +int8_t BMM350::readUncompMagData(float *rawData) { + if (rawData == nullptr) return ERR_NULL_PTR; + + int8_t result; + + uint8_t magData[12] = { 0 }; + + uint32_t rawMagX, rawMagY, rawMagZ, rawTemp; + + // Get uncompensated mag data + result = this->getRegisters(REGISTER_MAG_X_XLSB, magData, MAG_TEMP_DATA_LEN); + if (result != SUCCESS) return result; + + rawMagX = magData[0] + ((uint32_t)magData[1] << 8) + ((uint32_t)magData[2] << 16); + rawMagY = magData[3] + ((uint32_t)magData[4] << 8) + ((uint32_t)magData[5] << 16); + rawMagZ = magData[6] + ((uint32_t)magData[7] << 8) + ((uint32_t)magData[8] << 16); + rawTemp = magData[9] + ((uint32_t)magData[10] << 8) + ((uint32_t)magData[11] << 16); + + if ((this->enabledAxes & EN_X_MASK) == DISABLE) { + rawData[0] = DISABLE; + } else { + rawData[0] = BMM350::fixSign(rawMagX, SIGNED_24_BIT); + } + + if ((this->enabledAxes & EN_Y_MASK) == DISABLE) { + rawData[1] = DISABLE; + } else { + rawData[1] = BMM350::fixSign(rawMagY, SIGNED_24_BIT); + } + + if ((this->enabledAxes & EN_Z_MASK) == DISABLE) { + rawData[2] = DISABLE; + } else { + rawData[2] = BMM350::fixSign(rawMagZ, SIGNED_24_BIT); + } + + rawData[3] = BMM350::fixSign(rawTemp, SIGNED_24_BIT); + + return SUCCESS; +} \ No newline at end of file diff --git a/lib/bmm350/bmm350.h b/lib/bmm350/bmm350.h new file mode 100644 index 000000000..0107a6a47 --- /dev/null +++ b/lib/bmm350/bmm350.h @@ -0,0 +1,442 @@ +/* + SlimeVR Code is placed under the MIT license + Copyright (c) 2024 jojos38 + + 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 BMM350_H +#define BMM350_H + +#include + +typedef int8_t (*ReadFunction)(uint8_t, uint8_t*, uint32_t, void*); +typedef int8_t (*WriteFunction)(uint8_t, const uint8_t*, uint32_t, void *); +typedef void (*DelayFunction)(uint32_t, void *); + +class BMM350 { + + public: + /****************************************************************************************** + * PUBLIC STRUCTS + ******************************************************************************************/ + /** + * @brief Magnetometer dut offset coefficient structure + */ + struct DutOffsetCoef { + // Temperature offset + float tOffs; + + // Offset x-axis + float offsetX; + + // Offset y-axis + float offsetY; + + // Offset z-axis + float offsetZ; + }; + + /** + * @brief Magnetometer dut sensitivity coefficient structure + */ + struct DutSensitCoef { + // Temperature sensitivity + float tSens; + + // Sensitivity x-axis + float sensX; + + // Sensitivity y-axis + float sensY; + + // Sensitivity z-axis + float sensZ; + }; + + /** + * @brief Magnetometer dut tco structure + */ + struct DutTco { + float tcoX; + float tcoY; + float tcoZ; + }; + + /** + * @brief Magnetometer dut tcs structure + */ + struct DutTcs { + float tcsX; + float tcsY; + float tcsZ; + }; + + /** + * @brief Magnetometer cross-axis compensation structure + */ + struct CrossAxis { + float crossXY; + float crossYX; + float crossZX; + float crossZY; + }; + + /** + * @brief Magnetometer compensation structure + */ + struct MagCompensate { + // Structure to store dut offset coefficient + DutOffsetCoef dutOffsetCoef; + + // Structure to store dut sensitivity coefficient + DutSensitCoef dutSensitCoef; + + // Structure to store dut tco + DutTco dutTco; + + // Structure to store dut tcs + DutTcs dutTcs; + + // Initialize T0_reading parameter + float dutT0; + + // Structure to define cross-axis compensation + CrossAxis crossAxis; + }; + + /****************************************************************************************** + * PUBLIC ENUMS + ******************************************************************************************/ + enum Powermode { + SUSPEND_MODE = 0x00, // Replace with the actual values + NORMAL_MODE = 0x01, + FORCED_MODE = 0x03, + FORCED_MODE_FAST = 0x04 + }; + + /****************************************************************************************** + * PUBLIC CONSTANTS + ******************************************************************************************/ + // General + static const uint8_t CHIP_ID = UINT8_C(0x33); + static const uint8_t ENABLE = UINT8_C(1); + static const uint8_t DISABLE = UINT8_C(0); + static const uint8_t ADDRESS_I2C_PRIMARY = UINT8_C(0x14); + static const uint8_t ADDRESS_I2C_SECONDARY = UINT8_C(0x15); + + // Success + static const int8_t SUCCESS = INT8_C(0); + static const int8_t SUCCESS_INTERFACE_RESULT = INT8_C(0); + + // Errors + static const int8_t ERR_NULL_PTR = INT8_C(-1); + static const int8_t ERR_INTERFACE_RESULT = INT8_C(-2); + static const int8_t ERR_DEVICE_NOT_FOUND = INT8_C(-3); + static const int8_t ERR_INVALID_CONFIG = INT8_C(-4); + static const int8_t ERR_PMU_CMD_VALUE = INT8_C(-16); + static const int8_t ERR_OTP_BOOT = INT8_C(-9); + static const int8_t ERR_OTP_PAGE_RD = INT8_C(-10); + static const int8_t ERR_OTP_PAGE_PRG = INT8_C(-11); + static const int8_t ERR_OTP_SIGN = INT8_C(-12); + static const int8_t ERR_OTP_INV_CMD = INT8_C(-13); + static const int8_t ERR_OTP_UNDEFINED = INT8_C(-14); + + /** + * Choose the number of values the magnometer should average for the last reading. + * Averaging can help reduce noise and provide a smoother magnometer output. + * AVG_NO_AVG: No averaging (raw values) + * AVG_X: Averae over X values + */ + static const uint8_t AVG_NO_AVG = UINT8_C(0x0); + static const uint8_t AVG_2 = UINT8_C(0x1); + static const uint8_t AVG_4 = UINT8_C(0x2); + static const uint8_t AVG_8 = UINT8_C(0x3); + + /** + * Magnometer data output rate in Hertz + * Numbers with underscores should be interpreted as comma + * ODR_1_5625HZ: 1.5625Hz + * ODR_100HZ: 100Hz + * etc... + */ + static const uint8_t ODR_400HZ = UINT8_C(0x2); + static const uint8_t ODR_200HZ = UINT8_C(0x3); + static const uint8_t ODR_100HZ = UINT8_C(0x4); + static const uint8_t ODR_50HZ = UINT8_C(0x5); + static const uint8_t ODR_25HZ = UINT8_C(0x6); + static const uint8_t ODR_12_5HZ = UINT8_C(0x7); + static const uint8_t ODR_6_25HZ = UINT8_C(0x8); + static const uint8_t ODR_3_125HZ = UINT8_C(0x9); + static const uint8_t ODR_1_5625HZ = UINT8_C(0xA); + + // I²2 + static const uint8_t READ_BUFFER_LENGTH = UINT8_C(127); + static const uint8_t DUMMY_BYTE = UINT8_C(2); + static const uint8_t OTP_DATA_LENGTH = UINT8_C(32); + + /****************************************************************************************** + * CONSTRUCTOR FUNCTIONS + ******************************************************************************************/ + BMM350(ReadFunction readFunc, WriteFunction writeFunc, DelayFunction delayFunc, void* interfacePtr) : + readFunction(readFunc), + writeFunction(writeFunc), + delayFunction(delayFunc), + interfacePointer(interfacePtr) + {}; + ~BMM350() {}; + + int8_t read(uint8_t registerAddress, uint8_t* regData, uint32_t length) { + return readFunction(registerAddress, regData, length, this->interfacePointer); + } + + int8_t write(uint8_t registerAddress, const uint8_t* regData, uint32_t length) { + return writeFunction(registerAddress, regData, length, this->interfacePointer); + } + + void delayMicroseconds(uint32_t period) { + delayFunction(period, this->interfacePointer); + } + + /****************************************************************************************** + * PUBLIC FUNCTIONS + ******************************************************************************************/ + int8_t initI2C(); + int8_t magneticResetAndWait(); + int8_t setPowermode(enum Powermode powermode); + int8_t seOdrAvg(uint8_t odr, uint8_t avg); + int8_t setInterruptEnabled(uint8_t enableDisable); + int8_t getCompensatedMagData(float *magData); + + /****************************************************************************************** + * PUBLIC VARIABLES + ******************************************************************************************/ + // The chip id of the sensor + int8_t chipId; + // Variant id + uint8_t variantId; + // The result of the last I²C read or write operation (the return value of the user's function) + int8_t interfaceResult; + // The enabled axes + uint8_t enabledAxes; + // OTP Data + uint16_t otpData[OTP_DATA_LENGTH]; + // Compensation parameters + MagCompensate magComp; + + private: + /****************************************************************************************** + * PUBLIC STRUCTS + ******************************************************************************************/ + /** + * @brief BMM350 PMU command status 0 structure + */ + struct PmuCmdStatus0 { + // The previous PMU CMD is still in processing + uint8_t pmuCmdBusy; + + // The previous PMU_CMD_AGGR_SET.odr has been overwritten + uint8_t odrOvwr; + + // The previous PMU_CMD_AGGR_SET.avg has been overwritten + uint8_t avrOvwr; + + // The chip is in normal power mode + uint8_t pwrModeIsNormal; + + // CMD value is not allowed + uint8_t cmdIsIllegal; + + // Stores the latest PMU_CMD code processed + uint8_t pmuCmdValue; + }; + + /****************************************************************************************** + * PRIVATE CONSTANTS + ******************************************************************************************/ + // Commands + static const uint8_t OTP_CMD_PWR_OFF_OTP = UINT8_C(0x80); + static const uint8_t COMMAND_SOFTRESET = UINT8_C(0xB6); + + // Data length + static const uint8_t MAG_TEMP_DATA_LEN = UINT8_C(12); + + // Registers + static const uint8_t REGISTER_CHIP_ID = UINT8_C(0x00); + static const uint8_t REGISTER_REV_ID = UINT8_C(0x01); + static const uint8_t REGISTER_OTP_CMD_REG = UINT8_C(0x50); + static const uint8_t REGISTER_OTP_STATUS_REG = UINT8_C(0x55); + static const uint8_t REGISTER_CMD = UINT8_C(0x7E); + static const uint8_t REGISTER_OTP_DATA_MSB_REG = UINT8_C(0x52); + static const uint8_t REGISTER_OTP_DATA_LSB_REG = UINT8_C(0x53); + static const uint8_t REGISTER_PMU_CMD_STATUS_0 = UINT8_C(0x07); + static const uint8_t REGISTER_PMU_CMD = UINT8_C(0x06); + static const uint8_t REGISTER_PMU_CMD_AGGR_SET = UINT8_C(0x04); + static const uint8_t REGISTER_INT_CTRL = UINT8_C(0x2E); + static const uint8_t REGISTER_MAG_X_XLSB = UINT8_C(0x31); + + // Delays + static const uint32_t SOFT_RESET_DELAY = UINT32_C(24000); + static const uint32_t MAGNETIC_RESET_DELAY = UINT32_C(40000); + static const uint32_t START_UP_TIME_FROM_POR = UINT32_C(3000); + static const uint32_t GOTO_SUSPEND_DELAY = UINT32_C(6000); + static const uint32_t SUSPEND_TO_NORMAL_DELAY = UINT32_C(38000); + static const uint32_t SUS_TO_FORCED_MODE_NO_AVG_DELAY = UINT32_C(15000); + static const uint32_t SUS_TO_FORCED_MODE_AVG_2_DELAY = UINT32_C(17000); + static const uint32_t SUS_TO_FORCED_MODE_AVG_4_DELAY = UINT32_C(20000); + static const uint32_t SUS_TO_FORCED_MODE_AVG_8_DELAY = UINT32_C(28000); + static const uint32_t SUS_TO_FORCED_MODE_FAST_NO_AVG_DELAY = UINT32_C(4000); + static const uint32_t SUS_TO_FORCED_MODE_FAST_AVG_2_DELAY = UINT32_C(5000); + static const uint32_t SUS_TO_FORCED_MODE_FAST_AVG_4_DELAY = UINT32_C(9000); + static const uint32_t SUS_TO_FORCED_MODE_FAST_AVG_8_DELAY = UINT32_C(16000); + static const uint16_t BR_DELAY = UINT16_C(14000); + static const uint16_t FGR_DELAY = UINT16_C(18000); + static const uint16_t UPD_OAE_DELAY = UINT16_C(1000); + + // OTP + static const uint8_t OTP_CMD_DIR_READ = UINT8_C(0x20); + static const uint8_t OTP_WORD_ADDR_MASK = UINT8_C(0x1F); + static const uint8_t OTP_STATUS_ERROR_MASK = UINT8_C(0xE0); + + static const uint8_t OTP_STATUS_NO_ERROR = UINT8_C(0x00); + static const uint8_t OTP_STATUS_BOOT_ERR = UINT8_C(0x20); + static const uint8_t OTP_STATUS_PAGE_RD_ERR = UINT8_C(0x40); + static const uint8_t OTP_STATUS_PAGE_PRG_ERR = UINT8_C(0x60); + static const uint8_t OTP_STATUS_SIGN_ERR = UINT8_C(0x80); + static const uint8_t OTP_STATUS_INV_CMD_ERR = UINT8_C(0xA0); + static const uint8_t OTP_STATUS_CMD_DONE = UINT8_C(0x01); + + static const uint8_t PMU_CMD_BUSY_MASK = UINT8_C(0x1); + static const uint8_t PMU_CMD_BUSY_POS = UINT8_C(0x0); + static const uint8_t ODR_OVWR_MASK = UINT8_C(0x2); + static const uint8_t ODR_OVWR_POS = UINT8_C(0x1); + static const uint8_t AVG_OVWR_MASK = UINT8_C(0x4); + static const uint8_t AVG_OVWR_POS = UINT8_C(0x2); + static const uint8_t PWR_MODE_IS_NORMAL_MASK = UINT8_C(0x8); + static const uint8_t PWR_MODE_IS_NORMAL_POS = UINT8_C(0x3); + static const uint8_t CMD_IS_ILLEGAL_MASK = UINT8_C(0x10); + static const uint8_t CMD_IS_ILLEGAL_POS = UINT8_C(0x4); + static const uint8_t PMU_CMD_VALUE_MASK = UINT8_C(0xE0); + static const uint8_t PMU_CMD_VALUE_POS = UINT8_C(0x5); + static const uint8_t DRDY_DATA_REG_EN_MASK = UINT8_C(0x80); + static const uint8_t DRDY_DATA_REG_EN_POS = UINT8_C(0x7); + + // Axes + static const uint8_t EN_XYZ_MASK = UINT8_C(0x7); + static const uint8_t EN_XYZ_POS = UINT8_C(0x0); + static const uint8_t EN_X_MASK = UINT8_C(0x01); + static const uint8_t EN_X_POS = UINT8_C(0x0); + static const uint8_t EN_Y_MASK = UINT8_C(0x02); + static const uint8_t EN_Y_POS = UINT8_C(0x1); + static const uint8_t EN_Z_MASK = UINT8_C(0x04); + static const uint8_t EN_Z_POS = UINT8_C(0x2); + + // OTP indices + static const uint8_t TEMP_OFF_SENS = UINT8_C(0x0D); + + static const uint8_t MAG_OFFSET_X = UINT8_C(0x0E); + static const uint8_t MAG_OFFSET_Y = UINT8_C(0x0F); + static const uint8_t MAG_OFFSET_Z = UINT8_C(0x10); + + static const uint8_t MAG_SENS_X = UINT8_C(0x10); + static const uint8_t MAG_SENS_Y = UINT8_C(0x11); + static const uint8_t MAG_SENS_Z = UINT8_C(0x11); + + static const uint8_t MAG_TCO_X = UINT8_C(0x12); + static const uint8_t MAG_TCO_Y = UINT8_C(0x13); + static const uint8_t MAG_TCO_Z = UINT8_C(0x14); + + static const uint8_t MAG_TCS_X = UINT8_C(0x12); + static const uint8_t MAG_TCS_Y = UINT8_C(0x13); + static const uint8_t MAG_TCS_Z = UINT8_C(0x14); + + static const uint8_t MAG_DUT_T_0 = UINT8_C(0x18); + + static const uint8_t CROSS_X_Y = UINT8_C(0x15); + static const uint8_t CROSS_Y_X = UINT8_C(0x15); + static const uint8_t CROSS_Z_X = UINT8_C(0x16); + static const uint8_t CROSS_Z_Y = UINT8_C(0x16); + + static constexpr float SENS_CORR_Y = 0.01f; + static constexpr float TCS_CORR_Z = 0.0001f; + + // Other + static const uint16_t LSB_MASK = UINT16_C(0x00FF); + static const uint16_t MSB_MASK = UINT16_C(0xFF00); + + static const uint8_t AVG_MASK = UINT8_C(0x30); + static const uint8_t AVG_POS = UINT8_C(0x4); + static const uint8_t ODR_MSK = UINT8_C(0xf); + static const uint8_t ODR_POS = UINT8_C(0x0); + + static const uint8_t SIGNED_8_BIT = UINT8_C(8); + static const uint8_t SIGNED_12_BIT = UINT8_C(12); + static const uint8_t SIGNED_16_BIT = UINT8_C(16); + static const uint8_t SIGNED_21_BIT = UINT8_C(21); + static const uint8_t SIGNED_24_BIT = UINT8_C(24); + + // PMU + static const uint8_t PMU_CMD_SUS = UINT8_C(0x00); + static const uint8_t PMU_CMD_NM = UINT8_C(0x01); + static const uint8_t PMU_CMD_UPD_OAE = UINT8_C(0x02); + static const uint8_t PMU_CMD_FM = UINT8_C(0x03); + static const uint8_t PMU_CMD_FM_FAST = UINT8_C(0x04); + static const uint8_t PMU_CMD_FGR = UINT8_C(0x05); + static const uint8_t PMU_CMD_FGR_FAST = UINT8_C(0x06); + static const uint8_t PMU_CMD_BR = UINT8_C(0x07); + static const uint8_t PMU_CMD_BR_FAST = UINT8_C(0x08); + static const uint8_t PMU_CMD_NM_TC = UINT8_C(0x09); + + static const uint8_t PMU_CMD_STATUS_0_SUS = UINT8_C(0x00); + static const uint8_t PMU_CMD_STATUS_0_NM = UINT8_C(0x01); + static const uint8_t PMU_CMD_STATUS_0_UPD_OAE = UINT8_C(0x02); + static const uint8_t PMU_CMD_STATUS_0_FM = UINT8_C(0x03); + static const uint8_t PMU_CMD_STATUS_0_FM_FAST = UINT8_C(0x04); + static const uint8_t PMU_CMD_STATUS_0_FGR = UINT8_C(0x05); + static const uint8_t PMU_CMD_STATUS_0_FGR_FAST = UINT8_C(0x06); + static const uint8_t PMU_CMD_STATUS_0_BR = UINT8_C(0x07); + static const uint8_t PMU_CMD_STATUS_0_BR_FAST = UINT8_C(0x07); + + /****************************************************************************************** + * PRIVATE FUNCTIONS + ******************************************************************************************/ + static int32_t fixSign(uint32_t inval, int8_t bitsCount); + static uint16_t getBits(uint16_t registerData, uint16_t bitMask, uint16_t bitPos); + static uint16_t getBitPos0(uint16_t registerData, uint16_t bitMask); + static uint16_t setBits(uint16_t registerData, uint16_t bitMask, uint16_t bitPos, uint16_t data); + + int8_t setRegisters(uint8_t registerAddress, const uint8_t *data, uint16_t length); + int8_t getRegisters(uint8_t registerAddress, uint8_t *data, uint16_t length); + int8_t otpDumpAfterBoot(); + int8_t readOtpWord(uint8_t address, uint16_t *lsbMsb); + int8_t getPmuCmdStatus0(struct PmuCmdStatus0 *pmuCmdStat0); + int8_t switchPowermode(enum Powermode PowerMode); + int8_t readOutRawData(float *outData); + int8_t readUncompMagData(float *rawData); + void updateMagOffSens(); + + /****************************************************************************************** + * PRIVATE VARIABLES + ******************************************************************************************/ + ReadFunction readFunction; + WriteFunction writeFunction; + DelayFunction delayFunction; + void *interfacePointer; +}; +#endif \ No newline at end of file diff --git a/src/consts.h b/src/consts.h index a6640879a..47faa6d1e 100644 --- a/src/consts.h +++ b/src/consts.h @@ -42,6 +42,7 @@ enum class ImuID { LSM6DSV, LSM6DSO, LSM6DSR, + BMI323, Empty = 255 }; @@ -61,6 +62,7 @@ enum class ImuID { #define IMU_LSM6DSV SoftFusionLSM6DSV #define IMU_LSM6DSO SoftFusionLSM6DSO #define IMU_LSM6DSR SoftFusionLSM6DSR +#define IMU_BMI323 SoftFusionBMI323 #define IMU_MPU6050_SF SoftFusionMPU6050 #define IMU_DEV_RESERVED 250 // Reserved, should not be used in any release firmware diff --git a/src/defines.h b/src/defines.h index 33296454a..3d6275160 100644 --- a/src/defines.h +++ b/src/defines.h @@ -26,9 +26,9 @@ // ================================================ // Set parameters of IMU and board used -#define IMU IMU_BNO085 -#define SECOND_IMU IMU -#define BOARD BOARD_SLIMEVR +#define IMU IMU_BMI323 +#define SECOND_IMU IMU_BMI323 +#define BOARD BOARD_CUSTOM #define IMU_ROTATION DEG_270 #define SECOND_IMU_ROTATION DEG_270 @@ -149,6 +149,24 @@ IMU_DESC_ENTRY(IMU_BMP160, PRIMARY_IMU_ADDRESS_ONE, IMU_ROTATION, PIN_IMU_SCL, P // #define LED_INVERTED false #elif BOARD == BOARD_CUSTOM // Define pins by the examples above + #define PIN_IMU_SDA 4 + #define PIN_IMU_SCL 5 + #define PIN_IMU_INT 14 + #define PIN_IMU_INT_2 12 + #define PIN_BATTERY_LEVEL A0 + #define LED_PIN 15 + #define LED_INVERTED false + // The Wemos D1 mini has an internal voltage divider that I don't have on my custom board + // Therefor, BATTERY_SHIELD_RESISTANCE is set to 0 and SHIELD_R1 and R2 are adjusted instead + #ifndef BATTERY_SHIELD_RESISTANCE + #define BATTERY_SHIELD_RESISTANCE 0 + #endif + #ifndef BATTERY_SHIELD_R1 + #define BATTERY_SHIELD_R1 36 + #endif + #ifndef BATTERY_SHIELD_R2 + #define BATTERY_SHIELD_R2 123.5 + #endif #elif BOARD == BOARD_WROOM32 #define PIN_IMU_SDA 21 #define PIN_IMU_SCL 22 diff --git a/src/sensors/SensorManager.cpp b/src/sensors/SensorManager.cpp index 1c1dc9cc7..181c36e73 100644 --- a/src/sensors/SensorManager.cpp +++ b/src/sensors/SensorManager.cpp @@ -38,6 +38,7 @@ #include "softfusion/drivers/lsm6dso.h" #include "softfusion/drivers/lsm6dsr.h" #include "softfusion/drivers/mpu6050.h" +#include "softfusion/drivers/bmi323.h" #include "softfusion/i2cimpl.h" @@ -56,6 +57,7 @@ namespace SlimeVR using SoftFusionLSM6DSO = SoftFusionSensor; using SoftFusionLSM6DSR = SoftFusionSensor; using SoftFusionMPU6050 = SoftFusionSensor; + using SoftFusionBMI323 = SoftFusionSensor; // TODO Make it more generic in the future and move another place (abstract sensor interface) void SensorManager::swapI2C(uint8_t sclPin, uint8_t sdaPin) diff --git a/src/sensors/softfusion/drivers/bmi323.h b/src/sensors/softfusion/drivers/bmi323.h new file mode 100644 index 000000000..7c0abe4cc --- /dev/null +++ b/src/sensors/softfusion/drivers/bmi323.h @@ -0,0 +1,279 @@ +/* + SlimeVR Code is placed under the MIT license + Copyright (c) 2024 Tailsy13 & SlimeVR Contributors + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + THE SOFTWARE. +*/ + +#pragma once + +#include +#include +#include +#include +#include "bmi323_lib.h" + +namespace SlimeVR::Sensors::SoftFusion::Drivers +{ + +// Driver uses acceleration range at 16g +// and gyroscope range at 1000dps +// Gyroscope ODR = 400Hz, accel ODR = 100Hz +// Timestamps reading are not used + +template +struct BMI323 +{ + // Sensor Fusion required variables + static constexpr uint8_t Address = 0x68; // i2c address + static constexpr auto Name = "BMI323"; + static constexpr auto Type = ImuID::BMI323; + + static constexpr float GyrTs=1.0/200.0; // Delay between each gyro sample + static constexpr float AccTs=1.0/200.0; // Dealy between each accel sample + static constexpr float MagTs=1.0/25.0; // Delay between each mag sample + + // BMI323 specific constants + static constexpr float GyroSensitivity = 32.768f; + static constexpr float AccelSensitivity = 4096.0f; + static constexpr float GScale = ((32768. / GyroSensitivity) / 32768.) * (PI / 180.0); + static constexpr float AScale = CONST_EARTH_GRAVITY / AccelSensitivity; + + struct MotionlessCalibrationData + { + bool valid; + uint8_t x, y, z; + }; + + I2CImpl i2c; + SlimeVR::Logging::Logger &logger; + int8_t zxFactor; + BMI323(I2CImpl i2c, SlimeVR::Logging::Logger &logger) + : i2c(i2c), logger(logger), zxFactor(0) {} + + struct Regs { + struct WhoAmI { + static constexpr uint8_t reg = 0x00; + static constexpr uint8_t value = 0x68; + }; + }; + + static void delayUs(uint32_t period, const void *) + { + delay(period / 1000); + } + + static int8_t i2cRead(uint8_t registerAddress, uint8_t *registerData, uint32_t length, const void *interfacePointer) + { + return 0; + } + + static int8_t i2cWrite(uint8_t registerAddress, const uint8_t *registerData, uint32_t length, const void *interfacePointer) + { + return 0; + } + + bool restartAndInit() { + + return true; + } + + bool initialize(MotionlessCalibrationData &gyroSensitivity) + { + logger.info("jojos38 BMI323 firmware V1.4"); + + int8_t result; + + BMI323_LIB bmi323Lib(i2cRead, i2cWrite, delayUs, &Address); + + // Initialize the sensor + result = bmi323Lib.initI2C(); + if (result == BMI323_LIB::SUCCESS) { + logger.info("BMI323 Initialized on address 0x"); + } else { + logger.info("BMI323 Initialization failed"); + return false; + } + + // Apply the calibration data + /*if (m_calibration.G_G[0] != 0 || m_calibration.G_G[1] != 0 || m_calibration.G_G[2] != 0 || m_calibration.G_O[0] != 0 || m_calibration.G_O[1] != 0 || m_calibration.G_O[2] != 0) { + m_Logger.info("Calibration data found"); + printCalibrationData(); + result = bmi323.setGyroOffsetGain(m_calibration.G_O, m_calibration.G_G); + if (result == BMI323::SUCCESS) { + m_Logger.info("BMI323 Calibration data applied"); + } else { + m_Logger.error("BMI323 Calibration data apply failed"); + error = true; + } + } else { + m_Logger.warn("No calibration data found, please calibrate the sensor it only takes a few seconds"); + calibrate = true; + } + + // Set gyroscope configuration + result = bmi323.setGyroConfig( + BMI323::GYRO_ODR_200HZ, + BMI323::GYRO_BANDWIDTH_ODR_HALF, + BMI323::GYRO_MODE_HIGH_PERF, + BMI323::GYRO_RANGE_1000DPS, + BMI323::GYRO_AVG_2 + ); + if (result == BMI323::SUCCESS) { + m_Logger.info("BMI323 Gyroscope configured"); + } else { + m_Logger.error("BMI323 Gyroscope configuration failed"); + error = true; + } + + // Set accelerometer configuration + result = bmi323.setAccelConfig( + BMI323::ACCEL_ODR_200HZ, + BMI323::ACCEL_BANDWIDTH_ODR_HALF, + BMI323::ACCEL_MODE_HIGH_PERF, + BMI323::ACCEL_RANGE_8G, + BMI323::ACCEL_AVG_2 + ); + if (result == BMI323::SUCCESS) { + m_Logger.info("BMI323 Accelerometer configured"); + } else { + m_Logger.error("BMI323 Accelerometer configuration failed"); + error = true; + } + + // Set FIFO configuration + bmi323.setFifoConfig(BMI323::FIFO_ACCEL_EN | BMI323::FIFO_GYRO_EN | BMI323::FIFO_TEMP_EN, BMI323::ENABLE); + if (result == BMI323::SUCCESS) { + m_Logger.info("BMI323 FIFO enabled"); + } else { + m_Logger.error("BMI323 FIFO enable failed"); + error = true; + } + + // Calculate variables for BMI323 FIFO + frameLength = BMI323::LENGTH_FIFO_ACCEL + BMI323::LENGTH_FIFO_GYRO + BMI323::LENGTH_TEMPERATURE; + + #if BMI323_USE_BMM350 + // BMM350 + bmm350Address = (this->address == BMI323::ADDRESS_I2C_PRIMARY) ? BMM350::ADDRESS_I2C_PRIMARY : BMM350::ADDRESS_I2C_SECONDARY; + result = bmm350.initI2C(); + if (result == BMM350::SUCCESS) { + m_Logger.info(String("BMM350 initialized on address 0x" + String(bmm350Address, HEX)).c_str()); + } else { + m_Logger.info("BMM350 initialization failed"); + error = true; + } + + // Configure the sensor + result = bmm350.seOdrAvg(BMM350::ODR_25HZ, BMM350::AVG_4); + if (result == BMM350::SUCCESS) { + m_Logger.info("BMM350 configured"); + } else { + m_Logger.error("BMM350 configuration failed"); + error = true; + } + + // Disable interrupt + result = bmm350.setInterruptEnabled(BMM350::DISABLE); + if (result == BMM350::SUCCESS) { + m_Logger.info("BMM350 interrupt disabled"); + } else { + m_Logger.error("BMM350 interrupt disable failed"); + error = true; + } + + // Set power mode + result = bmm350.setPowermode(BMM350::NORMAL_MODE); + if (result == BMM350::SUCCESS) { + m_Logger.info("BMM350 power mode set"); + } else { + m_Logger.error("BMM350 power mode set failed"); + error = true; + } + #endif + + #if BMI323_USE_TEMP_CAL + // allocate temperature memory after calibration because OOM + gyroTempCalibrator = new GyroTemperatureCalibrator( + SlimeVR::Configuration::CalibrationConfigType::BMI323, + sensorId, + GyroSensitivity, + (uint32_t)(0.2f / (1.0f / 200.0f)) + ); + + gyroTempCalibrator->loadConfig(GyroSensitivity); + if (gyroTempCalibrator->config.hasCoeffs) { + gyroTempCalibrator->approximateOffset(m_calibration.temperature, GOxyzStaticTempCompensated); + } + #endif + + #if BMI323_USE_SENSCAL + String localDevice = WiFi.macAddress(); + for (auto const& offsets : sensitivityOffsets) { + if (!localDevice.equals(offsets.mac)) continue; + if (offsets.sensorId != sensorId) continue; + #define BMI323_CALCULATE_SENSITIVTY_MUL(degrees) (1.0 / (1.0 - ((degrees)/(360.0 * offsets.spins)))) + gScaleX = GScale * BMI323_CALCULATE_SENSITIVTY_MUL(offsets.x); + gScaleY = GScale * BMI323_CALCULATE_SENSITIVTY_MUL(offsets.y); + gScaleZ = GScale * BMI323_CALCULATE_SENSITIVTY_MUL(offsets.z); + m_Logger.debug("Custom sensitivity offset enabled: %s %s", + offsets.mac, + offsets.sensorId == SENSORID_PRIMARY ? "primary" : "aux" + ); + } + #endif + + if (calibrate) { + startCalibration(0); + } + + // Tell SlimeVR that the sensor is working and ready + if (!error) { + m_status = SensorStatus::SENSOR_OK; + working = true; + m_Logger.info("BMI323 initialized"); + } else { + m_status = SensorStatus::SENSOR_ERROR; + working = false; + m_Logger.error("BMI323 initialization failed"); + }*/ + + return true; + } + + void motionlessCalibration(MotionlessCalibrationData &gyroSensitivity) + { + + } + + template + void bulkRead(AccelCall &&processAccelSample, GyroCall &&processGyroSample) { + + } + + float getDirectTemp() const + { + const float temp = 1.0f; + + return temp; + } + +}; + +} // namespace \ No newline at end of file From f920164d6f3d2fe8cc0615edaa6b348fef894b82 Mon Sep 17 00:00:00 2001 From: jojos38 Date: Sun, 15 Sep 2024 17:13:50 +0200 Subject: [PATCH 02/10] BMI323 Library adjustements --- lib/bmi323/bmi323_lib.h | 10 +++++----- src/sensors/softfusion/drivers/bmi323.h | 16 ++++++++++------ 2 files changed, 15 insertions(+), 11 deletions(-) diff --git a/lib/bmi323/bmi323_lib.h b/lib/bmi323/bmi323_lib.h index ded06bd21..d8c20b68b 100644 --- a/lib/bmi323/bmi323_lib.h +++ b/lib/bmi323/bmi323_lib.h @@ -26,9 +26,9 @@ #include -typedef int8_t (*ReadFunction)(uint8_t, uint8_t*, uint32_t, const void*); -typedef int8_t (*WriteFunction)(uint8_t, const uint8_t*, uint32_t, const void *); -typedef void (*DelayFunction)(uint32_t, const void *); +typedef int8_t (*ReadFunction)(uint8_t, uint8_t*, uint32_t, void*); +typedef int8_t (*WriteFunction)(uint8_t, const uint8_t*, uint32_t, void *); +typedef void (*DelayFunction)(uint32_t, void *); class BMI323_LIB { @@ -273,7 +273,7 @@ class BMI323_LIB { /****************************************************************************************** * CONSTRUCTOR FUNCTIONS ******************************************************************************************/ - BMI323_LIB(ReadFunction readFunc, WriteFunction writeFunc, DelayFunction delayFunc, const void* interfacePtr) : + BMI323_LIB(ReadFunction readFunc, WriteFunction writeFunc, DelayFunction delayFunc, void* interfacePtr) : readFunction(readFunc), writeFunction(writeFunc), delayFunction(delayFunc), @@ -454,6 +454,6 @@ class BMI323_LIB { ReadFunction readFunction; WriteFunction writeFunction; DelayFunction delayFunction; - const void *interfacePointer; + void *interfacePointer; }; #endif \ No newline at end of file diff --git a/src/sensors/softfusion/drivers/bmi323.h b/src/sensors/softfusion/drivers/bmi323.h index 7c0abe4cc..85a1b9697 100644 --- a/src/sensors/softfusion/drivers/bmi323.h +++ b/src/sensors/softfusion/drivers/bmi323.h @@ -70,22 +70,26 @@ struct BMI323 struct Regs { struct WhoAmI { static constexpr uint8_t reg = 0x00; - static constexpr uint8_t value = 0x68; + static constexpr uint8_t value = 0x24; }; }; - static void delayUs(uint32_t period, const void *) + static void delayUs(uint32_t period, void *) { delay(period / 1000); } - static int8_t i2cRead(uint8_t registerAddress, uint8_t *registerData, uint32_t length, const void *interfacePointer) + static int8_t i2cRead(uint8_t registerAddress, uint8_t *registerData, uint32_t length, void *interfacePointer) { + I2CImpl i2c = *static_cast(interfacePointer); + i2c.readBytes(registerAddress, length, registerData); return 0; } - static int8_t i2cWrite(uint8_t registerAddress, const uint8_t *registerData, uint32_t length, const void *interfacePointer) + static int8_t i2cWrite(uint8_t registerAddress, const uint8_t *registerData, uint32_t length, void *interfacePointer) { + I2CImpl i2c = *static_cast(interfacePointer); + i2c.writeBytes(registerAddress, length, const_cast(registerData)); return 0; } @@ -100,12 +104,12 @@ struct BMI323 int8_t result; - BMI323_LIB bmi323Lib(i2cRead, i2cWrite, delayUs, &Address); + BMI323_LIB bmi323Lib(&i2cRead, &i2cWrite, &delayUs, &i2c); // Initialize the sensor result = bmi323Lib.initI2C(); if (result == BMI323_LIB::SUCCESS) { - logger.info("BMI323 Initialized on address 0x"); + logger.info("BMI323 Initialized on address 0x%x", Address); } else { logger.info("BMI323 Initialization failed"); return false; From 9222e9b285628c7870bf2cdb4de2269a8e07ca29 Mon Sep 17 00:00:00 2001 From: jojos38 Date: Wed, 18 Sep 2024 22:00:27 +0200 Subject: [PATCH 03/10] Sensor initialization working --- src/sensors/softfusion/drivers/bmi323.h | 155 ++++++++++++++---------- 1 file changed, 89 insertions(+), 66 deletions(-) diff --git a/src/sensors/softfusion/drivers/bmi323.h b/src/sensors/softfusion/drivers/bmi323.h index 85a1b9697..298a956c6 100644 --- a/src/sensors/softfusion/drivers/bmi323.h +++ b/src/sensors/softfusion/drivers/bmi323.h @@ -32,11 +32,6 @@ namespace SlimeVR::Sensors::SoftFusion::Drivers { -// Driver uses acceleration range at 16g -// and gyroscope range at 1000dps -// Gyroscope ODR = 400Hz, accel ODR = 100Hz -// Timestamps reading are not used - template struct BMI323 { @@ -45,9 +40,9 @@ struct BMI323 static constexpr auto Name = "BMI323"; static constexpr auto Type = ImuID::BMI323; - static constexpr float GyrTs=1.0/200.0; // Delay between each gyro sample - static constexpr float AccTs=1.0/200.0; // Dealy between each accel sample - static constexpr float MagTs=1.0/25.0; // Delay between each mag sample + static constexpr float GyrTs = 1.0/200.0; // Delay between each gyro sample + static constexpr float AccTs = 1.0/200.0; // Dealy between each accel sample + static constexpr float MagTs = 1.0/25.0; // Delay between each mag sample // BMI323 specific constants static constexpr float GyroSensitivity = 32.768f; @@ -55,22 +50,21 @@ struct BMI323 static constexpr float GScale = ((32768. / GyroSensitivity) / 32768.) * (PI / 180.0); static constexpr float AScale = CONST_EARTH_GRAVITY / AccelSensitivity; - struct MotionlessCalibrationData - { - bool valid; - uint8_t x, y, z; - }; + // Calculate frame length for BMI323 FIFO + static constexpr int8_t frameLength = BMI323_LIB::LENGTH_FIFO_ACCEL + BMI323_LIB::LENGTH_FIFO_GYRO + BMI323_LIB::LENGTH_TEMPERATURE; I2CImpl i2c; SlimeVR::Logging::Logger &logger; int8_t zxFactor; - BMI323(I2CImpl i2c, SlimeVR::Logging::Logger &logger) - : i2c(i2c), logger(logger), zxFactor(0) {} + BMI323_LIB bmi323Lib; + // BMI323(I2CImpl i2c, SlimeVR::Logging::Logger &logger): i2c(i2c), logger(logger), zxFactor(0) {} + BMI323(I2CImpl i2c, SlimeVR::Logging::Logger &logger): i2c(i2c), logger(logger), zxFactor(0), bmi323Lib(&i2cRead, &i2cWrite, &delayUs, &i2c) {} + struct Regs { struct WhoAmI { static constexpr uint8_t reg = 0x00; - static constexpr uint8_t value = 0x24; + static constexpr uint8_t value = 0x00; // 0x43 }; }; @@ -93,19 +87,62 @@ struct BMI323 return 0; } - bool restartAndInit() { - - return true; + /** + * @brief Extracts the next frame from a FIFO frame + * @note The indexes (accelIndex, gyroIndex, tempIndex, timeIndex) needs to + * be reset before the first call + */ + static uint8_t extractFrame(uint8_t *data, uint8_t index, float *accelData, float *gyroData, float *tempData) { + uint8_t dataValidity = ACCEL_VALID | GYRO_VALID | TEMP_VALID; + + // Unpack accelerometer + uint8_t accelIndex = index * frameLength + BMI323_LIB::DUMMY_BYTE; + int16_t isValid = (int16_t)((data[accelIndex + 1] << 8) | data[accelIndex]); + if (isValid == BMI323_LIB::FIFO_ACCEL_DUMMY_FRAME) { + dataValidity &= ~ACCEL_VALID; + } else { + accelData[0] = lsbToMps2(isValid); + accelData[1] = lsbToMps2((int16_t)((data[accelIndex + 3] << 8) | data[accelIndex + 2])); + accelData[2] = lsbToMps2((int16_t)((data[accelIndex + 5] << 8) | data[accelIndex + 4])); + // accelData.sensor_time = (int16_t)((data[accelIndex + 13] << 8) | data[accelIndex + 12]); + } + + // Unpack gyroscope data + uint8_t gyroIndex = accelIndex + BMI323_LIB::LENGTH_FIFO_ACCEL; + isValid = (int16_t)((data[gyroIndex + 1] << 8) | data[gyroIndex]); + if (isValid == BMI323_LIB::FIFO_GYRO_DUMMY_FRAME) { + dataValidity &= ~GYRO_VALID; + } else { + #if BMI323_USE_TEMP_CAL + gyroData[0] = isValid; + gyroData[1] = (int16_t)((data[gyroIndex + 3] << 8) | data[gyroIndex + 2]); + gyroData[2] = (int16_t)((data[gyroIndex + 5] << 8) | data[gyroIndex + 4]); + #else + gyroData[0] = (isValid) * gScaleX; + gyroData[1] = ((int16_t)((data[gyroIndex + 3] << 8) | data[gyroIndex + 2])) * gScaleY; + gyroData[2] = ((int16_t)((data[gyroIndex + 5] << 8) | data[gyroIndex + 4])) * gScaleZ; + #endif + // gyroData.sensor_time = (int16_t)((data[gyroIndex + 7] << 8) | data[gyroIndex + 6]); + } + + // Unpack temperature data + uint8_t tempIndex = gyroIndex + BMI323_LIB::LENGTH_FIFO_GYRO; + uint16_t isTempValid = (uint16_t)((data[tempIndex + 1] << 8) | data[tempIndex]); + if (isTempValid == BMI323_LIB::FIFO_TEMP_DUMMY_FRAME) { + dataValidity &= ~TEMP_VALID; + } else { + tempData[0] = ((int16_t)((data[tempIndex + 1] << 8) | data[tempIndex]) / 512.0f) + 23.0f; + } + + return dataValidity; } - bool initialize(MotionlessCalibrationData &gyroSensitivity) + bool initialize() { logger.info("jojos38 BMI323 firmware V1.4"); int8_t result; - BMI323_LIB bmi323Lib(&i2cRead, &i2cWrite, &delayUs, &i2c); - // Initialize the sensor result = bmi323Lib.initI2C(); if (result == BMI323_LIB::SUCCESS) { @@ -115,6 +152,8 @@ struct BMI323 return false; } + logger.info("Chip ID 0x%x", bmi323Lib.chipId); + // Apply the calibration data /*if (m_calibration.G_G[0] != 0 || m_calibration.G_G[1] != 0 || m_calibration.G_G[2] != 0 || m_calibration.G_O[0] != 0 || m_calibration.G_O[1] != 0 || m_calibration.G_O[2] != 0) { m_Logger.info("Calibration data found"); @@ -129,50 +168,47 @@ struct BMI323 } else { m_Logger.warn("No calibration data found, please calibrate the sensor it only takes a few seconds"); calibrate = true; - } + }*/ // Set gyroscope configuration - result = bmi323.setGyroConfig( - BMI323::GYRO_ODR_200HZ, - BMI323::GYRO_BANDWIDTH_ODR_HALF, - BMI323::GYRO_MODE_HIGH_PERF, - BMI323::GYRO_RANGE_1000DPS, - BMI323::GYRO_AVG_2 + result = bmi323Lib.setGyroConfig( + BMI323_LIB::GYRO_ODR_200HZ, + BMI323_LIB::GYRO_BANDWIDTH_ODR_HALF, + BMI323_LIB::GYRO_MODE_HIGH_PERF, + BMI323_LIB::GYRO_RANGE_1000DPS, + BMI323_LIB::GYRO_AVG_2 ); - if (result == BMI323::SUCCESS) { - m_Logger.info("BMI323 Gyroscope configured"); + if (result == BMI323_LIB::SUCCESS) { + logger.info("BMI323 Gyroscope configured"); } else { - m_Logger.error("BMI323 Gyroscope configuration failed"); - error = true; + logger.error("BMI323 Gyroscope configuration failed"); + return false; } // Set accelerometer configuration - result = bmi323.setAccelConfig( - BMI323::ACCEL_ODR_200HZ, - BMI323::ACCEL_BANDWIDTH_ODR_HALF, - BMI323::ACCEL_MODE_HIGH_PERF, - BMI323::ACCEL_RANGE_8G, - BMI323::ACCEL_AVG_2 + result = bmi323Lib.setAccelConfig( + BMI323_LIB::ACCEL_ODR_200HZ, + BMI323_LIB::ACCEL_BANDWIDTH_ODR_HALF, + BMI323_LIB::ACCEL_MODE_HIGH_PERF, + BMI323_LIB::ACCEL_RANGE_8G, + BMI323_LIB::ACCEL_AVG_2 ); - if (result == BMI323::SUCCESS) { - m_Logger.info("BMI323 Accelerometer configured"); + if (result == BMI323_LIB::SUCCESS) { + logger.info("BMI323 Accelerometer configured"); } else { - m_Logger.error("BMI323 Accelerometer configuration failed"); - error = true; + logger.error("BMI323 Accelerometer configuration failed"); + return false; } // Set FIFO configuration - bmi323.setFifoConfig(BMI323::FIFO_ACCEL_EN | BMI323::FIFO_GYRO_EN | BMI323::FIFO_TEMP_EN, BMI323::ENABLE); - if (result == BMI323::SUCCESS) { - m_Logger.info("BMI323 FIFO enabled"); + bmi323Lib.setFifoConfig(BMI323_LIB::FIFO_ACCEL_EN | BMI323_LIB::FIFO_GYRO_EN | BMI323_LIB::FIFO_TEMP_EN, BMI323_LIB::ENABLE); + if (result == BMI323_LIB::SUCCESS) { + logger.info("BMI323 FIFO enabled"); } else { - m_Logger.error("BMI323 FIFO enable failed"); - error = true; + logger.error("BMI323 FIFO enable failed"); + return false; } - // Calculate variables for BMI323 FIFO - frameLength = BMI323::LENGTH_FIFO_ACCEL + BMI323::LENGTH_FIFO_GYRO + BMI323::LENGTH_TEMPERATURE; - #if BMI323_USE_BMM350 // BMM350 bmm350Address = (this->address == BMI323::ADDRESS_I2C_PRIMARY) ? BMM350::ADDRESS_I2C_PRIMARY : BMM350::ADDRESS_I2C_SECONDARY; @@ -243,25 +279,12 @@ struct BMI323 } #endif - if (calibrate) { - startCalibration(0); - } - - // Tell SlimeVR that the sensor is working and ready - if (!error) { - m_status = SensorStatus::SENSOR_OK; - working = true; - m_Logger.info("BMI323 initialized"); - } else { - m_status = SensorStatus::SENSOR_ERROR; - working = false; - m_Logger.error("BMI323 initialization failed"); - }*/ + logger.info("BMI323 initialized"); return true; } - void motionlessCalibration(MotionlessCalibrationData &gyroSensitivity) + void motionlessCalibration() { } From 435e3da0c346732af9471fdde471afc6bcec5522 Mon Sep 17 00:00:00 2001 From: jojos38 Date: Wed, 18 Sep 2024 22:47:54 +0200 Subject: [PATCH 04/10] Gyro and accel data working --- src/sensors/softfusion/drivers/bmi323.h | 121 +++++++++++++++++++----- 1 file changed, 95 insertions(+), 26 deletions(-) diff --git a/src/sensors/softfusion/drivers/bmi323.h b/src/sensors/softfusion/drivers/bmi323.h index 298a956c6..60cc862b5 100644 --- a/src/sensors/softfusion/drivers/bmi323.h +++ b/src/sensors/softfusion/drivers/bmi323.h @@ -40,8 +40,8 @@ struct BMI323 static constexpr auto Name = "BMI323"; static constexpr auto Type = ImuID::BMI323; - static constexpr float GyrTs = 1.0/200.0; // Delay between each gyro sample - static constexpr float AccTs = 1.0/200.0; // Dealy between each accel sample + static constexpr float GyrTs = 1.0/50.0; // Delay between each gyro sample + static constexpr float AccTs = 1.0/50.0; // Dealy between each accel sample static constexpr float MagTs = 1.0/25.0; // Delay between each mag sample // BMI323 specific constants @@ -50,15 +50,24 @@ struct BMI323 static constexpr float GScale = ((32768. / GyroSensitivity) / 32768.) * (PI / 180.0); static constexpr float AScale = CONST_EARTH_GRAVITY / AccelSensitivity; - // Calculate frame length for BMI323 FIFO - static constexpr int8_t frameLength = BMI323_LIB::LENGTH_FIFO_ACCEL + BMI323_LIB::LENGTH_FIFO_GYRO + BMI323_LIB::LENGTH_TEMPERATURE; + // FIFO variables + static constexpr int8_t FifoFrameLength = BMI323_LIB::LENGTH_FIFO_ACCEL + BMI323_LIB::LENGTH_FIFO_GYRO + BMI323_LIB::LENGTH_TEMPERATURE; + uint8_t fifoData[I2C_BUFFER_LENGTH] = { 0 }; // 2048 is the maximum size of the FIFO + int16_t accelData[3] = { 0 }; + int16_t gyroData[3] = { 0 }; + float temperatureData = 0; + int16_t magData[3] = { 0 }; + + // FIFO Data validity bytes + static const uint8_t ACCEL_VALID = 0x01; // Bit 0 represents accelerometer data validity + static const uint8_t GYRO_VALID = 0x02; // Bit 1 represents gyroscope data validity + static const uint8_t TEMP_VALID = 0x04; // Bit 2 represents temperature data validity I2CImpl i2c; SlimeVR::Logging::Logger &logger; - int8_t zxFactor; BMI323_LIB bmi323Lib; // BMI323(I2CImpl i2c, SlimeVR::Logging::Logger &logger): i2c(i2c), logger(logger), zxFactor(0) {} - BMI323(I2CImpl i2c, SlimeVR::Logging::Logger &logger): i2c(i2c), logger(logger), zxFactor(0), bmi323Lib(&i2cRead, &i2cWrite, &delayUs, &i2c) {} + BMI323(I2CImpl i2c, SlimeVR::Logging::Logger &logger): i2c(i2c), logger(logger), bmi323Lib(&i2cRead, &i2cWrite, &delayUs, &i2c) {} struct Regs { @@ -75,16 +84,41 @@ struct BMI323 static int8_t i2cRead(uint8_t registerAddress, uint8_t *registerData, uint32_t length, void *interfacePointer) { - I2CImpl i2c = *static_cast(interfacePointer); - i2c.readBytes(registerAddress, length, registerData); + uint8_t address = 0x68; + + // Read data from the sensor + Wire.beginTransmission(address); + Wire.write(registerAddress); + Wire.endTransmission(); + Wire.requestFrom(address, length); + for (auto i = 0u; i < length; i++) { + registerData[i] = Wire.read(); + } + return 0; + + /*I2CImpl i2c = *static_cast(interfacePointer); + i2c.readBytes(registerAddress, length, registerData); + return 0;*/ } static int8_t i2cWrite(uint8_t registerAddress, const uint8_t *registerData, uint32_t length, void *interfacePointer) { - I2CImpl i2c = *static_cast(interfacePointer); - i2c.writeBytes(registerAddress, length, const_cast(registerData)); + uint8_t dev_addr = 0x68; + + // Write data to the sensor + Wire.beginTransmission(dev_addr); + Wire.write(registerAddress); + for (auto i = 0u; i < length; i++) { + Wire.write(registerData[i]); + } + Wire.endTransmission(); + return 0; + + /*I2CImpl i2c = *static_cast(interfacePointer); + i2c.writeBytes(registerAddress, length, const_cast(registerData)); + return 0;*/ } /** @@ -92,19 +126,18 @@ struct BMI323 * @note The indexes (accelIndex, gyroIndex, tempIndex, timeIndex) needs to * be reset before the first call */ - static uint8_t extractFrame(uint8_t *data, uint8_t index, float *accelData, float *gyroData, float *tempData) { + static uint8_t extractFrame(uint8_t *data, uint8_t index, int16_t *accelData, int16_t *gyroData, float *tempData) { uint8_t dataValidity = ACCEL_VALID | GYRO_VALID | TEMP_VALID; // Unpack accelerometer - uint8_t accelIndex = index * frameLength + BMI323_LIB::DUMMY_BYTE; + uint8_t accelIndex = index * FifoFrameLength + BMI323_LIB::DUMMY_BYTE; int16_t isValid = (int16_t)((data[accelIndex + 1] << 8) | data[accelIndex]); if (isValid == BMI323_LIB::FIFO_ACCEL_DUMMY_FRAME) { dataValidity &= ~ACCEL_VALID; } else { - accelData[0] = lsbToMps2(isValid); - accelData[1] = lsbToMps2((int16_t)((data[accelIndex + 3] << 8) | data[accelIndex + 2])); - accelData[2] = lsbToMps2((int16_t)((data[accelIndex + 5] << 8) | data[accelIndex + 4])); - // accelData.sensor_time = (int16_t)((data[accelIndex + 13] << 8) | data[accelIndex + 12]); + accelData[0] = isValid; + accelData[1] = (int16_t)((data[accelIndex + 3] << 8) | data[accelIndex + 2]); + accelData[2] = (int16_t)((data[accelIndex + 5] << 8) | data[accelIndex + 4]); } // Unpack gyroscope data @@ -118,11 +151,10 @@ struct BMI323 gyroData[1] = (int16_t)((data[gyroIndex + 3] << 8) | data[gyroIndex + 2]); gyroData[2] = (int16_t)((data[gyroIndex + 5] << 8) | data[gyroIndex + 4]); #else - gyroData[0] = (isValid) * gScaleX; - gyroData[1] = ((int16_t)((data[gyroIndex + 3] << 8) | data[gyroIndex + 2])) * gScaleY; - gyroData[2] = ((int16_t)((data[gyroIndex + 5] << 8) | data[gyroIndex + 4])) * gScaleZ; + gyroData[0] = (isValid); + gyroData[1] = ((int16_t)((data[gyroIndex + 3] << 8) | data[gyroIndex + 2])); + gyroData[2] = ((int16_t)((data[gyroIndex + 5] << 8) | data[gyroIndex + 4])); #endif - // gyroData.sensor_time = (int16_t)((data[gyroIndex + 7] << 8) | data[gyroIndex + 6]); } // Unpack temperature data @@ -172,7 +204,7 @@ struct BMI323 // Set gyroscope configuration result = bmi323Lib.setGyroConfig( - BMI323_LIB::GYRO_ODR_200HZ, + BMI323_LIB::GYRO_ODR_50HZ, BMI323_LIB::GYRO_BANDWIDTH_ODR_HALF, BMI323_LIB::GYRO_MODE_HIGH_PERF, BMI323_LIB::GYRO_RANGE_1000DPS, @@ -187,7 +219,7 @@ struct BMI323 // Set accelerometer configuration result = bmi323Lib.setAccelConfig( - BMI323_LIB::ACCEL_ODR_200HZ, + BMI323_LIB::ACCEL_ODR_50HZ, BMI323_LIB::ACCEL_BANDWIDTH_ODR_HALF, BMI323_LIB::ACCEL_MODE_HIGH_PERF, BMI323_LIB::ACCEL_RANGE_8G, @@ -291,14 +323,51 @@ struct BMI323 template void bulkRead(AccelCall &&processAccelSample, GyroCall &&processGyroSample) { - + // Get available data length in the FIFO + uint16_t availableFifoLength; + bmi323Lib.getFifoLength(&availableFifoLength); + + // Check for FIFO overflow + if (availableFifoLength >= 127) { + logger.error("FIFO OVERFLOW"); + } + + // If there is enough data in the FIFO to get at least a single frame + if (availableFifoLength >= FifoFrameLength) { + // Make sure the length that we read is a multiple of the frame length + uint16_t fifoReadLength = availableFifoLength = std::min(availableFifoLength, static_cast(I2C_BUFFER_LENGTH - BMI323_LIB::DUMMY_BYTE)) / FifoFrameLength * FifoFrameLength + BMI323_LIB::DUMMY_BYTE; + int8_t result = bmi323Lib.readFifoData(fifoData, fifoReadLength); + if (result == BMI323_LIB::SUCCESS) { + // Feed sensor fusion + const uint8_t frameCount = fifoReadLength / FifoFrameLength; + for (int i = 0; i < frameCount; i++) { + float temperature = 0; + uint8_t dataValidity = extractFrame(fifoData, i, accelData, gyroData, &temperature); + + // If the accel data is valid + if (dataValidity & ACCEL_VALID) { + processAccelSample(accelData, AccTs); + } + + // If the gyro data is valid + if (dataValidity & GYRO_VALID) { + processGyroSample(gyroData, GyrTs); + } + + // If the temp data is valid + if (dataValidity & TEMP_VALID) { + temperatureData = temperature; + } + } + } else { + logger.error("FIFO data read failed"); + } + } } float getDirectTemp() const { - const float temp = 1.0f; - - return temp; + return temperatureData; } }; From dc239b549b2b09af5d79f0323a6803668fa87ce2 Mon Sep 17 00:00:00 2001 From: jojos38 Date: Thu, 19 Sep 2024 20:29:43 +0200 Subject: [PATCH 05/10] Fix softfusion watchdog timer reset Fix some microcontrollers crashing (Soft WDT reset) during the calibration loop --- src/sensors/softfusion/softfusionsensor.h | 1 + 1 file changed, 1 insertion(+) diff --git a/src/sensors/softfusion/softfusionsensor.h b/src/sensors/softfusion/softfusionsensor.h index ec076a757..d2a423e83 100644 --- a/src/sensors/softfusion/softfusionsensor.h +++ b/src/sensors/softfusion/softfusionsensor.h @@ -154,6 +154,7 @@ class SoftFusionSensor : public Sensor gyro[1] = xyz[1]; gyro[2] = xyz[2]; } + yield(); ); } return std::make_pair(accel, gyro); From 6607235df947a81b1267e1457c7597cf4b642af8 Mon Sep 17 00:00:00 2001 From: jojos38 Date: Thu, 19 Sep 2024 20:49:03 +0200 Subject: [PATCH 06/10] Fix softfusion watchdog timer reset Fix some microcontrollers crashing (Soft WDT reset) during the calibration loop --- src/sensors/softfusion/softfusionsensor.h | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/sensors/softfusion/softfusionsensor.h b/src/sensors/softfusion/softfusionsensor.h index d2a423e83..5f70e5ff0 100644 --- a/src/sensors/softfusion/softfusionsensor.h +++ b/src/sensors/softfusion/softfusionsensor.h @@ -154,8 +154,8 @@ class SoftFusionSensor : public Sensor gyro[1] = xyz[1]; gyro[2] = xyz[2]; } - yield(); ); + yield(); } return std::make_pair(accel, gyro); } @@ -485,6 +485,7 @@ class SoftFusionSensor : public Sensor [&accelSamples](const int16_t xyz[3], const sensor_real_t timeDelta) { accelSamples++; }, [&gyroSamples](const int16_t xyz[3], const sensor_real_t timeDelta) { gyroSamples++; } ); + yield(); } const auto millisFromStart = currentTime - (calibTarget - 1000 * SampleRateCalibSeconds); From 7ded129e45a7a1647da3fd4c867120d04666d019 Mon Sep 17 00:00:00 2001 From: jojos38 Date: Thu, 19 Sep 2024 20:49:14 +0200 Subject: [PATCH 07/10] BMI323 sfusion ready --- src/sensors/softfusion/drivers/bmi323.h | 256 ++++++++++++------------ 1 file changed, 125 insertions(+), 131 deletions(-) diff --git a/src/sensors/softfusion/drivers/bmi323.h b/src/sensors/softfusion/drivers/bmi323.h index 60cc862b5..d09dc3f7d 100644 --- a/src/sensors/softfusion/drivers/bmi323.h +++ b/src/sensors/softfusion/drivers/bmi323.h @@ -35,42 +35,56 @@ namespace SlimeVR::Sensors::SoftFusion::Drivers template struct BMI323 { - // Sensor Fusion required variables - static constexpr uint8_t Address = 0x68; // i2c address + /** + * Sensor Fusion required variables + */ + static constexpr uint8_t Address = 0x68; // IMU's i2c address used to communicate static constexpr auto Name = "BMI323"; static constexpr auto Type = ImuID::BMI323; - - static constexpr float GyrTs = 1.0/50.0; // Delay between each gyro sample - static constexpr float AccTs = 1.0/50.0; // Dealy between each accel sample + + static constexpr float GyrTs = 1.0/200.0; // Delay between each gyro sample + static constexpr float AccTs = 1.0/200.0; // Dealy between each accel sample static constexpr float MagTs = 1.0/25.0; // Delay between each mag sample - // BMI323 specific constants static constexpr float GyroSensitivity = 32.768f; static constexpr float AccelSensitivity = 4096.0f; static constexpr float GScale = ((32768. / GyroSensitivity) / 32768.) * (PI / 180.0); static constexpr float AScale = CONST_EARTH_GRAVITY / AccelSensitivity; // FIFO variables + float latestTemperature = 0; static constexpr int8_t FifoFrameLength = BMI323_LIB::LENGTH_FIFO_ACCEL + BMI323_LIB::LENGTH_FIFO_GYRO + BMI323_LIB::LENGTH_TEMPERATURE; - uint8_t fifoData[I2C_BUFFER_LENGTH] = { 0 }; // 2048 is the maximum size of the FIFO - int16_t accelData[3] = { 0 }; - int16_t gyroData[3] = { 0 }; - float temperatureData = 0; - int16_t magData[3] = { 0 }; - - // FIFO Data validity bytes static const uint8_t ACCEL_VALID = 0x01; // Bit 0 represents accelerometer data validity static const uint8_t GYRO_VALID = 0x02; // Bit 1 represents gyroscope data validity static const uint8_t TEMP_VALID = 0x04; // Bit 2 represents temperature data validity + // Intialization and variables I2CImpl i2c; SlimeVR::Logging::Logger &logger; BMI323_LIB bmi323Lib; - // BMI323(I2CImpl i2c, SlimeVR::Logging::Logger &logger): i2c(i2c), logger(logger), zxFactor(0) {} BMI323(I2CImpl i2c, SlimeVR::Logging::Logger &logger): i2c(i2c), logger(logger), bmi323Lib(&i2cRead, &i2cWrite, &delayUs, &i2c) {} + /** + * Calibration data for the basic static calibration + * Detected by sfusion and saved when values are put in + * Anything can be put in this struct + */ + struct MotionlessCalibrationData + { + bool valid; + + // accelerometer offsets and gain + uint16_t G_O[3]; + uint8_t G_G[3]; + }; struct Regs { + /** + * The first value (reg) defines which register address the chip ID is located at + * The second value (value) defines the expected value + * This is used to make sure we have the correct IMU + * BUT... It doesn't work on this IMU so I bypassed it + */ struct WhoAmI { static constexpr uint8_t reg = 0x00; static constexpr uint8_t value = 0x00; // 0x43 @@ -84,49 +98,52 @@ struct BMI323 static int8_t i2cRead(uint8_t registerAddress, uint8_t *registerData, uint32_t length, void *interfacePointer) { - uint8_t address = 0x68; - - // Read data from the sensor - Wire.beginTransmission(address); + Wire.beginTransmission(Address); Wire.write(registerAddress); Wire.endTransmission(); - Wire.requestFrom(address, length); + Wire.requestFrom(Address, length); for (auto i = 0u; i < length; i++) { registerData[i] = Wire.read(); } - return 0; - - /*I2CImpl i2c = *static_cast(interfacePointer); - i2c.readBytes(registerAddress, length, registerData); + /*I2CImpl* i2c = static_cast(interfacePointer);; + i2c->readBytes(registerAddress, length + 2, registerData); return 0;*/ } static int8_t i2cWrite(uint8_t registerAddress, const uint8_t *registerData, uint32_t length, void *interfacePointer) { - uint8_t dev_addr = 0x68; - - // Write data to the sensor - Wire.beginTransmission(dev_addr); + Wire.beginTransmission(Address); Wire.write(registerAddress); for (auto i = 0u; i < length; i++) { Wire.write(registerData[i]); } Wire.endTransmission(); - return 0; - - /*I2CImpl i2c = *static_cast(interfacePointer); - i2c.writeBytes(registerAddress, length, const_cast(registerData)); + /*I2CImpl* i2c = static_cast(interfacePointer); + i2c->writeBytes(registerAddress, length + 2, const_cast(registerData)); return 0;*/ } + /** + * @brief Print calibration data + */ + void printCalibrationData(MotionlessCalibrationData &calibrationData) { + if (!calibrationData.valid) { + logger.debug("No calibration data"); + } else { + logger.debug("Gyro offset: %u, %u, %u", calibrationData.G_O[0], calibrationData.G_O[1], calibrationData.G_O[2]); + logger.debug("Gyro gain: %u, %u, %u", calibrationData.G_G[0], calibrationData.G_G[1], calibrationData.G_G[2]); + } + } + /** * @brief Extracts the next frame from a FIFO frame * @note The indexes (accelIndex, gyroIndex, tempIndex, timeIndex) needs to * be reset before the first call */ static uint8_t extractFrame(uint8_t *data, uint8_t index, int16_t *accelData, int16_t *gyroData, float *tempData) { + // Set dataValidity to all valid uint8_t dataValidity = ACCEL_VALID | GYRO_VALID | TEMP_VALID; // Unpack accelerometer @@ -146,15 +163,9 @@ struct BMI323 if (isValid == BMI323_LIB::FIFO_GYRO_DUMMY_FRAME) { dataValidity &= ~GYRO_VALID; } else { - #if BMI323_USE_TEMP_CAL - gyroData[0] = isValid; - gyroData[1] = (int16_t)((data[gyroIndex + 3] << 8) | data[gyroIndex + 2]); - gyroData[2] = (int16_t)((data[gyroIndex + 5] << 8) | data[gyroIndex + 4]); - #else - gyroData[0] = (isValid); - gyroData[1] = ((int16_t)((data[gyroIndex + 3] << 8) | data[gyroIndex + 2])); - gyroData[2] = ((int16_t)((data[gyroIndex + 5] << 8) | data[gyroIndex + 4])); - #endif + gyroData[0] = (isValid); + gyroData[1] = ((int16_t)((data[gyroIndex + 3] << 8) | data[gyroIndex + 2])); + gyroData[2] = ((int16_t)((data[gyroIndex + 5] << 8) | data[gyroIndex + 4])); } // Unpack temperature data @@ -169,7 +180,7 @@ struct BMI323 return dataValidity; } - bool initialize() + bool initialize(MotionlessCalibrationData &calibrationData) { logger.info("jojos38 BMI323 firmware V1.4"); @@ -187,24 +198,23 @@ struct BMI323 logger.info("Chip ID 0x%x", bmi323Lib.chipId); // Apply the calibration data - /*if (m_calibration.G_G[0] != 0 || m_calibration.G_G[1] != 0 || m_calibration.G_G[2] != 0 || m_calibration.G_O[0] != 0 || m_calibration.G_O[1] != 0 || m_calibration.G_O[2] != 0) { - m_Logger.info("Calibration data found"); - printCalibrationData(); - result = bmi323.setGyroOffsetGain(m_calibration.G_O, m_calibration.G_G); - if (result == BMI323::SUCCESS) { - m_Logger.info("BMI323 Calibration data applied"); + if (calibrationData.valid) { + logger.info("Calibration data found"); + printCalibrationData(calibrationData); + result = bmi323Lib.setGyroOffsetGain(calibrationData.G_O, calibrationData.G_G); + if (result == BMI323_LIB::SUCCESS) { + logger.info("BMI323 Calibration data applied"); } else { - m_Logger.error("BMI323 Calibration data apply failed"); - error = true; + logger.error("BMI323 Calibration data apply failed"); + return false; } } else { - m_Logger.warn("No calibration data found, please calibrate the sensor it only takes a few seconds"); - calibrate = true; - }*/ + logger.warn("No calibration data found, please calibrate the sensor"); + } // Set gyroscope configuration result = bmi323Lib.setGyroConfig( - BMI323_LIB::GYRO_ODR_50HZ, + BMI323_LIB::GYRO_ODR_200HZ, BMI323_LIB::GYRO_BANDWIDTH_ODR_HALF, BMI323_LIB::GYRO_MODE_HIGH_PERF, BMI323_LIB::GYRO_RANGE_1000DPS, @@ -219,7 +229,7 @@ struct BMI323 // Set accelerometer configuration result = bmi323Lib.setAccelConfig( - BMI323_LIB::ACCEL_ODR_50HZ, + BMI323_LIB::ACCEL_ODR_200HZ, BMI323_LIB::ACCEL_BANDWIDTH_ODR_HALF, BMI323_LIB::ACCEL_MODE_HIGH_PERF, BMI323_LIB::ACCEL_RANGE_8G, @@ -241,84 +251,58 @@ struct BMI323 return false; } - #if BMI323_USE_BMM350 - // BMM350 - bmm350Address = (this->address == BMI323::ADDRESS_I2C_PRIMARY) ? BMM350::ADDRESS_I2C_PRIMARY : BMM350::ADDRESS_I2C_SECONDARY; - result = bmm350.initI2C(); - if (result == BMM350::SUCCESS) { - m_Logger.info(String("BMM350 initialized on address 0x" + String(bmm350Address, HEX)).c_str()); - } else { - m_Logger.info("BMM350 initialization failed"); - error = true; - } - - // Configure the sensor - result = bmm350.seOdrAvg(BMM350::ODR_25HZ, BMM350::AVG_4); - if (result == BMM350::SUCCESS) { - m_Logger.info("BMM350 configured"); - } else { - m_Logger.error("BMM350 configuration failed"); - error = true; - } - - // Disable interrupt - result = bmm350.setInterruptEnabled(BMM350::DISABLE); - if (result == BMM350::SUCCESS) { - m_Logger.info("BMM350 interrupt disabled"); - } else { - m_Logger.error("BMM350 interrupt disable failed"); - error = true; - } - - // Set power mode - result = bmm350.setPowermode(BMM350::NORMAL_MODE); - if (result == BMM350::SUCCESS) { - m_Logger.info("BMM350 power mode set"); - } else { - m_Logger.error("BMM350 power mode set failed"); - error = true; - } - #endif - - #if BMI323_USE_TEMP_CAL - // allocate temperature memory after calibration because OOM - gyroTempCalibrator = new GyroTemperatureCalibrator( - SlimeVR::Configuration::CalibrationConfigType::BMI323, - sensorId, - GyroSensitivity, - (uint32_t)(0.2f / (1.0f / 200.0f)) - ); - - gyroTempCalibrator->loadConfig(GyroSensitivity); - if (gyroTempCalibrator->config.hasCoeffs) { - gyroTempCalibrator->approximateOffset(m_calibration.temperature, GOxyzStaticTempCompensated); - } - #endif - - #if BMI323_USE_SENSCAL - String localDevice = WiFi.macAddress(); - for (auto const& offsets : sensitivityOffsets) { - if (!localDevice.equals(offsets.mac)) continue; - if (offsets.sensorId != sensorId) continue; - #define BMI323_CALCULATE_SENSITIVTY_MUL(degrees) (1.0 / (1.0 - ((degrees)/(360.0 * offsets.spins)))) - gScaleX = GScale * BMI323_CALCULATE_SENSITIVTY_MUL(offsets.x); - gScaleY = GScale * BMI323_CALCULATE_SENSITIVTY_MUL(offsets.y); - gScaleZ = GScale * BMI323_CALCULATE_SENSITIVTY_MUL(offsets.z); - m_Logger.debug("Custom sensitivity offset enabled: %s %s", - offsets.mac, - offsets.sensorId == SENSORID_PRIMARY ? "primary" : "aux" - ); - } - #endif - logger.info("BMI323 initialized"); return true; } - void motionlessCalibration() + void motionlessCalibration(MotionlessCalibrationData &calibrationData) { + // Calibrate sensitivity (ALWAYS FIRST) + logger.info("Calibrating gyroscope sensitivity in 5 seconds... Please do not move the device"); + delay(5000); + ledManager.on(); + struct BMI323_LIB::SelfCalibResult calibSensitivityResult; + uint8_t result = bmi323Lib.performGyroCalibration(BMI323_LIB::CALIBRATION_SENSITIVITY, BMI323_LIB::CALIBRATION_APPLY_TRUE, &calibSensitivityResult); + if (result == BMI323_LIB::SUCCESS) { + logger.info("Gyroscope sensitivity calibration succeed"); + } else { + logger.error("Gyroscope sensitivity calibration failed"); + return; + } + // Calibrate offset + logger.info("Calibrating gyroscope offset... Please do not move the device"); + struct BMI323_LIB::SelfCalibResult calibOffsetResult; + result = bmi323Lib.performGyroCalibration(BMI323_LIB::CALIBRATION_OFFSET, BMI323_LIB::CALIBRATION_APPLY_TRUE, &calibOffsetResult); + if (result == BMI323_LIB::SUCCESS) { + logger.info("Gyroscope offset calibration succeed"); + } else { + logger.error("Gyroscope offset calibration failed"); + return; + } + + // Save results + uint16_t offset[3]; + uint8_t gain[3]; + bmi323Lib.getGyroOffsetGain(offset, gain); + + // Save the calibration data + logger.info("Saving calibration data"); + calibrationData.G_O[0] = offset[0]; + calibrationData.G_O[1] = offset[1]; + calibrationData.G_O[2] = offset[2]; + + calibrationData.G_G[0] = gain[0]; + calibrationData.G_G[1] = gain[1]; + calibrationData.G_G[2] = gain[2]; + + calibrationData.valid = true; + + printCalibrationData(calibrationData); + + logger.info("Motionless calibration done"); + ledManager.off(); } template @@ -328,20 +312,30 @@ struct BMI323 bmi323Lib.getFifoLength(&availableFifoLength); // Check for FIFO overflow - if (availableFifoLength >= 127) { + /*if (availableFifoLength >= 127) { logger.error("FIFO OVERFLOW"); - } + }*/ // If there is enough data in the FIFO to get at least a single frame - if (availableFifoLength >= FifoFrameLength) { + if (availableFifoLength >= FifoFrameLength) { + + // Variable to store FIFO data + uint8_t fifoData[I2C_BUFFER_LENGTH] = { 0 }; + // Make sure the length that we read is a multiple of the frame length uint16_t fifoReadLength = availableFifoLength = std::min(availableFifoLength, static_cast(I2C_BUFFER_LENGTH - BMI323_LIB::DUMMY_BYTE)) / FifoFrameLength * FifoFrameLength + BMI323_LIB::DUMMY_BYTE; + + // Read the FIFO data int8_t result = bmi323Lib.readFifoData(fifoData, fifoReadLength); + if (result == BMI323_LIB::SUCCESS) { - // Feed sensor fusion + // Count the number of frames in the FIFO const uint8_t frameCount = fifoReadLength / FifoFrameLength; + // Unpack each frame one by one for (int i = 0; i < frameCount; i++) { float temperature = 0; + int16_t accelData[3] = { 0 }; + int16_t gyroData[3] = { 0 }; uint8_t dataValidity = extractFrame(fifoData, i, accelData, gyroData, &temperature); // If the accel data is valid @@ -356,7 +350,7 @@ struct BMI323 // If the temp data is valid if (dataValidity & TEMP_VALID) { - temperatureData = temperature; + latestTemperature = temperature; } } } else { @@ -367,7 +361,7 @@ struct BMI323 float getDirectTemp() const { - return temperatureData; + return latestTemperature; } }; From c3d07b78da7ffeadcaacab1fafac580159b29b6a Mon Sep 17 00:00:00 2001 From: jojos38 Date: Thu, 19 Sep 2024 21:41:23 +0200 Subject: [PATCH 08/10] Revert defines.h changes --- src/defines.h | 24 +++--------------------- 1 file changed, 3 insertions(+), 21 deletions(-) diff --git a/src/defines.h b/src/defines.h index 3d6275160..33296454a 100644 --- a/src/defines.h +++ b/src/defines.h @@ -26,9 +26,9 @@ // ================================================ // Set parameters of IMU and board used -#define IMU IMU_BMI323 -#define SECOND_IMU IMU_BMI323 -#define BOARD BOARD_CUSTOM +#define IMU IMU_BNO085 +#define SECOND_IMU IMU +#define BOARD BOARD_SLIMEVR #define IMU_ROTATION DEG_270 #define SECOND_IMU_ROTATION DEG_270 @@ -149,24 +149,6 @@ IMU_DESC_ENTRY(IMU_BMP160, PRIMARY_IMU_ADDRESS_ONE, IMU_ROTATION, PIN_IMU_SCL, P // #define LED_INVERTED false #elif BOARD == BOARD_CUSTOM // Define pins by the examples above - #define PIN_IMU_SDA 4 - #define PIN_IMU_SCL 5 - #define PIN_IMU_INT 14 - #define PIN_IMU_INT_2 12 - #define PIN_BATTERY_LEVEL A0 - #define LED_PIN 15 - #define LED_INVERTED false - // The Wemos D1 mini has an internal voltage divider that I don't have on my custom board - // Therefor, BATTERY_SHIELD_RESISTANCE is set to 0 and SHIELD_R1 and R2 are adjusted instead - #ifndef BATTERY_SHIELD_RESISTANCE - #define BATTERY_SHIELD_RESISTANCE 0 - #endif - #ifndef BATTERY_SHIELD_R1 - #define BATTERY_SHIELD_R1 36 - #endif - #ifndef BATTERY_SHIELD_R2 - #define BATTERY_SHIELD_R2 123.5 - #endif #elif BOARD == BOARD_WROOM32 #define PIN_IMU_SDA 21 #define PIN_IMU_SCL 22 From c34a80e82d245e9964d88db4f2809f19603778dd Mon Sep 17 00:00:00 2001 From: jojos38 Date: Thu, 19 Sep 2024 22:07:42 +0200 Subject: [PATCH 09/10] Remove sfusion changes --- src/sensors/softfusion/softfusionsensor.h | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/sensors/softfusion/softfusionsensor.h b/src/sensors/softfusion/softfusionsensor.h index 5f70e5ff0..ec076a757 100644 --- a/src/sensors/softfusion/softfusionsensor.h +++ b/src/sensors/softfusion/softfusionsensor.h @@ -155,7 +155,6 @@ class SoftFusionSensor : public Sensor gyro[2] = xyz[2]; } ); - yield(); } return std::make_pair(accel, gyro); } @@ -485,7 +484,6 @@ class SoftFusionSensor : public Sensor [&accelSamples](const int16_t xyz[3], const sensor_real_t timeDelta) { accelSamples++; }, [&gyroSamples](const int16_t xyz[3], const sensor_real_t timeDelta) { gyroSamples++; } ); - yield(); } const auto millisFromStart = currentTime - (calibTarget - 1000 * SampleRateCalibSeconds); From 20d80c845a8b8c10946024a55c54492768e82636 Mon Sep 17 00:00:00 2001 From: jojos38 Date: Sat, 25 Jan 2025 17:23:01 +0100 Subject: [PATCH 10/10] Fix for latest commits --- src/sensors/softfusion/drivers/bmi323.h | 1 + 1 file changed, 1 insertion(+) diff --git a/src/sensors/softfusion/drivers/bmi323.h b/src/sensors/softfusion/drivers/bmi323.h index d09dc3f7d..450476b0f 100644 --- a/src/sensors/softfusion/drivers/bmi323.h +++ b/src/sensors/softfusion/drivers/bmi323.h @@ -28,6 +28,7 @@ #include #include #include "bmi323_lib.h" +#include "GlobalVars.h" namespace SlimeVR::Sensors::SoftFusion::Drivers {