diff --git a/README.md b/README.md index 76bfd25..f112996 100644 --- a/README.md +++ b/README.md @@ -26,9 +26,11 @@ The following IMUs and their corresponding `IMU` values are supported by the fir * Specify `IMU_MPU6500` in your `defines.h` to use without magnetometer in 6DoF mode. * Experimental support! * BMI160 (IMU_BMI160) - * Using Mahony sensor fusion of Gyroscope and Accelerometer - * See *Sensor calibration* below for info on calibrating this sensor. + * Using sensor fusion of Gyroscope and Accelerometer. + * **See Sensor calibration** below for info on calibrating this sensor. + * Calibration file format is unstable and may not be able to load using newer firmware versions. * Experimental support! + * Support for the following magnetometers is implemented (even more experimental): HMC5883L, QMC5883L. * ICM-20948 (IMU_ICM20948) * Using fusion in internal DMP for 6Dof or 9DoF, 9DoF mode requires good magnetic environment. * Comment out `USE_6DOF` in `debug.h` for 9DoF mode. @@ -41,13 +43,58 @@ Firmware can work with both ESP8266 and ESP32. Please edit `defines.h` and set y *It is generally recommended to turn trackers on and let them lay down on a flat surface for a few seconds.** This will calibrate them better. **Some trackers require special calibration steps on startup:** -* MPU-9250, BMI160 +### MPU-9250 * Turn them on with chip facing down. Flip up and put on a surface for a couple of seconds, the LED will light up. * After a few blinks, the LED will light up again * Slowly rotate the tracker in an 8-motion facing different directions for about 30 seconds, while LED is blinking * LED will turn off when calibration is complete * You don't have to calibrate next time you power it on, calibration values will be saved for the next use +### BMI160 + + If you have any problems with this procedure, connect the device via USB and open the serial console to check for any warnings or errors that may indicate hardware problems. + + - **Step 0: Power up with the chip facing down.** Or press the reset/reboot button. + + > The LED will be lit continuously. If you have the tracker connected via USB and open the serial console, you will see text prompts in addition to the LEDs. You can only calibrate 1 IMU at a time. + + Flip it back up while the LED is still solid. Wait a few seconds, do not touch the device. + + - **Step 1: It will flash 3 times when gyroscope calibration begins.** + + > If done incorrectly, this step is the most likely source of constant drift. + + Make sure the tracker does not move or vibrate for 5 seconds - still do not touch it. + + - **Step 2: It will flash 6 times when accelerometer calibration begins.** + + > The accelerometer calibration process requires you to **hold the device in 6 unique orientations** (e.g. sides of a cube), + > keep it still, and not hold or touch for 3 seconds each. It uses gravity as a reference and automatically detects when it is stabilized - this process is not time-sensitive. + + > If you are unable to keep it on a flat surface without touching, press the device against a wall, it does not have to be absolutely perfect. + + **There will be two very short blinks when each position is recorded.** + + Rotate the device 90 or 180 degrees in any direction. It should be on a different side each time. Continue to rotate until all 6 sides have been recorded. + + The last position has a long flash when recorded, indicating exit from calibration mode. + + #### Additional info for BMI160 + - For best results, **calibrate when the trackers are warmed up** - put them on for a few minutes, + wait for the temperature to stabilize at 30-40 degrees C, then calibrate. + Enable developer mode in SlimeVR settings to see tracker temperature. + + - There is a legacy accelerometer calibration method that collects data during in-place rotation by holding it in hand instead. + If you are absolutely unable to use the default 6-point calibration method, you can switch it in config file `defines_bmi160.h`. + + - For faster recalibration, you disable accelerometer calibration by setting `BMI160_ACCEL_CALIBRATION_METHOD` option to `ACCEL_CALIBRATION_METHOD_SKIP` in `defines_bmi160.h`. + Accelerometer calibration can be safely skipped if you don't have issues with pitch and roll. + You can check it by enabling developer mode in SlimeVR settings (*General / Interface*) and going back to the *"Home"* tab. + Press *"Preview"* button inside the tracker settings (of each tracker) to show the IMU visualizer. + Check if pitch/roll resembles its real orientation. + + - Calibration data is written to the flash of your MCU and is unique for each BMI160, keep that in mind if you have detachable aux trackers. + ## Infos about ESP32-C3 with direct connection to USB The ESP32-C3 has two ways to connect the serial port. One is directly via the onboard USB CDC or via the onboard UART. diff --git a/ci/platformio.ini b/ci/platformio.ini index c78a480..36fb510 100644 --- a/ci/platformio.ini +++ b/ci/platformio.ini @@ -5,4 +5,5 @@ monitor_speed = 115200 framework = arduino build_flags = -O2 -build_unflags = -Os + -std=gnu++17 +build_unflags = -Os -std=gnu++11 diff --git a/lib/bmi160/BMI160.cpp b/lib/bmi160/BMI160.cpp index 56a74e1..ebb1c8b 100644 --- a/lib/bmi160/BMI160.cpp +++ b/lib/bmi160/BMI160.cpp @@ -72,38 +72,64 @@ BMI160::BMI160() {}; * This will activate the device and take it out of sleep mode (which must be done * after start-up). */ -void BMI160::initialize(uint8_t addr) -{ +void BMI160::initialize(uint8_t addr, + BMI160GyroRate gyroRate, BMI160GyroRange gyroRange, BMI160DLPFMode gyroFilterMode, + BMI160AccelRate accelRate, BMI160AccelRange accelRange, BMI160DLPFMode accelFilterMode +) { devAddr = addr; /* Issue a soft-reset to bring the device into a clean state */ - I2CdevMod::writeByte(devAddr, BMI160_RA_CMD, BMI160_CMD_SOFT_RESET); + setRegister(BMI160_RA_CMD, BMI160_CMD_SOFT_RESET); + delay(12); + + setGyroRate(gyroRate); + delay(1); + setAccelRate(accelRate); + delay(1); + setFullScaleGyroRange(gyroRange); + delay(1); + setFullScaleAccelRange(accelRange); + delay(1); + setGyroDLPFMode(gyroFilterMode); + delay(1); + setAccelDLPFMode(accelFilterMode); delay(1); /* Power up the accelerometer */ - I2CdevMod::writeByte(devAddr, BMI160_RA_CMD, BMI160_CMD_ACC_MODE_NORMAL); + setRegister(BMI160_RA_CMD, BMI160_CMD_ACC_MODE_NORMAL); delay(BMI160_ACCEL_POWERUP_DELAY_MS); /* Power up the gyroscope */ - I2CdevMod::writeByte(devAddr, BMI160_RA_CMD, BMI160_CMD_GYR_MODE_NORMAL); + setRegister(BMI160_RA_CMD, BMI160_CMD_GYR_MODE_NORMAL); delay(BMI160_GYRO_POWERUP_DELAY_MS); - setGyroRate(BMI160_GYRO_RATE_800HZ); - delay(1); - setAccelRate(BMI160_ACCEL_RATE_800HZ); - delay(1); - setFullScaleGyroRange(BMI160_GYRO_RANGE_500); - delay(1); - setFullScaleAccelRange(BMI160_ACCEL_RANGE_4G); - delay(1); - setGyroDLPFMode(BMI160_DLPF_MODE_OSR4); - delay(1); - setAccelDLPFMode(BMI160_DLPF_MODE_OSR4); - delay(1); - /* Only PIN1 interrupts currently supported - map all interrupts to PIN1 */ - I2CdevMod::writeByte(devAddr, BMI160_RA_INT_MAP_0, 0xFF); - I2CdevMod::writeByte(devAddr, BMI160_RA_INT_MAP_1, 0xF0); - I2CdevMod::writeByte(devAddr, BMI160_RA_INT_MAP_2, 0x00); + // I2CdevMod::writeByte(devAddr, BMI160_RA_INT_MAP_0, 0xFF); + // I2CdevMod::writeByte(devAddr, BMI160_RA_INT_MAP_1, 0xF0); + // I2CdevMod::writeByte(devAddr, BMI160_RA_INT_MAP_2, 0x00); +} + +bool BMI160::getErrReg(uint8_t* out) { + bool ok = I2CdevMod::readByte(devAddr, BMI160_RA_ERR, buffer) >= 0; + if (!ok) return false; + *out = buffer[0]; + return true; +} + + +void BMI160::setMagDeviceAddress(uint8_t addr) { + setRegister(BMI160_RA_MAG_IF_0_DEVADDR, addr << 1); // 0 bit of address is reserved and needs to be shifted +} + +bool BMI160::setMagRegister(uint8_t addr, uint8_t value) { + setRegister(BMI160_RA_MAG_IF_4_WRITE_VALUE, value); + setRegister(BMI160_RA_MAG_IF_3_WRITE_RA, addr); + delay(3); + I2CdevMod::readByte(devAddr, BMI160_RA_ERR, buffer); + if (buffer[0] & BMI160_ERR_MASK_I2C_FAIL) { + printf("BMI160: mag register proxy write error\n"); + return false; + } + return true; } /** Get Device ID. @@ -1450,6 +1476,31 @@ void BMI160::setGyroFIFOEnabled(bool enabled) { 1, enabled ? 0x1 : 0); } + +/** Get magnetometer FIFO enabled value. + * When set to 1, this bit enables magnetometer data samples to be + * written into the FIFO buffer. + * @return Current magnetometer FIFO enabled value + * @see BMI160_RA_FIFO_CONFIG_1 + */ +bool BMI160::getMagFIFOEnabled() { + I2CdevMod::readBits(devAddr, BMI160_RA_FIFO_CONFIG_1, + BMI160_FIFO_MAG_EN_BIT, + 1, buffer); + return !!buffer[0]; +} + +/** Set magnetometer FIFO enabled value. + * @param enabled New magnetometer FIFO enabled value + * @see getMagFIFOEnabled() + * @see BMI160_RA_FIFO_CONFIG_1 + */ +void BMI160::setMagFIFOEnabled(bool enabled) { + I2CdevMod::writeBits(devAddr, BMI160_RA_FIFO_CONFIG_1, + BMI160_FIFO_MAG_EN_BIT, + 1, enabled ? 0x1 : 0); +} + /** Get current FIFO buffer size. * This value indicates the number of bytes stored in the FIFO buffer. This * number is in turn the number of bytes that can be read from the FIFO buffer. @@ -1458,12 +1509,15 @@ void BMI160::setGyroFIFOEnabled(bool enabled) { * samples available given the set of sensor data bound to be stored in the * FIFO. See @ref getFIFOHeaderModeEnabled(). * - * @return Current FIFO buffer size + * @param outCount Current FIFO buffer size + * @return Bool if value was read successfully * @see BMI160_RA_FIFO_LENGTH_0 */ -uint16_t BMI160::getFIFOCount() { - I2CdevMod::readBytes(devAddr, BMI160_RA_FIFO_LENGTH_0, 2, buffer); - return (((int16_t)buffer[1]) << 8) | buffer[0]; +bool BMI160::getFIFOCount(uint16_t* outCount) { + bool ok = I2CdevMod::readBytes(devAddr, BMI160_RA_FIFO_LENGTH_0, 2, buffer) >= 0; + if (!ok) return false; + *outCount = (((int16_t)buffer[1]) << 8) | buffer[0]; + return ok; } /** Reset the FIFO. @@ -1541,12 +1595,14 @@ void BMI160::setFIFOHeaderModeEnabled(bool enabled) { * check FIFO_LENGTH to ensure that the FIFO buffer is not read when empty (see * @getFIFOCount()). * - * @return Data frames from FIFO buffer + * @param data Data frames from FIFO buffer + * @param length Buffer length + * @return Bool if value was read successfully */ -void BMI160::getFIFOBytes(uint8_t *data, uint16_t length) { - if (length) { - I2CdevMod::readBytes(devAddr, BMI160_RA_FIFO_DATA, length, data); - } +bool BMI160::getFIFOBytes(uint8_t *data, uint16_t length) { + if (!length) return true; + bool ok = I2CdevMod::readBytes(devAddr, BMI160_RA_FIFO_DATA, length, data) >= 0; + return ok; } /** Get full set of interrupt status bits from INT_STATUS[0] register. @@ -2213,12 +2269,15 @@ int16_t BMI160::getAccelerationZ() { * 0x8001 | -41 + 1/2^9 degrees C * 0x8000 | Invalid * - * @return Temperature reading in 16-bit 2's complement format + * @param out Temperature reading in 16-bit 2's complement format + * @return Bool if value was read successfully * @see BMI160_RA_TEMP_L */ -int16_t BMI160::getTemperature() { - I2CdevMod::readBytes(devAddr, BMI160_RA_TEMP_L, 2, buffer); - return (((int16_t)buffer[1]) << 8) | buffer[0]; +bool BMI160::getTemperature(int16_t* out) { + bool ok = I2CdevMod::readBytes(devAddr, BMI160_RA_TEMP_L, 2, buffer) >= 0; + if (!ok) return false; + *out = (((int16_t)buffer[1]) << 8) | buffer[0]; + return ok; } /** Get 3-axis gyroscope readings. @@ -2291,6 +2350,25 @@ int16_t BMI160::getRotationZ() { return (((int16_t)buffer[1]) << 8) | buffer[0]; } +/** Get magnetometer readings + * @return Z-axis rotation measurement in 16-bit 2's complement format + * @see getMotion6() + * @see BMI160_RA_GYRO_Z_L + */ +void BMI160::getMagnetometer(int16_t* mx, int16_t* my, int16_t* mz) { + I2CdevMod::readBytes(devAddr, BMI160_RA_MAG_X_L, 6, buffer); + // *mx = (((int16_t)buffer[1]) << 8) | buffer[0]; + // *my = (((int16_t)buffer[3]) << 8) | buffer[2]; + // *mz = (((int16_t)buffer[5]) << 8) | buffer[4]; + *mx = ((int16_t)buffer[0] << 8) | buffer[1]; + *mz = ((int16_t)buffer[2] << 8) | buffer[3]; + *my = ((int16_t)buffer[4] << 8) | buffer[5]; +} + +void BMI160::getMagnetometerXYZBuffer(uint8_t* data) { + I2CdevMod::readBytes(devAddr, BMI160_RA_MAG_X_L, 6, data); +} + /** Read a BMI160 register directly. * @param reg register address * @return 8-bit register value @@ -2306,4 +2384,41 @@ uint8_t BMI160::getRegister(uint8_t reg) { */ void BMI160::setRegister(uint8_t reg, uint8_t data) { I2CdevMod::writeByte(devAddr, reg, data); -} \ No newline at end of file +} + +bool BMI160::getGyroDrdy() { + I2CdevMod::readBits(devAddr, BMI160_RA_STATUS, BMI160_STATUS_DRDY_GYR, 1, buffer); + return buffer[0]; +} + +void BMI160::waitForGyroDrdy() { + do { + getGyroDrdy(); + if (!buffer[0]) delayMicroseconds(150); + } while (!buffer[0]); +} + +void BMI160::waitForAccelDrdy() { + do { + I2CdevMod::readBits(devAddr, BMI160_RA_STATUS, BMI160_STATUS_DRDY_ACC, 1, buffer); + if (!buffer[0]) delayMicroseconds(150); + } while (!buffer[0]); +} + +void BMI160::waitForMagDrdy() { + do { + I2CdevMod::readBits(devAddr, BMI160_RA_STATUS, BMI160_STATUS_DRDY_MAG, 1, buffer); + if (!buffer[0]) delay(5); + } while (!buffer[0]); +} + +bool BMI160::getSensorTime(uint32_t *v_sensor_time_u32) { + bool ok = I2CdevMod::readBytes(devAddr, BMI160_RA_SENSORTIME, 3, buffer) >= 0; + if (!ok) return false; + *v_sensor_time_u32 = (uint32_t)( + (((uint32_t)buffer[2]) << 16) | + (((uint32_t)buffer[1]) << 8) | + ((uint32_t)buffer[0]) + ); + return ok; +} diff --git a/lib/bmi160/BMI160.h b/lib/bmi160/BMI160.h index 05ad7e1..dc7701a 100644 --- a/lib/bmi160/BMI160.h +++ b/lib/bmi160/BMI160.h @@ -40,6 +40,56 @@ THE SOFTWARE. #define BMI160_RA_CHIP_ID 0x00 +#define BMI160_MAG_PMU_STATUS_BIT 0 +#define BMI160_MAG_PMU_STATUS_LEN 2 + +#define BMI160_STATUS_DRDY_MAG 5 +#define BMI160_STATUS_MAG_MAN_OP 2 +#define BMI160_MAG_RATE_SEL_BIT 0 +#define BMI160_MAG_RATE_SEL_LEN 4 +#define BMI160_FIFO_MAG_EN_BIT 5 + +#define BMI160_RA_MAG_CONF 0x44 +#define BMI160_RA_MAG_IF_0_DEVADDR 0x4B +#define BMI160_RA_MAG_IF_1_MODE 0x4C +#define BMI160_RA_MAG_IF_2_READ_RA 0x4D +#define BMI160_RA_MAG_IF_3_WRITE_RA 0x4E +#define BMI160_RA_MAG_IF_4_WRITE_VALUE 0x4F +#define BMI160_RA_IF_CONF 0x6B + +#define BMI160_IF_CONF_MODE_PRI_AUTO_SEC_OFF 0 << 4 +#define BMI160_IF_CONF_MODE_PRI_I2C_SEC_OIS 1 << 4 +#define BMI160_IF_CONF_MODE_PRI_AUTO_SEC_MAG 2 << 4 + +#define BMI160_MAG_SETUP_MODE 0x80 +#define BMI160_MAG_DATA_MODE_2 0x01 +#define BMI160_MAG_DATA_MODE_6 0x02 +#define BMI160_MAG_DATA_MODE_8 0x03 + +typedef enum { + BMI160_MAG_RATE_25_32thHZ = 1, /**< 25/32 Hz */ + BMI160_MAG_RATE_25_16thHZ, /**< 25/16 Hz */ + BMI160_MAG_RATE_25_8thHZ, /**< 25/8 Hz */ + BMI160_MAG_RATE_25_4thHZ, /**< 25/4 Hz */ + BMI160_MAG_RATE_25_2thHZ, /**< 25/2 Hz */ + BMI160_MAG_RATE_25HZ, /**< 25 Hz */ + BMI160_MAG_RATE_50HZ, /**< 50 Hz */ + BMI160_MAG_RATE_100HZ, /**< 100 Hz */ + BMI160_MAG_RATE_200HZ, /**< 200 Hz */ + BMI160_MAG_RATE_400HZ, /**< 400 Hz */ + BMI160_MAG_RATE_800HZ, /**< 800 Hz */ +} BMI160MagRate; + +#define BMI160_CMD_MAG_MODE_NORMAL 0x19 + +#define BMI160_EN_PULL_UP_REG_1 0x37 +#define BMI160_EN_PULL_UP_REG_2 0x9A +#define BMI160_EN_PULL_UP_REG_3 0xC0 +#define BMI160_EN_PULL_UP_REG_4 0x90 +#define BMI160_EN_PULL_UP_REG_5 0x80 + +#define BMI160_7F 0x7F + #define BMI160_ACC_PMU_STATUS_BIT 4 #define BMI160_ACC_PMU_STATUS_LEN 2 #define BMI160_GYR_PMU_STATUS_BIT 2 @@ -47,6 +97,12 @@ THE SOFTWARE. #define BMI160_RA_PMU_STATUS 0x03 +#define BMI160_RA_MAG_X_L 0x04 +#define BMI160_RA_MAG_X_H 0x05 +#define BMI160_RA_MAG_Y_L 0x06 +#define BMI160_RA_MAG_Y_H 0x07 +#define BMI160_RA_MAG_Z_L 0x08 +#define BMI160_RA_MAG_Z_H 0x09 #define BMI160_RA_GYRO_X_L 0x0C #define BMI160_RA_GYRO_X_H 0x0D #define BMI160_RA_GYRO_Y_L 0x0E @@ -60,8 +116,11 @@ THE SOFTWARE. #define BMI160_RA_ACCEL_Z_L 0x16 #define BMI160_RA_ACCEL_Z_H 0x17 +#define BMI160_RA_SENSORTIME 0x18 + #define BMI160_STATUS_FOC_RDY 3 #define BMI160_STATUS_NVM_RDY 4 +#define BMI160_STATUS_DRDY_MAG 5 #define BMI160_STATUS_DRDY_GYR 6 #define BMI160_STATUS_DRDY_ACC 7 @@ -116,6 +175,7 @@ THE SOFTWARE. #define BMI160_RA_GYRO_RANGE 0x43 #define BMI160_FIFO_HEADER_EN_BIT 4 +#define BMI160_FIFO_MAG_EN_BIT 5 #define BMI160_FIFO_ACC_EN_BIT 6 #define BMI160_FIFO_GYR_EN_BIT 7 @@ -262,6 +322,33 @@ THE SOFTWARE. #define BMI160_RA_CMD 0x7E + // mode parm ext +#define BMI160_FIFO_HEADER_CTL_SKIP_FRAME 0x40 // 0b01 0 000 00 +#define BMI160_FIFO_HEADER_CTL_SENSOR_TIME 0x44 // 0b01 0 001 00 +#define BMI160_FIFO_HEADER_CTL_INPUT_CONFIG 0x48 // 0b01 0 010 00 +#define BMI160_FIFO_HEADER_DATA_FRAME_BASE 0x80 // 0b10 0 000 00 +#define BMI160_FIFO_HEADER_DATA_FRAME_FLAG_M 1 << 4 // 0b00 0 100 00 +#define BMI160_FIFO_HEADER_DATA_FRAME_FLAG_G 1 << 3 // 0b00 0 010 00 +#define BMI160_FIFO_HEADER_DATA_FRAME_FLAG_A 1 << 2 // 0b00 0 001 00 +#define BMI160_FIFO_HEADER_DATA_FRAME_MASK_HAS_DATA \ + (BMI160_FIFO_HEADER_DATA_FRAME_FLAG_M |\ + BMI160_FIFO_HEADER_DATA_FRAME_FLAG_G |\ + BMI160_FIFO_HEADER_DATA_FRAME_FLAG_A) + +#define BMI160_FIFO_SKIP_FRAME_LEN 1 +#define BMI160_FIFO_INPUT_CONFIG_LEN 1 +#define BMI160_FIFO_SENSOR_TIME_LEN 3 +#define BMI160_FIFO_M_LEN 8 +#define BMI160_FIFO_G_LEN 6 +#define BMI160_FIFO_A_LEN 6 + +#define BMI160_RA_ERR 0x02 +#define BMI160_ERR_MASK_MAG_DRDY_ERR 0x10000000 +#define BMI160_ERR_MASK_DROP_CMD_ERR 0x01000000 +#define BMI160_ERR_MASK_I2C_FAIL 0x00100000 +#define BMI160_ERR_MASK_ERROR_CODE 0x00011110 +#define BMI160_ERR_MASK_CHIP_NOT_OPERABLE 0x00000001 + /** * Interrupt Latch Mode options * @see setInterruptLatch() @@ -467,7 +554,15 @@ typedef enum { class BMI160 { public: BMI160(); - void initialize(uint8_t addr); + void initialize( + uint8_t addr, + BMI160GyroRate gyroRate = BMI160_GYRO_RATE_800HZ, + BMI160GyroRange gyroRange = BMI160_GYRO_RANGE_500, + BMI160DLPFMode gyroFilterMode = BMI160_DLPF_MODE_NORM, + BMI160AccelRate accelRate = BMI160_ACCEL_RATE_800HZ, + BMI160AccelRange accelRange = BMI160_ACCEL_RANGE_4G, + BMI160DLPFMode accelFilterMode = BMI160_DLPF_MODE_OSR4 + ); bool testConnection(); uint8_t getGyroRate(); @@ -573,6 +668,8 @@ class BMI160 { void setGyroFIFOEnabled(bool enabled); bool getAccelFIFOEnabled(); void setAccelFIFOEnabled(bool enabled); + bool getMagFIFOEnabled(); + void setMagFIFOEnabled(bool enabled); bool getIntFIFOBufferFullEnabled(); void setIntFIFOBufferFullEnabled(bool enabled); @@ -599,7 +696,10 @@ class BMI160 { int16_t getAccelerationY(); int16_t getAccelerationZ(); - int16_t getTemperature(); + void getMagnetometer(int16_t* mx, int16_t* my, int16_t* mz); + void getMagnetometerXYZBuffer(uint8_t* data); + + bool getTemperature(int16_t* out); void getRotation(int16_t* x, int16_t* y, int16_t* z); int16_t getRotationX(); @@ -631,8 +731,8 @@ class BMI160 { void setFIFOHeaderModeEnabled(bool enabled); void resetFIFO(); - uint16_t getFIFOCount(); - void getFIFOBytes(uint8_t *data, uint16_t length); + bool getFIFOCount(uint16_t* outCount); + bool getFIFOBytes(uint8_t *data, uint16_t length); uint8_t getDeviceID(); @@ -648,6 +748,16 @@ class BMI160 { uint8_t getInterruptLatch(); void setInterruptLatch(uint8_t latch); void resetInterrupt(); + + bool getGyroDrdy(); + void waitForGyroDrdy(); + void waitForAccelDrdy(); + void waitForMagDrdy(); + bool getSensorTime(uint32_t *v_sensor_time_u32); + void setMagDeviceAddress(uint8_t addr); + bool setMagRegister(uint8_t addr, uint8_t value); + + bool getErrReg(uint8_t* out); private: uint8_t buffer[14]; uint8_t devAddr; diff --git a/lib/magneto/madgwick.cpp b/lib/magneto/madgwick.cpp index 8a19c39..57887c6 100644 --- a/lib/magneto/madgwick.cpp +++ b/lib/magneto/madgwick.cpp @@ -1,6 +1,6 @@ #include "madgwick.h" -volatile float beta = 0.1f; +volatile float madgwickBeta = 0.1f; void madgwickQuaternionUpdate(float q[4], float ax, float ay, float az, float gx, float gy, float gz, float deltat) { @@ -51,10 +51,10 @@ void madgwickQuaternionUpdate(float q[4], float ax, float ay, float az, float gx s3 *= recipNorm; // Apply feedback step - qDot1 -= beta * s0; - qDot2 -= beta * s1; - qDot3 -= beta * s2; - qDot4 -= beta * s3; + qDot1 -= madgwickBeta * s0; + qDot2 -= madgwickBeta * s1; + qDot3 -= madgwickBeta * s2; + qDot4 -= madgwickBeta * s3; } // Integrate rate of change of quaternion to yield quaternion @@ -147,10 +147,10 @@ void madgwickQuaternionUpdate(float q[4], float ax, float ay, float az, float gx s3 *= recipNorm; // Apply feedback step - qDot1 -= beta * s0; - qDot2 -= beta * s1; - qDot3 -= beta * s2; - qDot4 -= beta * s3; + qDot1 -= madgwickBeta * s0; + qDot2 -= madgwickBeta * s1; + qDot3 -= madgwickBeta * s2; + qDot4 -= madgwickBeta * s3; } // Integrate rate of change of quaternion to yield quaternion diff --git a/lib/magneto/mahony.h b/lib/magneto/mahony.h index 76de6e5..d58193a 100644 --- a/lib/magneto/mahony.h +++ b/lib/magneto/mahony.h @@ -29,4 +29,81 @@ void mahonyQuaternionUpdate(float q[4], float ax, float ay, float az, float gx, float gy, float gz, float deltat); void mahonyQuaternionUpdate(float q[4], float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz, float deltat); +template +class Mahony { +public: + void updateInto(T q[4], T ax, T ay, T az, T gx, T gy, T gz, T deltat) + { + constexpr double Kp = 10.0; + constexpr double Ki = 0.0; + // short name local variable for readability + T q1 = q[0], q2 = q[1], q3 = q[2], q4 = q[3]; + T norm; + T vx, vy, vz; + T ex, ey, ez; //error terms + T qa, qb, qc; + + // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation) + T tmp = ax * ax + ay * ay + az * az; + if (tmp > 0.0f) + { + // Normalise accelerometer (assumed to measure the direction of gravity in body frame) + norm = invSqrt(tmp); + ax *= norm; + ay *= norm; + az *= norm; + + // Estimated direction of gravity in the body frame (factor of two divided out) + vx = q2 * q4 - q1 * q3; + vy = q1 * q2 + q3 * q4; + vz = q1 * q1 - 0.5f + q4 * q4; + + // Error is cross product between estimated and measured direction of gravity in body frame + // (half the actual magnitude) + ex = (ay * vz - az * vy); + ey = (az * vx - ax * vz); + ez = (ax * vy - ay * vx); + + // Compute and apply to gyro term the integral feedback, if enabled + if (Ki > 0.0f) { + ix += Ki * ex * deltat; // integral error scaled by Ki + iy += Ki * ey * deltat; + iz += Ki * ez * deltat; + gx += ix; // apply integral feedback + gy += iy; + gz += iz; + } + + // Apply proportional feedback to gyro term + gx += Kp * ex; + gy += Kp * ey; + gz += Kp * ez; + } + + // Integrate rate of change of quaternion, q cross gyro term + deltat *= 0.5f; + gx *= deltat; // pre-multiply common factors + gy *= deltat; + gz *= deltat; + qa = q1; + qb = q2; + qc = q3; + q1 += (-qb * gx - qc * gy - q4 * gz); + q2 += (qa * gx + qc * gz - q4 * gy); + q3 += (qa * gy - qb * gz + q4 * gx); + q4 += (qa * gz + qb * gy - qc * gx); + + // normalise quaternion + norm = invSqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4); + q[0] = q1 * norm; + q[1] = q2 * norm; + q[2] = q3 * norm; + q[3] = q4 * norm; + } +private: + T ix = 0.0; + T iy = 0.0; + T iz = 0.0; +}; + #endif /* _MAHONY_H_ */ \ No newline at end of file diff --git a/lib/magnetometers/hmc5883l.h b/lib/magnetometers/hmc5883l.h new file mode 100644 index 0000000..101e537 --- /dev/null +++ b/lib/magnetometers/hmc5883l.h @@ -0,0 +1,33 @@ +#define HMC_DEVADDR 0x1E +#define HMC_RA_CFGA 0x00 +#define HMC_RA_CFGB 0x01 +#define HMC_RA_MODE 0x02 +#define HMC_RA_DATA 0x03 + +#define HMC_CFGA_DATA_RATE_0_75 0b000 << 2 +#define HMC_CFGA_DATA_RATE_1_5 0b001 << 2 +#define HMC_CFGA_DATA_RATE_3 0b010 << 2 +#define HMC_CFGA_DATA_RATE_7_5 0b011 << 2 +#define HMC_CFGA_DATA_RATE_15 0b100 << 2 +#define HMC_CFGA_DATA_RATE_30 0b101 << 2 +#define HMC_CFGA_DATA_RATE_75 0b110 << 2 +#define HMC_CFGA_AVG_SAMPLES_1 0b00 << 5 +#define HMC_CFGA_AVG_SAMPLES_2 0b01 << 5 +#define HMC_CFGA_AVG_SAMPLES_4 0b10 << 5 +#define HMC_CFGA_AVG_SAMPLES_8 0b11 << 5 +#define HMC_CFGA_BIAS_NORMAL 0b00 +#define HMC_CFGA_BIAS_POS 0b01 +#define HMC_CFGA_BIAS_NEG 0b10 + +#define HMC_CFGB_GAIN_0_88 0 +#define HMC_CFGB_GAIN_1_30 1 << 5 +#define HMC_CFGB_GAIN_1_90 2 << 5 +#define HMC_CFGB_GAIN_2_50 3 << 5 +#define HMC_CFGB_GAIN_4_00 4 << 5 +#define HMC_CFGB_GAIN_4_70 5 << 5 +#define HMC_CFGB_GAIN_5_60 6 << 5 +#define HMC_CFGB_GAIN_8_10 7 << 5 + +#define HMC_MODE_HIGHSPEED 1 << 7 +#define HMC_MODE_READ_CONTINUOUS 0b00 +#define HMC_MODE_READ_SINGLEMEAS 0b01 diff --git a/lib/magnetometers/qmc5883l.h b/lib/magnetometers/qmc5883l.h new file mode 100644 index 0000000..01730c3 --- /dev/null +++ b/lib/magnetometers/qmc5883l.h @@ -0,0 +1,17 @@ +#define QMC_DEVADDR 0x0D +#define QMC_RA_DATA 0x00 +#define QMC_RA_CONTROL 0x09 +#define QMC_RA_RESET 0x0B + +#define QMC_CFG_MODE_STANDBY 0b00 +#define QMC_CFG_MODE_CONTINUOUS 0b01 +#define QMC_CFG_ODR_10HZ 0b00 << 2 +#define QMC_CFG_ODR_50HZ 0b01 << 2 +#define QMC_CFG_ODR_100HZ 0b10 << 2 +#define QMC_CFG_ODR_200HZ 0b11 << 2 +#define QMC_CFG_RNG_2G 0b00 << 4 +#define QMC_CFG_RNG_8G 0b01 << 4 +#define QMC_CFG_OSR_512 0b00 << 6 +#define QMC_CFG_OSR_256 0b01 << 6 +#define QMC_CFG_OSR_128 0b10 << 6 +#define QMC_CFG_OSR_64 0b11 << 6 diff --git a/lib/vqf/basicvqf.cpp b/lib/vqf/basicvqf.cpp new file mode 100644 index 0000000..443a8bc --- /dev/null +++ b/lib/vqf/basicvqf.cpp @@ -0,0 +1,407 @@ +// SPDX-FileCopyrightText: 2021 Daniel Laidig +// +// SPDX-License-Identifier: MIT + +// Modified to add timestamps in: updateGyr(const vqf_real_t gyr[3], double gyrTs) +// Removed batch update functions + +#include "basicvqf.h" + +#include +#include +#define _USE_MATH_DEFINES +#include +#include + +#define EPS std::numeric_limits::epsilon() +#define NaN std::numeric_limits::quiet_NaN() + +inline vqf_real_t square(vqf_real_t x) { return x*x; } + +BasicVQFParams::BasicVQFParams() + : tauAcc(3.0) + , tauMag(9.0) +{ + +} + +BasicVQF::BasicVQF(vqf_real_t gyrTs, vqf_real_t accTs, vqf_real_t magTs) +{ + coeffs.gyrTs = gyrTs; + coeffs.accTs = accTs > 0 ? accTs : gyrTs; + coeffs.magTs = magTs > 0 ? magTs : gyrTs; + + setup(); +} + +BasicVQF::BasicVQF(const BasicVQFParams ¶ms, vqf_real_t gyrTs, vqf_real_t accTs, vqf_real_t magTs) +{ + this->params = params; + + coeffs.gyrTs = gyrTs; + coeffs.accTs = accTs > 0 ? accTs : gyrTs; + coeffs.magTs = magTs > 0 ? magTs : gyrTs; + + setup(); +} + +void BasicVQF::updateGyr(const vqf_real_t gyr[3], double gyrTs) +{ + // gyroscope prediction step + vqf_real_t gyrNorm = norm(gyr, 3); + vqf_real_t angle = gyrNorm * gyrTs; + if (gyrNorm > EPS) { + vqf_real_t c = cos(angle/2); + vqf_real_t s = sin(angle/2)/gyrNorm; + vqf_real_t gyrStepQuat[4] = {c, s*gyr[0], s*gyr[1], s*gyr[2]}; + quatMultiply(state.gyrQuat, gyrStepQuat, state.gyrQuat); + normalize(state.gyrQuat, 4); + } +} + +void BasicVQF::updateAcc(const vqf_real_t acc[3]) +{ + // ignore [0 0 0] samples + if (acc[0] == vqf_real_t(0.0) && acc[1] == vqf_real_t(0.0) && acc[2] == vqf_real_t(0.0)) { + return; + } + + vqf_real_t accEarth[3]; + + // filter acc in inertial frame + quatRotate(state.gyrQuat, acc, accEarth); + filterVec(accEarth, 3, params.tauAcc, coeffs.accTs, coeffs.accLpB, coeffs.accLpA, state.accLpState, state.lastAccLp); + + // transform to 6D earth frame and normalize + quatRotate(state.accQuat, state.lastAccLp, accEarth); + normalize(accEarth, 3); + + // inclination correction + vqf_real_t accCorrQuat[4]; + vqf_real_t q_w = sqrt((accEarth[2]+1)/2); + if (q_w > 1e-6) { + accCorrQuat[0] = q_w; + accCorrQuat[1] = 0.5*accEarth[1]/q_w; + accCorrQuat[2] = -0.5*accEarth[0]/q_w; + accCorrQuat[3] = 0; + } else { + // to avoid numeric issues when acc is close to [0 0 -1], i.e. the correction step is close (<= 0.00011°) to 180°: + accCorrQuat[0] = 0; + accCorrQuat[1] = 1; + accCorrQuat[2] = 0; + accCorrQuat[3] = 0; + } + quatMultiply(accCorrQuat, state.accQuat, state.accQuat); + normalize(state.accQuat, 4); +} + +void BasicVQF::updateMag(const vqf_real_t mag[3]) +{ + // ignore [0 0 0] samples + if (mag[0] == vqf_real_t(0.0) && mag[1] == vqf_real_t(0.0) && mag[2] == vqf_real_t(0.0)) { + return; + } + + vqf_real_t magEarth[3]; + + // bring magnetometer measurement into 6D earth frame + vqf_real_t accGyrQuat[4]; + getQuat6D(accGyrQuat); + quatRotate(accGyrQuat, mag, magEarth); + + // calculate disagreement angle based on current magnetometer measurement + vqf_real_t magDisAngle = atan2(magEarth[0], magEarth[1]) - state.delta; + + // make sure the disagreement angle is in the range [-pi, pi] + if (magDisAngle > vqf_real_t(M_PI)) { + magDisAngle -= vqf_real_t(2*M_PI); + } else if (magDisAngle < vqf_real_t(-M_PI)) { + magDisAngle += vqf_real_t(2*M_PI); + } + + vqf_real_t k = coeffs.kMag; + + // ensure fast initial convergence + if (state.kMagInit != vqf_real_t(0.0)) { + // make sure that the gain k is at least 1/N, N=1,2,3,... in the first few samples + if (k < state.kMagInit) { + k = state.kMagInit; + } + + // iterative expression to calculate 1/N + state.kMagInit = state.kMagInit/(state.kMagInit+1); + + // disable if t > tauMag + if (state.kMagInit*params.tauMag < coeffs.magTs) { + state.kMagInit = 0.0; + } + } + + // first-order filter step + state.delta += k*magDisAngle; + + // make sure delta is in the range [-pi, pi] + if (state.delta > vqf_real_t(M_PI)) { + state.delta -= vqf_real_t(2*M_PI); + } else if (state.delta < vqf_real_t(-M_PI)) { + state.delta += vqf_real_t(2*M_PI); + } +} + +void BasicVQF::getQuat3D(vqf_real_t out[4]) const +{ + std::copy(state.gyrQuat, state.gyrQuat+4, out); +} + +void BasicVQF::getQuat6D(vqf_real_t out[4]) const +{ + quatMultiply(state.accQuat, state.gyrQuat, out); +} + +void BasicVQF::getQuat9D(vqf_real_t out[4]) const +{ + quatMultiply(state.accQuat, state.gyrQuat, out); + quatApplyDelta(out, state.delta, out); +} + +vqf_real_t BasicVQF::getDelta() const +{ + return state.delta; +} + +void BasicVQF::setTauAcc(vqf_real_t tauAcc) +{ + if (params.tauAcc == tauAcc) { + return; + } + params.tauAcc = tauAcc; + double newB[3]; + double newA[3]; + + filterCoeffs(params.tauAcc, coeffs.accTs, newB, newA); + filterAdaptStateForCoeffChange(state.lastAccLp, 3, coeffs.accLpB, coeffs.accLpA, newB, newA, state.accLpState); + + std::copy(newB, newB+3, coeffs.accLpB); + std::copy(newA, newA+2, coeffs.accLpA); +} + +void BasicVQF::setTauMag(vqf_real_t tauMag) +{ + params.tauMag = tauMag; + coeffs.kMag = gainFromTau(params.tauMag, coeffs.magTs); +} + +const BasicVQFParams& BasicVQF::getParams() const +{ + return params; +} + +const BasicVQFCoefficients& BasicVQF::getCoeffs() const +{ + return coeffs; +} + +const BasicVQFState& BasicVQF::getState() const +{ + return state; +} + +void BasicVQF::setState(const BasicVQFState& state) +{ + this->state = state; +} + +void BasicVQF::resetState() +{ + quatSetToIdentity(state.gyrQuat); + quatSetToIdentity(state.accQuat); + state.delta = 0.0; + + std::fill(state.lastAccLp, state.lastAccLp+3, 0); + std::fill(state.accLpState, state.accLpState + 3*2, NaN); + + state.kMagInit = 1.0; +} + +void BasicVQF::quatMultiply(const vqf_real_t q1[4], const vqf_real_t q2[4], vqf_real_t out[4]) +{ + vqf_real_t w = q1[0] * q2[0] - q1[1] * q2[1] - q1[2] * q2[2] - q1[3] * q2[3]; + vqf_real_t x = q1[0] * q2[1] + q1[1] * q2[0] + q1[2] * q2[3] - q1[3] * q2[2]; + vqf_real_t y = q1[0] * q2[2] - q1[1] * q2[3] + q1[2] * q2[0] + q1[3] * q2[1]; + vqf_real_t z = q1[0] * q2[3] + q1[1] * q2[2] - q1[2] * q2[1] + q1[3] * q2[0]; + out[0] = w; out[1] = x; out[2] = y; out[3] = z; +} + +void BasicVQF::quatConj(const vqf_real_t q[4], vqf_real_t out[4]) +{ + vqf_real_t w = q[0]; + vqf_real_t x = -q[1]; + vqf_real_t y = -q[2]; + vqf_real_t z = -q[3]; + out[0] = w; out[1] = x; out[2] = y; out[3] = z; +} + + +void BasicVQF::quatSetToIdentity(vqf_real_t out[4]) +{ + out[0] = 1; + out[1] = 0; + out[2] = 0; + out[3] = 0; +} + +void BasicVQF::quatApplyDelta(vqf_real_t q[], vqf_real_t delta, vqf_real_t out[]) +{ + // out = quatMultiply([cos(delta/2), 0, 0, sin(delta/2)], q) + vqf_real_t c = cos(delta/2); + vqf_real_t s = sin(delta/2); + vqf_real_t w = c * q[0] - s * q[3]; + vqf_real_t x = c * q[1] - s * q[2]; + vqf_real_t y = c * q[2] + s * q[1]; + vqf_real_t z = c * q[3] + s * q[0]; + out[0] = w; out[1] = x; out[2] = y; out[3] = z; +} + +void BasicVQF::quatRotate(const vqf_real_t q[4], const vqf_real_t v[3], vqf_real_t out[3]) +{ + vqf_real_t x = (1 - 2*q[2]*q[2] - 2*q[3]*q[3])*v[0] + 2*v[1]*(q[2]*q[1] - q[0]*q[3]) + 2*v[2]*(q[0]*q[2] + q[3]*q[1]); + vqf_real_t y = 2*v[0]*(q[0]*q[3] + q[2]*q[1]) + v[1]*(1 - 2*q[1]*q[1] - 2*q[3]*q[3]) + 2*v[2]*(q[2]*q[3] - q[1]*q[0]); + vqf_real_t z = 2*v[0]*(q[3]*q[1] - q[0]*q[2]) + 2*v[1]*(q[0]*q[1] + q[3]*q[2]) + v[2]*(1 - 2*q[1]*q[1] - 2*q[2]*q[2]); + out[0] = x; out[1] = y; out[2] = z; +} + +vqf_real_t BasicVQF::norm(const vqf_real_t vec[], size_t N) +{ + vqf_real_t s = 0; + for(size_t i = 0; i < N; i++) { + s += vec[i]*vec[i]; + } + return sqrt(s); +} + +void BasicVQF::normalize(vqf_real_t vec[], size_t N) +{ + vqf_real_t n = norm(vec, N); + if (n < EPS) { + return; + } + for(size_t i = 0; i < N; i++) { + vec[i] /= n; + } +} + +void BasicVQF::clip(vqf_real_t vec[], size_t N, vqf_real_t min, vqf_real_t max) +{ + for(size_t i = 0; i < N; i++) { + if (vec[i] < min) { + vec[i] = min; + } else if (vec[i] > max) { + vec[i] = max; + } + } +} + +vqf_real_t BasicVQF::gainFromTau(vqf_real_t tau, vqf_real_t Ts) +{ + assert(Ts > 0); + if (tau < vqf_real_t(0.0)) { + return 0; // k=0 for negative tau (disable update) + } else if (tau == vqf_real_t(0.0)) { + return 1; // k=1 for tau=0 + } else { + return 1 - exp(-Ts/tau); // fc = 1/(2*pi*tau) + } +} + +void BasicVQF::filterCoeffs(vqf_real_t tau, vqf_real_t Ts, double outB[], double outA[]) +{ + assert(tau > 0); + assert(Ts > 0); + // second order Butterworth filter based on https://stackoverflow.com/a/52764064 + double fc = (M_SQRT2 / (2.0*M_PI))/double(tau); // time constant of dampened, non-oscillating part of step response + double C = tan(M_PI*fc*double(Ts)); + double D = C*C + sqrt(2)*C + 1; + double b0 = C*C/D; + outB[0] = b0; + outB[1] = 2*b0; + outB[2] = b0; + // a0 = 1.0 + outA[0] = 2*(C*C-1)/D; // a1 + outA[1] = (1-sqrt(2)*C+C*C)/D; // a2 +} + +void BasicVQF::filterInitialState(vqf_real_t x0, const double b[3], const double a[2], double out[]) +{ + // initial state for steady state (equivalent to scipy.signal.lfilter_zi, obtained by setting y=x=x0 in the filter + // update equation) + out[0] = x0*(1 - b[0]); + out[1] = x0*(b[2] - a[1]); +} + +void BasicVQF::filterAdaptStateForCoeffChange(vqf_real_t last_y[], size_t N, const double b_old[], + const double a_old[], const double b_new[], + const double a_new[], double state[]) +{ + if (isnan(state[0])) { + return; + } + for (size_t i = 0; i < N; i++) { + state[0+2*i] = state[0+2*i] + (b_old[0] - b_new[0])*last_y[i]; + state[1+2*i] = state[1+2*i] + (b_old[1] - b_new[1] - a_old[0] + a_new[0])*last_y[i]; + } +} + +vqf_real_t BasicVQF::filterStep(vqf_real_t x, const double b[3], const double a[2], double state[2]) +{ + // difference equations based on scipy.signal.lfilter documentation + // assumes that a0 == 1.0 + double y = b[0]*x + state[0]; + state[0] = b[1]*x - a[0]*y + state[1]; + state[1] = b[2]*x - a[1]*y; + return y; +} + +void BasicVQF::filterVec(const vqf_real_t x[], size_t N, vqf_real_t tau, vqf_real_t Ts, const double b[3], + const double a[2], double state[], vqf_real_t out[]) +{ + assert(N>=2); + + // to avoid depending on a single sample, average the first samples (for duration tau) + // and then use this average to calculate the filter initial state + if (isnan(state[0])) { // initialization phase + if (isnan(state[1])) { // first sample + state[1] = 0; // state[1] is used to store the sample count + for(size_t i = 0; i < N; i++) { + state[2+i] = 0; // state[2+i] is used to store the sum + } + } + state[1]++; + for (size_t i = 0; i < N; i++) { + state[2+i] += x[i]; + out[i] = state[2+i]/state[1]; + } + if (state[1]*Ts >= tau) { + for(size_t i = 0; i < N; i++) { + filterInitialState(out[i], b, a, state+2*i); + } + } + return; + } + + for (size_t i = 0; i < N; i++) { + out[i] = filterStep(x[i], b, a, state+2*i); + } +} + +void BasicVQF::setup() +{ + assert(coeffs.gyrTs > 0); + assert(coeffs.accTs > 0); + assert(coeffs.magTs > 0); + + filterCoeffs(params.tauAcc, coeffs.accTs, coeffs.accLpB, coeffs.accLpA); + + coeffs.kMag = gainFromTau(params.tauMag, coeffs.magTs); + + resetState(); +} diff --git a/lib/vqf/basicvqf.h b/lib/vqf/basicvqf.h new file mode 100644 index 0000000..db96f51 --- /dev/null +++ b/lib/vqf/basicvqf.h @@ -0,0 +1,502 @@ +// SPDX-FileCopyrightText: 2021 Daniel Laidig +// +// SPDX-License-Identifier: MIT + +// Modified to add timestamps in: updateGyr(const vqf_real_t gyr[3], double gyrTs) +// Removed batch update functions + +#ifndef BASICVQF_HPP +#define BASICVQF_HPP + +#include + +#define VQF_SINGLE_PRECISION +#define M_PI 3.14159265358979323846 +#define M_SQRT2 1.41421356237309504880 + +/** + * @brief Typedef for the floating-point data type used for most operations. + * + * By default, all floating-point calculations are performed using `double`. Set the `VQF_SINGLE_PRECISION` define to + * change this type to `float`. Note that the Butterworth filter implementation will always use double precision as + * using floats can cause numeric issues. + */ +#ifndef VQF_SINGLE_PRECISION +typedef double vqf_real_t; +#else +typedef float vqf_real_t; +#endif + +/** + * @brief Struct containing all tuning parameters used by the BasicVQF class. + * + * The parameters influence the behavior of the algorithm and are independent of the sampling rate of the IMU data. The + * constructor sets all parameters to the default values. + * + * The basic version of this algoirthm only has two parameters: The time constants #tauAcc and #tauMag can be tuned to + * change the trust on the accelerometer and magnetometer measurements, respectively. + */ +struct BasicVQFParams +{ + /** + * @brief Constructor that initializes the struct with the default parameters. + */ + BasicVQFParams(); + + /** + * @brief Time constant \f$\tau_\mathrm{acc}\f$ for accelerometer low-pass filtering in seconds. + * + * Small values for \f$\tau_\mathrm{acc}\f$ imply trust on the accelerometer measurements and while large values of + * \f$\tau_\mathrm{acc}\f$ imply trust on the gyroscope measurements. + * + * The time constant \f$\tau_\mathrm{acc}\f$ corresponds to the cutoff frequency \f$f_\mathrm{c}\f$ of the + * second-order Butterworth low-pass filter as follows: \f$f_\mathrm{c} = \frac{\sqrt{2}}{2\pi\tau_\mathrm{acc}}\f$. + * + * Default value: 3.0 s + */ + vqf_real_t tauAcc; + /** + * @brief Time constant \f$\tau_\mathrm{mag}\f$ for magnetometer update in seconds. + * + * Small values for \f$\tau_\mathrm{mag}\f$ imply trust on the magnetometer measurements and while large values of + * \f$\tau_\mathrm{mag}\f$ imply trust on the gyroscope measurements. + * + * The time constant \f$\tau_\mathrm{mag}\f$ corresponds to the cutoff frequency \f$f_\mathrm{c}\f$ of the + * first-order low-pass filter for the heading correction as follows: + * \f$f_\mathrm{c} = \frac{1}{2\pi\tau_\mathrm{mag}}\f$. + * + * Default value: 9.0 s + */ + vqf_real_t tauMag; +}; + +/** + * @brief Struct containing the filter state of the BasicVQF class. + * + * The relevant parts of the state can be accessed via functions of the BasicVVQF class, e.g. BasicVQF::getQuat6D() + * and BasicVQF::getQuat9D(). To reset the state to the initial values, use VQF::resetState(). + * + * Direct access to the full state is typically not needed but can be useful in some cases, e.g. for debugging. For this + * purpose, the state can be accessed by BasicVQF::getState() and set by BasicVQF::setState(). + */ +struct BasicVQFState { + /** + * @brief Angular velocity strapdown integration quaternion \f$^{\mathcal{S}_i}_{\mathcal{I}_i}\mathbf{q}\f$. + */ + vqf_real_t gyrQuat[4]; + /** + * @brief Inclination correction quaternion \f$^{\mathcal{I}_i}_{\mathcal{E}_i}\mathbf{q}\f$. + */ + vqf_real_t accQuat[4]; + /** + * @brief Heading difference \f$\delta\f$ between \f$\mathcal{E}_i\f$ and \f$\mathcal{E}\f$. + * + * \f$^{\mathcal{E}_i}_{\mathcal{E}}\mathbf{q} = \begin{bmatrix}\cos\frac{\delta}{2} & 0 & 0 & + * \sin\frac{\delta}{2}\end{bmatrix}^T\f$. + */ + vqf_real_t delta; + + /** + * @brief Last low-pass filtered acceleration in the \f$\mathcal{I}_i\f$ frame. + */ + vqf_real_t lastAccLp[3]; + /** + * @brief Internal low-pass filter state for #lastAccLp. + */ + double accLpState[3*2]; + + /** + * @brief Gain used for heading correction to ensure fast initial convergence. + * + * This value is used as the gain for heading correction in the beginning if it is larger than the normal filter + * gain. It is initialized to 1 and then updated to 0.5, 0.33, 0.25, ... After VQFParams::tauMag seconds, it is + * set to zero. + */ + vqf_real_t kMagInit; +}; + +/** + * @brief Struct containing coefficients used by the BasicVQF class. + * + * Coefficients are values that depend on the parameters and the sampling times, but do not change during update steps. + * They are calculated in BasicVQF::setup(). + */ +struct BasicVQFCoefficients +{ + /** + * @brief Sampling time of the gyroscope measurements (in seconds). + */ + vqf_real_t gyrTs; + /** + * @brief Sampling time of the accelerometer measurements (in seconds). + */ + vqf_real_t accTs; + /** + * @brief Sampling time of the magnetometer measurements (in seconds). + */ + vqf_real_t magTs; + + /** + * @brief Numerator coefficients of the acceleration low-pass filter. + * + * The array contains \f$\begin{bmatrix}b_0 & b_1 & b_2\end{bmatrix}\f$. + */ + double accLpB[3]; + /** + * @brief Denominator coefficients of the acceleration low-pass filter. + * + * The array contains \f$\begin{bmatrix}a_1 & a_2\end{bmatrix}\f$ and \f$a_0=1\f$. + */ + double accLpA[2]; + + /** + * @brief Gain of the first-order filter used for heading correction. + */ + vqf_real_t kMag; +}; + +/** + * @brief A Versatile Quaternion-based Filter for IMU Orientation Estimation. + * + * \rst + * This class implements the basic version of the orientation estimation filter described in the following publication: + * + * + * D. Laidig and T. Seel. "VQF: Highly Accurate IMU Orientation Estimation with Bias Estimation and Magnetic + * Disturbance Rejection." Information Fusion 2023, 91, 187--204. + * `doi:10.1016/j.inffus.2022.10.014 `_. + * [Accepted manuscript available at `arXiv:2203.17024 `_.] + * + * The filter can perform simultaneous 6D (magnetometer-free) and 9D (gyr+acc+mag) sensor fusion and can also be used + * without magnetometer data. Different sampling rates for gyroscopes, accelerometers and magnetometers are + * supported as well. While in most cases, the defaults will be reasonable, the algorithm can be influenced via two + * tuning parameters. + * + * To use this C++ implementation, + * + * 1. create a instance of the class and provide the sampling time and, optionally, parameters + * 2. for every sample, call one of the update functions to feed the algorithm with IMU data + * 3. access the estimation results with :meth:`getQuat6D() `, :meth:`getQuat9D() ` and + * the other getter methods. + * + * If the full data is available in (row-major) data buffers, you can use :meth:`updateBatch() `. + * + * This class is the C++ implementation of the basic algorithm version. This version does not include rest detection, + * gyroscope bias estimation, and magnetic disturbance detection and rejection. It is equivalent to the full version + * when the parameters :cpp:member:`VQFParams::motionBiasEstEnabled`, :cpp:member:`VQFParams::restBiasEstEnabled` and + * :cpp:member:`VQFParams::magDistRejectionEnabled` are set to false. + * Depending on use case and programming language of choice, the following alternatives might be useful: + * + * +------------------------+--------------------------+---------------------------+---------------------------+ + * | | Full Version | Basic Version | Offline Version | + * | | | | | + * +========================+==========================+===========================+===========================+ + * | **C++** | :cpp:class:`VQF` | **BasicVQF (this class)** | :cpp:func:`offlineVQF` | + * +------------------------+--------------------------+---------------------------+---------------------------+ + * | **Python/C++ (fast)** | :py:class:`vqf.VQF` | :py:class:`vqf.BasicVQF` | :py:meth:`vqf.offlineVQF` | + * +------------------------+--------------------------+---------------------------+---------------------------+ + * | **Pure Python (slow)** | :py:class:`vqf.PyVQF` | -- | -- | + * +------------------------+--------------------------+---------------------------+---------------------------+ + * | **Pure Matlab (slow)** | :mat:class:`VQF.m ` | -- | -- | + * +------------------------+--------------------------+---------------------------+---------------------------+ + * \endrst + */ + +class BasicVQF +{ +public: + /** + * Initializes the object with default parameters. + * + * In the most common case (using the default parameters and all data being sampled with the same frequency, + * create the class like this: + * \rst + * .. code-block:: c++ + * + * BasicVQF vqf(0.01); // 0.01 s sampling time, i.e. 100 Hz + * \endrst + * + * @param gyrTs sampling time of the gyroscope measurements in seconds + * @param accTs sampling time of the accelerometer measurements in seconds (the value of `gyrTs` is used if set to -1) + * @param magTs sampling time of the magnetometer measurements in seconds (the value of `gyrTs` is used if set to -1) + * + */ + BasicVQF(vqf_real_t gyrTs, vqf_real_t accTs=-1.0, vqf_real_t magTs=-1.0); + /** + * @brief Initializes the object with custom parameters. + * + * Example code to create an object with a different value for tauAcc: + * \rst + * .. code-block:: c++ + * + * BasicVQFParams params; + * params.tauAcc = 1.0; + * BasicVQF vqf(0.01); // 0.01 s sampling time, i.e. 100 Hz + * \endrst + * + * @param params BasicVQFParams struct containing the desired parameters + * @param gyrTs sampling time of the gyroscope measurements in seconds + * @param accTs sampling time of the accelerometer measurements in seconds (the value of `gyrTs` is used if set to -1) + * @param magTs sampling time of the magnetometer measurements in seconds (the value of `gyrTs` is used if set to -1) + */ + BasicVQF(const BasicVQFParams& params, vqf_real_t gyrTs, vqf_real_t accTs=-1.0, vqf_real_t magTs=-1.0); + + /** + * @brief Performs gyroscope update step. + * + * It is only necessary to call this function directly if gyroscope, accelerometers and magnetometers have + * different sampling rates. Otherwise, simply use #update(). + * + * @param gyr gyroscope measurement in rad/s + */ + void updateGyr(const vqf_real_t gyr[3], double gyrTs); + /** + * @brief Performs accelerometer update step. + * + * It is only necessary to call this function directly if gyroscope, accelerometers and magnetometers have + * different sampling rates. Otherwise, simply use #update(). + * + * Should be called after #updateGyr and before #updateMag. + * + * @param acc accelerometer measurement in m/s² + */ + void updateAcc(const vqf_real_t acc[3]); + /** + * @brief Performs magnetometer update step. + * + * It is only necessary to call this function directly if gyroscope, accelerometers and magnetometers have + * different sampling rates. Otherwise, simply use #update(). + * + * Should be called after #updateAcc. + * + * @param mag magnetometer measurement in arbitrary units + */ + void updateMag(const vqf_real_t mag[3]); + + /** + * @brief Returns the angular velocity strapdown integration quaternion + * \f$^{\mathcal{S}_i}_{\mathcal{I}_i}\mathbf{q}\f$. + * @param out output array for the quaternion + */ + void getQuat3D(vqf_real_t out[4]) const; + /** + * @brief Returns the 6D (magnetometer-free) orientation quaternion + * \f$^{\mathcal{S}_i}_{\mathcal{E}_i}\mathbf{q}\f$. + * @param out output array for the quaternion + */ + void getQuat6D(vqf_real_t out[4]) const; + /** + * @brief Returns the 9D (with magnetometers) orientation quaternion + * \f$^{\mathcal{S}_i}_{\mathcal{E}}\mathbf{q}\f$. + * @param out output array for the quaternion + */ + void getQuat9D(vqf_real_t out[4]) const; + /** + * @brief Returns the heading difference \f$\delta\f$ between \f$\mathcal{E}_i\f$ and \f$\mathcal{E}\f$. + * + * \f$^{\mathcal{E}_i}_{\mathcal{E}}\mathbf{q} = \begin{bmatrix}\cos\frac{\delta}{2} & 0 & 0 & + * \sin\frac{\delta}{2}\end{bmatrix}^T\f$. + * + * @return delta angle in rad (VQFState::delta) + */ + vqf_real_t getDelta() const; + + /** + * @brief Sets the time constant for accelerometer low-pass filtering. + * + * For more details, see VQFParams.tauAcc. + * + * @param tauAcc time constant \f$\tau_\mathrm{acc}\f$ in seconds + */ + void setTauAcc(vqf_real_t tauAcc); + /** + * @brief Sets the time constant for the magnetometer update. + * + * For more details, see VQFParams.tauMag. + * + * @param tauMag time constant \f$\tau_\mathrm{mag}\f$ in seconds + */ + void setTauMag(vqf_real_t tauMag); + + /** + * @brief Returns the current parameters. + */ + const BasicVQFParams& getParams() const; + /** + * @brief Returns the coefficients used by the algorithm. + */ + const BasicVQFCoefficients& getCoeffs() const; + /** + * @brief Returns the current state. + */ + const BasicVQFState& getState() const; + /** + * @brief Overwrites the current state. + * + * This method allows to set a completely arbitrary filter state and is intended for debugging purposes. In + * combination with #getState, individual elements of the state can be modified. + * + * @param state A BasicVQFState struct containing the new state + */ + void setState(const BasicVQFState& state); + /** + * @brief Resets the state to the default values at initialization. + * + * Resetting the state is equivalent to creating a new instance of this class. + */ + void resetState(); + + /** + * @brief Performs quaternion multiplication (\f$\mathbf{q}_\mathrm{out} = \mathbf{q}_1 \otimes \mathbf{q}_2\f$). + */ + static void quatMultiply(const vqf_real_t q1[4], const vqf_real_t q2[4], vqf_real_t out[4]); + /** + * @brief Calculates the quaternion conjugate (\f$\mathbf{q}_\mathrm{out} = \mathbf{q}^*\f$). + */ + static void quatConj(const vqf_real_t q[4], vqf_real_t out[4]); + /** + * @brief Sets the output quaternion to the identity quaternion (\f$\mathbf{q}_\mathrm{out} = + * \begin{bmatrix}1 & 0 & 0 & 0\end{bmatrix}\f$). + */ + static void quatSetToIdentity(vqf_real_t out[4]); + /** + * @brief Applies a heading rotation by the angle delta (in rad) to a quaternion. + * + * \f$\mathbf{q}_\mathrm{out} = \begin{bmatrix}\cos\frac{\delta}{2} & 0 & 0 & + * \sin\frac{\delta}{2}\end{bmatrix} \otimes \mathbf{q}\f$ + */ + static void quatApplyDelta(vqf_real_t q[4], vqf_real_t delta, vqf_real_t out[4]); + /** + * @brief Rotates a vector with a given quaternion. + * + * \f$\begin{bmatrix}0 & \mathbf{v}_\mathrm{out}\end{bmatrix} = + * \mathbf{q} \otimes \begin{bmatrix}0 & \mathbf{v}\end{bmatrix} \otimes \mathbf{q}^*\f$ + */ + static void quatRotate(const vqf_real_t q[4], const vqf_real_t v[3], vqf_real_t out[3]); + /** + * @brief Calculates the Euclidean norm of a vector. + * @param vec pointer to an array of N elements + * @param N number of elements + */ + static vqf_real_t norm(const vqf_real_t vec[], size_t N); + /** + * @brief Normalizes a vector in-place. + * @param vec pointer to an array of N elements that will be normalized + * @param N number of elements + */ + static void normalize(vqf_real_t vec[], size_t N); + /** + * @brief Clips a vector in-place. + * @param vec pointer to an array of N elements that will be clipped + * @param N number of elements + * @param min smallest allowed value + * @param max largest allowed value + */ + static void clip(vqf_real_t vec[], size_t N, vqf_real_t min, vqf_real_t max); + /** + * @brief Calculates the gain for a first-order low-pass filter from the 1/e time constant. + * + * \f$k = 1 - \exp\left(-\frac{T_\mathrm{s}}{\tau}\right)\f$ + * + * The cutoff frequency of the resulting filter is \f$f_\mathrm{c} = \frac{1}{2\pi\tau}\f$. + * + * @param tau time constant \f$\tau\f$ in seconds - use -1 to disable update (\f$k=0\f$) or 0 to obtain + * unfiltered values (\f$k=1\f$) + * @param Ts sampling time \f$T_\mathrm{s}\f$ in seconds + * @return filter gain *k* + */ + static vqf_real_t gainFromTau(vqf_real_t tau, vqf_real_t Ts); + /** + * @brief Calculates coefficients for a second-order Butterworth low-pass filter. + * + * The filter is parametrized via the time constant of the dampened, non-oscillating part of step response and the + * resulting cutoff frequency is \f$f_\mathrm{c} = \frac{\sqrt{2}}{2\pi\tau}\f$. + * + * @param tau time constant \f$\tau\f$ in seconds + * @param Ts sampling time \f$T_\mathrm{s}\f$ in seconds + * @param outB output array for numerator coefficients + * @param outA output array for denominator coefficients (without \f$a_0=1\f$) + */ + static void filterCoeffs(vqf_real_t tau, vqf_real_t Ts, double outB[3], double outA[2]); + /** + * @brief Calculates the initial filter state for a given steady-state value. + * @param x0 steady state value + * @param b numerator coefficients + * @param a denominator coefficients (without \f$a_0=1\f$) + * @param out output array for filter state + */ + static void filterInitialState(vqf_real_t x0, const double b[], const double a[], double out[2]); + /** + * @brief Adjusts the filter state when changing coefficients. + * + * This function assumes that the filter is currently in a steady state, i.e. the last input values and the last + * output values are all equal. Based on this, the filter state is adjusted to new filter coefficients so that the + * output does not jump. + * + * @param last_y last filter output values (array of size N) + * @param N number of values in vector-valued signal + * @param b_old previous numerator coefficients + * @param a_old previous denominator coefficients (without \f$a_0=1\f$) + * @param b_new new numerator coefficients + * @param a_new new denominator coefficients (without \f$a_0=1\f$) + * @param state filter state (array of size N*2, will be modified) + */ + static void filterAdaptStateForCoeffChange(vqf_real_t last_y[], size_t N, const double b_old[3], + const double a_old[2], const double b_new[3], + const double a_new[2], double state[]); + /** + * @brief Performs a filter step for a scalar value. + * @param x input value + * @param b numerator coefficients + * @param a denominator coefficients (without \f$a_0=1\f$) + * @param state filter state array (will be modified) + * @return filtered value + */ + static vqf_real_t filterStep(vqf_real_t x, const double b[3], const double a[2], double state[2]); + /** + * @brief Performs filter step for vector-valued signal with averaging-based initialization. + * + * During the first \f$\tau\f$ seconds, the filter output is the mean of the previous samples. At \f$t=\tau\f$, the + * initial conditions for the low-pass filter are calculated based on the current mean value and from then on, + * regular filtering with the rational transfer function described by the coefficients b and a is performed. + * + * @param x input values (array of size N) + * @param N number of values in vector-valued signal + * @param tau filter time constant \f$\tau\f$ in seconds (used for initialization) + * @param Ts sampling time \f$T_\mathrm{s}\f$ in seconds (used for initialization) + * @param b numerator coefficients + * @param a denominator coefficients (without \f$a_0=1\f$) + * @param state filter state (array of size N*2, will be modified) + * @param out output array for filtered values (size N) + */ + static void filterVec(const vqf_real_t x[], size_t N, vqf_real_t tau, vqf_real_t Ts, const double b[3], + const double a[2], double state[], vqf_real_t out[]); + +protected: + /** + * @brief Calculates coefficients based on parameters and sampling rates. + */ + void setup(); + + /** + * @brief Contains the current parameters. + * + * See #getParams. To set parameters, pass them to the constructor. The parameters can be changed with + * #setTauAcc and #setTauMag. + */ + BasicVQFParams params; + /** + * @brief Contains the current state. + * + * See #getState, #getState and #resetState. + */ + BasicVQFState state; + /** + * @brief Contains the current coefficients (calculated in #setup). + * + * See #getCoeffs. + */ + BasicVQFCoefficients coeffs; +}; + +#endif // BASICVQF_HPP diff --git a/lib/vqf/vqf.cpp b/lib/vqf/vqf.cpp new file mode 100644 index 0000000..3ecd9bd --- /dev/null +++ b/lib/vqf/vqf.cpp @@ -0,0 +1,943 @@ +// SPDX-FileCopyrightText: 2021 Daniel Laidig +// +// SPDX-License-Identifier: MIT + +// Modified to add timestamps in: updateGyr(const vqf_real_t gyr[3], double gyrTs) +// Removed batch update functions + +#include "vqf.h" + +#include +#include +#define _USE_MATH_DEFINES +#include +#include + +#define EPS std::numeric_limits::epsilon() +#define NaN std::numeric_limits::quiet_NaN() + +inline vqf_real_t square(vqf_real_t x) { return x*x; } + + +VQFParams::VQFParams() + : tauAcc(3.0) + , tauMag(9.0) +#ifndef VQF_NO_MOTION_BIAS_ESTIMATION + , motionBiasEstEnabled(true) +#endif + , restBiasEstEnabled(true) + , magDistRejectionEnabled(true) + , biasSigmaInit(0.5) + , biasForgettingTime(100.0) + , biasClip(2.0) +#ifndef VQF_NO_MOTION_BIAS_ESTIMATION + , biasSigmaMotion(0.1) + , biasVerticalForgettingFactor(0.0001) +#endif + , biasSigmaRest(0.03) + , restMinT(1.5) + , restFilterTau(0.5) + , restThGyr(2.0) + , restThAcc(0.5) + , magCurrentTau(0.05) + , magRefTau(20.0) + , magNormTh(0.1) + , magDipTh(10.0) + , magNewTime(20.0) + , magNewFirstTime(5.0) + , magNewMinGyr(20.0) + , magMinUndisturbedTime(0.5) + , magMaxRejectionTime(60.0) + , magRejectionFactor(2.0) +{ + +} + +VQF::VQF(vqf_real_t gyrTs, vqf_real_t accTs, vqf_real_t magTs) +{ + coeffs.gyrTs = gyrTs; + coeffs.accTs = accTs > 0 ? accTs : gyrTs; + coeffs.magTs = magTs > 0 ? magTs : gyrTs; + + setup(); +} + +VQF::VQF(const VQFParams ¶ms, vqf_real_t gyrTs, vqf_real_t accTs, vqf_real_t magTs) +{ + this->params = params; + + coeffs.gyrTs = gyrTs; + coeffs.accTs = accTs > 0 ? accTs : gyrTs; + coeffs.magTs = magTs > 0 ? magTs : gyrTs; + + setup(); +} + +void VQF::updateGyr(const vqf_real_t gyr[3], double gyrTs) +{ + // rest detection + if (params.restBiasEstEnabled || params.magDistRejectionEnabled) { + filterVec(gyr, 3, params.restFilterTau, coeffs.gyrTs, coeffs.restGyrLpB, coeffs.restGyrLpA, + state.restGyrLpState, state.restLastGyrLp); + + state.restLastSquaredDeviations[0] = square(gyr[0] - state.restLastGyrLp[0]) + + square(gyr[1] - state.restLastGyrLp[1]) + square(gyr[2] - state.restLastGyrLp[2]); + + vqf_real_t biasClip = params.biasClip*vqf_real_t(M_PI/180.0); + if (state.restLastSquaredDeviations[0] >= square(params.restThGyr*vqf_real_t(M_PI/180.0)) + || fabs(state.restLastGyrLp[0]) > biasClip || fabs(state.restLastGyrLp[1]) > biasClip + || fabs(state.restLastGyrLp[2]) > biasClip) { + state.restT = 0.0; + state.restDetected = false; + } + } + + // remove estimated gyro bias + vqf_real_t gyrNoBias[3] = {gyr[0]-state.bias[0], gyr[1]-state.bias[1], gyr[2]-state.bias[2]}; + // gyroscope prediction step + vqf_real_t gyrNorm = norm(gyrNoBias, 3); + vqf_real_t angle = gyrNorm * gyrTs; + if (gyrNorm > EPS) { + vqf_real_t c = cos(angle/2); + vqf_real_t s = sin(angle/2)/gyrNorm; + vqf_real_t gyrStepQuat[4] = {c, s*gyrNoBias[0], s*gyrNoBias[1], s*gyrNoBias[2]}; + quatMultiply(state.gyrQuat, gyrStepQuat, state.gyrQuat); + normalize(state.gyrQuat, 4); + } +} + +void VQF::updateAcc(const vqf_real_t acc[3]) +{ + // ignore [0 0 0] samples + if (acc[0] == vqf_real_t(0.0) && acc[1] == vqf_real_t(0.0) && acc[2] == vqf_real_t(0.0)) { + return; + } + + // rest detection + if (params.restBiasEstEnabled) { + filterVec(acc, 3, params.restFilterTau, coeffs.accTs, coeffs.restAccLpB, coeffs.restAccLpA, + state.restAccLpState, state.restLastAccLp); + + state.restLastSquaredDeviations[1] = square(acc[0] - state.restLastAccLp[0]) + + square(acc[1] - state.restLastAccLp[1]) + square(acc[2] - state.restLastAccLp[2]); + + if (state.restLastSquaredDeviations[1] >= square(params.restThAcc)) { + state.restT = 0.0; + state.restDetected = false; + } else { + state.restT += coeffs.accTs; + if (state.restT >= params.restMinT) { + state.restDetected = true; + } + } + } + + vqf_real_t accEarth[3]; + + // filter acc in inertial frame + quatRotate(state.gyrQuat, acc, accEarth); + filterVec(accEarth, 3, params.tauAcc, coeffs.accTs, coeffs.accLpB, coeffs.accLpA, state.accLpState, state.lastAccLp); + + // transform to 6D earth frame and normalize + quatRotate(state.accQuat, state.lastAccLp, accEarth); + normalize(accEarth, 3); + + // inclination correction + vqf_real_t accCorrQuat[4]; + vqf_real_t q_w = sqrt((accEarth[2]+1)/2); + if (q_w > 1e-6) { + accCorrQuat[0] = q_w; + accCorrQuat[1] = 0.5*accEarth[1]/q_w; + accCorrQuat[2] = -0.5*accEarth[0]/q_w; + accCorrQuat[3] = 0; + } else { + // to avoid numeric issues when acc is close to [0 0 -1], i.e. the correction step is close (<= 0.00011°) to 180°: + accCorrQuat[0] = 0; + accCorrQuat[1] = 1; + accCorrQuat[2] = 0; + accCorrQuat[3] = 0; + } + quatMultiply(accCorrQuat, state.accQuat, state.accQuat); + normalize(state.accQuat, 4); + + // calculate correction angular rate to facilitate debugging + state.lastAccCorrAngularRate = acos(accEarth[2])/coeffs.accTs; + + // bias estimation +#ifndef VQF_NO_MOTION_BIAS_ESTIMATION + if (params.motionBiasEstEnabled || params.restBiasEstEnabled) { + vqf_real_t biasClip = params.biasClip*vqf_real_t(M_PI/180.0); + + vqf_real_t accGyrQuat[4]; + vqf_real_t R[9]; + vqf_real_t biasLp[2]; + + // get rotation matrix corresponding to accGyrQuat + getQuat6D(accGyrQuat); + R[0] = 1 - 2*square(accGyrQuat[2]) - 2*square(accGyrQuat[3]); // r11 + R[1] = 2*(accGyrQuat[2]*accGyrQuat[1] - accGyrQuat[0]*accGyrQuat[3]); // r12 + R[2] = 2*(accGyrQuat[0]*accGyrQuat[2] + accGyrQuat[3]*accGyrQuat[1]); // r13 + R[3] = 2*(accGyrQuat[0]*accGyrQuat[3] + accGyrQuat[2]*accGyrQuat[1]); // r21 + R[4] = 1 - 2*square(accGyrQuat[1]) - 2*square(accGyrQuat[3]); // r22 + R[5] = 2*(accGyrQuat[2]*accGyrQuat[3] - accGyrQuat[1]*accGyrQuat[0]); // r23 + R[6] = 2*(accGyrQuat[3]*accGyrQuat[1] - accGyrQuat[0]*accGyrQuat[2]); // r31 + R[7] = 2*(accGyrQuat[0]*accGyrQuat[1] + accGyrQuat[3]*accGyrQuat[2]); // r32 + R[8] = 1 - 2*square(accGyrQuat[1]) - 2*square(accGyrQuat[2]); // r33 + + // calculate R*b_hat (only the x and y component, as z is not needed) + biasLp[0] = R[0]*state.bias[0] + R[1]*state.bias[1] + R[2]*state.bias[2]; + biasLp[1] = R[3]*state.bias[0] + R[4]*state.bias[1] + R[5]*state.bias[2]; + + // low-pass filter R and R*b_hat + filterVec(R, 9, params.tauAcc, coeffs.accTs, coeffs.accLpB, coeffs.accLpA, state.motionBiasEstRLpState, R); + filterVec(biasLp, 2, params.tauAcc, coeffs.accTs, coeffs.accLpB, coeffs.accLpA, state.motionBiasEstBiasLpState, + biasLp); + + // set measurement error and covariance for the respective Kalman filter update + vqf_real_t w[3]; + vqf_real_t e[3]; + if (state.restDetected && params.restBiasEstEnabled) { + e[0] = state.restLastGyrLp[0] - state.bias[0]; + e[1] = state.restLastGyrLp[1] - state.bias[1]; + e[2] = state.restLastGyrLp[2] - state.bias[2]; + matrix3SetToScaledIdentity(1.0, R); + std::fill(w, w+3, coeffs.biasRestW); + } else if (params.motionBiasEstEnabled) { + e[0] = -accEarth[1]/coeffs.accTs + biasLp[0] - R[0]*state.bias[0] - R[1]*state.bias[1] - R[2]*state.bias[2]; + e[1] = accEarth[0]/coeffs.accTs + biasLp[1] - R[3]*state.bias[0] - R[4]*state.bias[1] - R[5]*state.bias[2]; + e[2] = - R[6]*state.bias[0] - R[7]*state.bias[1] - R[8]*state.bias[2]; + w[0] = coeffs.biasMotionW; + w[1] = coeffs.biasMotionW; + w[2] = coeffs.biasVerticalW; + } else { + std::fill(w, w+3, -1); // disable update + } + + // Kalman filter update + // step 1: P = P + V (also increase covariance if there is no measurement update!) + if (state.biasP[0] < coeffs.biasP0) { + state.biasP[0] += coeffs.biasV; + } + if (state.biasP[4] < coeffs.biasP0) { + state.biasP[4] += coeffs.biasV; + } + if (state.biasP[8] < coeffs.biasP0) { + state.biasP[8] += coeffs.biasV; + } + if (w[0] >= 0) { + // clip disagreement to -2..2 °/s + // (this also effectively limits the harm done by the first inclination correction step) + clip(e, 3, -biasClip, biasClip); + + // step 2: K = P R^T inv(W + R P R^T) + vqf_real_t K[9]; + matrix3MultiplyTpsSecond(state.biasP, R, K); // K = P R^T + matrix3Multiply(R, K, K); // K = R P R^T + K[0] += w[0]; + K[4] += w[1]; + K[8] += w[2]; // K = W + R P R^T + matrix3Inv(K, K); // K = inv(W + R P R^T) + matrix3MultiplyTpsFirst(R, K, K); // K = R^T inv(W + R P R^T) + matrix3Multiply(state.biasP, K, K); // K = P R^T inv(W + R P R^T) + + // step 3: bias = bias + K (y - R bias) = bias + K e + state.bias[0] += K[0]*e[0] + K[1]*e[1] + K[2]*e[2]; + state.bias[1] += K[3]*e[0] + K[4]*e[1] + K[5]*e[2]; + state.bias[2] += K[6]*e[0] + K[7]*e[1] + K[8]*e[2]; + + // step 4: P = P - K R P + matrix3Multiply(K, R, K); // K = K R + matrix3Multiply(K, state.biasP, K); // K = K R P + for(size_t i = 0; i < 9; i++) { + state.biasP[i] -= K[i]; + } + + // clip bias estimate to -2..2 °/s + clip(state.bias, 3, -biasClip, biasClip); + } + } +#else + // simplified implementation of bias estimation for the special case in which only rest bias estimation is enabled + if (params.restBiasEstEnabled) { + vqf_real_t biasClip = params.biasClip*vqf_real_t(M_PI/180.0); + if (state.biasP < coeffs.biasP0) { + state.biasP += coeffs.biasV; + } + if (state.restDetected) { + vqf_real_t e[3]; + e[0] = state.restLastGyrLp[0] - state.bias[0]; + e[1] = state.restLastGyrLp[1] - state.bias[1]; + e[2] = state.restLastGyrLp[2] - state.bias[2]; + clip(e, 3, -biasClip, biasClip); + + // Kalman filter update, simplified scalar version for rest update + // (this version only uses the first entry of P as P is diagonal and all diagonal elements are the same) + // step 1: P = P + V (done above!) + // step 2: K = P R^T inv(W + R P R^T) + vqf_real_t k = state.biasP/(coeffs.biasRestW + state.biasP); + // step 3: bias = bias + K (y - R bias) = bias + K e + state.bias[0] += k*e[0]; + state.bias[1] += k*e[1]; + state.bias[2] += k*e[2]; + // step 4: P = P - K R P + state.biasP -= k*state.biasP; + clip(state.bias, 3, -biasClip, biasClip); + } + } +#endif +} + +void VQF::updateMag(const vqf_real_t mag[3]) +{ + // ignore [0 0 0] samples + if (mag[0] == vqf_real_t(0.0) && mag[1] == vqf_real_t(0.0) && mag[2] == vqf_real_t(0.0)) { + return; + } + + vqf_real_t magEarth[3]; + + // bring magnetometer measurement into 6D earth frame + vqf_real_t accGyrQuat[4]; + getQuat6D(accGyrQuat); + quatRotate(accGyrQuat, mag, magEarth); + + if (params.magDistRejectionEnabled) { + state.magNormDip[0] = norm(magEarth, 3); + state.magNormDip[1] = -asin(magEarth[2]/state.magNormDip[0]); + + if (params.magCurrentTau > 0) { + filterVec(state.magNormDip, 2, params.magCurrentTau, coeffs.magTs, coeffs.magNormDipLpB, + coeffs.magNormDipLpA, state.magNormDipLpState, state.magNormDip); + } + + // magnetic disturbance detection + if (fabs(state.magNormDip[0] - state.magRefNorm) < params.magNormTh*state.magRefNorm + && fabs(state.magNormDip[1] - state.magRefDip) < params.magDipTh*vqf_real_t(M_PI/180.0)) { + state.magUndisturbedT += coeffs.magTs; + if (state.magUndisturbedT >= params.magMinUndisturbedTime) { + state.magDistDetected = false; + state.magRefNorm += coeffs.kMagRef*(state.magNormDip[0] - state.magRefNorm); + state.magRefDip += coeffs.kMagRef*(state.magNormDip[1] - state.magRefDip); + } + } else { + state.magUndisturbedT = 0.0; + state.magDistDetected = true; + } + + // new magnetic field acceptance + if (fabs(state.magNormDip[0] - state.magCandidateNorm) < params.magNormTh*state.magCandidateNorm + && fabs(state.magNormDip[1] - state.magCandidateDip) < params.magDipTh*vqf_real_t(M_PI/180.0)) { + if (norm(state.restLastGyrLp, 3) >= params.magNewMinGyr*M_PI/180.0) { + state.magCandidateT += coeffs.magTs; + } + state.magCandidateNorm += coeffs.kMagRef*(state.magNormDip[0] - state.magCandidateNorm); + state.magCandidateDip += coeffs.kMagRef*(state.magNormDip[1] - state.magCandidateDip); + + if (state.magDistDetected && (state.magCandidateT >= params.magNewTime || ( + state.magRefNorm == 0.0 && state.magCandidateT >= params.magNewFirstTime))) { + state.magRefNorm = state.magCandidateNorm; + state.magRefDip = state.magCandidateDip; + state.magDistDetected = false; + state.magUndisturbedT = params.magMinUndisturbedTime; + } + } else { + state.magCandidateT = 0.0; + state.magCandidateNorm = state.magNormDip[0]; + state.magCandidateDip = state.magNormDip[1]; + } + } + + // calculate disagreement angle based on current magnetometer measurement + state.lastMagDisAngle = atan2(magEarth[0], magEarth[1]) - state.delta; + + // make sure the disagreement angle is in the range [-pi, pi] + if (state.lastMagDisAngle > vqf_real_t(M_PI)) { + state.lastMagDisAngle -= vqf_real_t(2*M_PI); + } else if (state.lastMagDisAngle < vqf_real_t(-M_PI)) { + state.lastMagDisAngle += vqf_real_t(2*M_PI); + } + + vqf_real_t k = coeffs.kMag; + + if (params.magDistRejectionEnabled) { + // magnetic disturbance rejection + if (state.magDistDetected) { + if (state.magRejectT <= params.magMaxRejectionTime) { + state.magRejectT += coeffs.magTs; + k = 0; + } else { + k /= params.magRejectionFactor; + } + } else { + state.magRejectT = std::max(state.magRejectT - params.magRejectionFactor*coeffs.magTs, vqf_real_t(0.0)); + } + } + + // ensure fast initial convergence + if (state.kMagInit != vqf_real_t(0.0)) { + // make sure that the gain k is at least 1/N, N=1,2,3,... in the first few samples + if (k < state.kMagInit) { + k = state.kMagInit; + } + + // iterative expression to calculate 1/N + state.kMagInit = state.kMagInit/(state.kMagInit+1); + + // disable if t > tauMag + if (state.kMagInit*params.tauMag < coeffs.magTs) { + state.kMagInit = 0.0; + } + } + + // first-order filter step + state.delta += k*state.lastMagDisAngle; + // calculate correction angular rate to facilitate debugging + state.lastMagCorrAngularRate = k*state.lastMagDisAngle/coeffs.magTs; + + // make sure delta is in the range [-pi, pi] + if (state.delta > vqf_real_t(M_PI)) { + state.delta -= vqf_real_t(2*M_PI); + } else if (state.delta < vqf_real_t(-M_PI)) { + state.delta += vqf_real_t(2*M_PI); + } +} + +void VQF::getQuat3D(vqf_real_t out[4]) const +{ + std::copy(state.gyrQuat, state.gyrQuat+4, out); +} + +void VQF::getQuat6D(vqf_real_t out[4]) const +{ + quatMultiply(state.accQuat, state.gyrQuat, out); +} + +void VQF::getQuat9D(vqf_real_t out[4]) const +{ + quatMultiply(state.accQuat, state.gyrQuat, out); + quatApplyDelta(out, state.delta, out); +} + +vqf_real_t VQF::getDelta() const +{ + return state.delta; +} + +vqf_real_t VQF::getBiasEstimate(vqf_real_t out[3]) const +{ + if (out) { + std::copy(state.bias, state.bias+3, out); + } +#ifndef VQF_NO_MOTION_BIAS_ESTIMATION + // use largest absolute row sum as upper bound estimate for largest eigenvalue (Gershgorin circle theorem) + // and clip output to biasSigmaInit + vqf_real_t sum1 = fabs(state.biasP[0]) + fabs(state.biasP[1]) + fabs(state.biasP[2]); + vqf_real_t sum2 = fabs(state.biasP[3]) + fabs(state.biasP[4]) + fabs(state.biasP[5]); + vqf_real_t sum3 = fabs(state.biasP[6]) + fabs(state.biasP[7]) + fabs(state.biasP[8]); + vqf_real_t P = std::min(std::max(std::max(sum1, sum2), sum3), coeffs.biasP0); +#else + vqf_real_t P = state.biasP; +#endif + // convert standard deviation from 0.01deg to rad + return sqrt(P)*vqf_real_t(M_PI/100.0/180.0); +} + +void VQF::setBiasEstimate(vqf_real_t bias[3], vqf_real_t sigma) +{ + std::copy(bias, bias+3, state.bias); + if (sigma > 0) { + vqf_real_t P = square(sigma*vqf_real_t(180.0*100.0/M_PI)); +#ifndef VQF_NO_MOTION_BIAS_ESTIMATION + matrix3SetToScaledIdentity(P, state.biasP); +#else + state.biasP = P; +#endif + } +} + +bool VQF::getRestDetected() const +{ + return state.restDetected; +} + +bool VQF::getMagDistDetected() const +{ + return state.magDistDetected; +} + +void VQF::getRelativeRestDeviations(vqf_real_t out[2]) const +{ + out[0] = sqrt(state.restLastSquaredDeviations[0]) / (params.restThGyr*vqf_real_t(M_PI/180.0)); + out[1] = sqrt(state.restLastSquaredDeviations[1]) / params.restThAcc; +} + +vqf_real_t VQF::getMagRefNorm() const +{ + return state.magRefNorm; +} + +vqf_real_t VQF::getMagRefDip() const +{ + return state.magRefDip; +} + +void VQF::setMagRef(vqf_real_t norm, vqf_real_t dip) +{ + state.magRefNorm = norm; + state.magRefDip = dip; +} + +#ifndef VQF_NO_MOTION_BIAS_ESTIMATION +void VQF::setMotionBiasEstEnabled(bool enabled) +{ + if (params.motionBiasEstEnabled == enabled) { + return; + } + params.motionBiasEstEnabled = enabled; + std::fill(state.motionBiasEstRLpState, state.motionBiasEstRLpState + 9*2, NaN); + std::fill(state.motionBiasEstBiasLpState, state.motionBiasEstBiasLpState + 2*2, NaN); +} +#endif + +void VQF::setRestBiasEstEnabled(bool enabled) +{ + if (params.restBiasEstEnabled == enabled) { + return; + } + params.restBiasEstEnabled = enabled; + state.restDetected = false; + std::fill(state.restLastSquaredDeviations, state.restLastSquaredDeviations + 3, 0.0); + state.restT = 0.0; + std::fill(state.restLastGyrLp, state.restLastGyrLp + 3, 0.0); + std::fill(state.restGyrLpState, state.restGyrLpState + 3*2, NaN); + std::fill(state.restLastAccLp, state.restLastAccLp + 3, 0.0); + std::fill(state.restAccLpState, state.restAccLpState + 3*2, NaN); +} + +void VQF::setMagDistRejectionEnabled(bool enabled) +{ + if (params.magDistRejectionEnabled == enabled) { + return; + } + params.magDistRejectionEnabled = enabled; + state.magDistDetected = true; + state.magRefNorm = 0.0; + state.magRefDip = 0.0; + state.magUndisturbedT = 0.0; + state.magRejectT = params.magMaxRejectionTime; + state.magCandidateNorm = -1.0; + state.magCandidateDip = 0.0; + state.magCandidateT = 0.0; + std::fill(state.magNormDipLpState, state.magNormDipLpState + 2*2, NaN); +} + +void VQF::setTauAcc(vqf_real_t tauAcc) +{ + if (params.tauAcc == tauAcc) { + return; + } + params.tauAcc = tauAcc; + double newB[3]; + double newA[3]; + + filterCoeffs(params.tauAcc, coeffs.accTs, newB, newA); + filterAdaptStateForCoeffChange(state.lastAccLp, 3, coeffs.accLpB, coeffs.accLpA, newB, newA, state.accLpState); + +#ifndef VQF_NO_MOTION_BIAS_ESTIMATION + // For R and biasLP, the last value is not saved in the state. + // Since b0 is small (at reasonable settings), the last output is close to state[0]. + vqf_real_t R[9]; + for (size_t i = 0; i < 9; i++) { + R[i] = state.motionBiasEstRLpState[2*i]; + } + filterAdaptStateForCoeffChange(R, 9, coeffs.accLpB, coeffs.accLpA, newB, newA, state.motionBiasEstRLpState); + vqf_real_t biasLp[2]; + for (size_t i = 0; i < 2; i++) { + biasLp[i] = state.motionBiasEstBiasLpState[2*i]; + } + filterAdaptStateForCoeffChange(biasLp, 2, coeffs.accLpB, coeffs.accLpA, newB, newA, state.motionBiasEstBiasLpState); +#endif + + std::copy(newB, newB+3, coeffs.accLpB); + std::copy(newA, newA+2, coeffs.accLpA); +} + +void VQF::setTauMag(vqf_real_t tauMag) +{ + params.tauMag = tauMag; + coeffs.kMag = gainFromTau(params.tauMag, coeffs.magTs); +} + +void VQF::setRestDetectionThresholds(vqf_real_t thGyr, vqf_real_t thAcc) +{ + params.restThGyr = thGyr; + params.restThAcc = thAcc; +} + +const VQFParams& VQF::getParams() const +{ + return params; +} + +const VQFCoefficients& VQF::getCoeffs() const +{ + return coeffs; +} + +const VQFState& VQF::getState() const +{ + return state; +} + +void VQF::setState(const VQFState& state) +{ + this->state = state; +} + +void VQF::resetState() +{ + quatSetToIdentity(state.gyrQuat); + quatSetToIdentity(state.accQuat); + state.delta = 0.0; + + state.restDetected = false; + state.magDistDetected = true; + + std::fill(state.lastAccLp, state.lastAccLp+3, 0); + std::fill(state.accLpState, state.accLpState + 3*2, NaN); + state.lastAccCorrAngularRate = 0.0; + + state.kMagInit = 1.0; + state.lastMagDisAngle = 0.0; + state.lastMagCorrAngularRate = 0.0; + + std::fill(state.bias, state.bias+3, 0); +#ifndef VQF_NO_MOTION_BIAS_ESTIMATION + matrix3SetToScaledIdentity(coeffs.biasP0, state.biasP); +#else + state.biasP = coeffs.biasP0; +#endif + +#ifndef VQF_NO_MOTION_BIAS_ESTIMATION + std::fill(state.motionBiasEstRLpState, state.motionBiasEstRLpState + 9*2, NaN); + std::fill(state.motionBiasEstBiasLpState, state.motionBiasEstBiasLpState + 2*2, NaN); +#endif + + std::fill(state.restLastSquaredDeviations, state.restLastSquaredDeviations + 3, 0.0); + state.restT = 0.0; + std::fill(state.restLastGyrLp, state.restLastGyrLp + 3, 0.0); + std::fill(state.restGyrLpState, state.restGyrLpState + 3*2, NaN); + std::fill(state.restLastAccLp, state.restLastAccLp + 3, 0.0); + std::fill(state.restAccLpState, state.restAccLpState + 3*2, NaN); + + state.magRefNorm = 0.0; + state.magRefDip = 0.0; + state.magUndisturbedT = 0.0; + state.magRejectT = params.magMaxRejectionTime; + state.magCandidateNorm = -1.0; + state.magCandidateDip = 0.0; + state.magCandidateT = 0.0; + std::fill(state.magNormDip, state.magNormDip + 2, 0); + std::fill(state.magNormDipLpState, state.magNormDipLpState + 2*2, NaN); +} + +void VQF::quatMultiply(const vqf_real_t q1[4], const vqf_real_t q2[4], vqf_real_t out[4]) +{ + vqf_real_t w = q1[0] * q2[0] - q1[1] * q2[1] - q1[2] * q2[2] - q1[3] * q2[3]; + vqf_real_t x = q1[0] * q2[1] + q1[1] * q2[0] + q1[2] * q2[3] - q1[3] * q2[2]; + vqf_real_t y = q1[0] * q2[2] - q1[1] * q2[3] + q1[2] * q2[0] + q1[3] * q2[1]; + vqf_real_t z = q1[0] * q2[3] + q1[1] * q2[2] - q1[2] * q2[1] + q1[3] * q2[0]; + out[0] = w; out[1] = x; out[2] = y; out[3] = z; +} + +void VQF::quatConj(const vqf_real_t q[4], vqf_real_t out[4]) +{ + vqf_real_t w = q[0]; + vqf_real_t x = -q[1]; + vqf_real_t y = -q[2]; + vqf_real_t z = -q[3]; + out[0] = w; out[1] = x; out[2] = y; out[3] = z; +} + + +void VQF::quatSetToIdentity(vqf_real_t out[4]) +{ + out[0] = 1; + out[1] = 0; + out[2] = 0; + out[3] = 0; +} + +void VQF::quatApplyDelta(vqf_real_t q[], vqf_real_t delta, vqf_real_t out[]) +{ + // out = quatMultiply([cos(delta/2), 0, 0, sin(delta/2)], q) + vqf_real_t c = cos(delta/2); + vqf_real_t s = sin(delta/2); + vqf_real_t w = c * q[0] - s * q[3]; + vqf_real_t x = c * q[1] - s * q[2]; + vqf_real_t y = c * q[2] + s * q[1]; + vqf_real_t z = c * q[3] + s * q[0]; + out[0] = w; out[1] = x; out[2] = y; out[3] = z; +} + +void VQF::quatRotate(const vqf_real_t q[4], const vqf_real_t v[3], vqf_real_t out[3]) +{ + vqf_real_t x = (1 - 2*q[2]*q[2] - 2*q[3]*q[3])*v[0] + 2*v[1]*(q[2]*q[1] - q[0]*q[3]) + 2*v[2]*(q[0]*q[2] + q[3]*q[1]); + vqf_real_t y = 2*v[0]*(q[0]*q[3] + q[2]*q[1]) + v[1]*(1 - 2*q[1]*q[1] - 2*q[3]*q[3]) + 2*v[2]*(q[2]*q[3] - q[1]*q[0]); + vqf_real_t z = 2*v[0]*(q[3]*q[1] - q[0]*q[2]) + 2*v[1]*(q[0]*q[1] + q[3]*q[2]) + v[2]*(1 - 2*q[1]*q[1] - 2*q[2]*q[2]); + out[0] = x; out[1] = y; out[2] = z; +} + +vqf_real_t VQF::norm(const vqf_real_t vec[], size_t N) +{ + vqf_real_t s = 0; + for(size_t i = 0; i < N; i++) { + s += vec[i]*vec[i]; + } + return sqrt(s); +} + +void VQF::normalize(vqf_real_t vec[], size_t N) +{ + vqf_real_t n = norm(vec, N); + if (n < EPS) { + return; + } + for(size_t i = 0; i < N; i++) { + vec[i] /= n; + } +} + +void VQF::clip(vqf_real_t vec[], size_t N, vqf_real_t min, vqf_real_t max) +{ + for(size_t i = 0; i < N; i++) { + if (vec[i] < min) { + vec[i] = min; + } else if (vec[i] > max) { + vec[i] = max; + } + } +} + +vqf_real_t VQF::gainFromTau(vqf_real_t tau, vqf_real_t Ts) +{ + assert(Ts > 0); + if (tau < vqf_real_t(0.0)) { + return 0; // k=0 for negative tau (disable update) + } else if (tau == vqf_real_t(0.0)) { + return 1; // k=1 for tau=0 + } else { + return 1 - exp(-Ts/tau); // fc = 1/(2*pi*tau) + } +} + +void VQF::filterCoeffs(vqf_real_t tau, vqf_real_t Ts, double outB[], double outA[]) +{ + assert(tau > 0); + assert(Ts > 0); + // second order Butterworth filter based on https://stackoverflow.com/a/52764064 + double fc = (M_SQRT2 / (2.0*M_PI))/double(tau); // time constant of dampened, non-oscillating part of step response + double C = tan(M_PI*fc*double(Ts)); + double D = C*C + sqrt(2)*C + 1; + double b0 = C*C/D; + outB[0] = b0; + outB[1] = 2*b0; + outB[2] = b0; + // a0 = 1.0 + outA[0] = 2*(C*C-1)/D; // a1 + outA[1] = (1-sqrt(2)*C+C*C)/D; // a2 +} + +void VQF::filterInitialState(vqf_real_t x0, const double b[3], const double a[2], double out[]) +{ + // initial state for steady state (equivalent to scipy.signal.lfilter_zi, obtained by setting y=x=x0 in the filter + // update equation) + out[0] = x0*(1 - b[0]); + out[1] = x0*(b[2] - a[1]); +} + +void VQF::filterAdaptStateForCoeffChange(vqf_real_t last_y[], size_t N, const double b_old[], + const double a_old[], const double b_new[], + const double a_new[], double state[]) +{ + if (isnan(state[0])) { + return; + } + for (size_t i = 0; i < N; i++) { + state[0+2*i] = state[0+2*i] + (b_old[0] - b_new[0])*last_y[i]; + state[1+2*i] = state[1+2*i] + (b_old[1] - b_new[1] - a_old[0] + a_new[0])*last_y[i]; + } +} + +vqf_real_t VQF::filterStep(vqf_real_t x, const double b[3], const double a[2], double state[2]) +{ + // difference equations based on scipy.signal.lfilter documentation + // assumes that a0 == 1.0 + double y = b[0]*x + state[0]; + state[0] = b[1]*x - a[0]*y + state[1]; + state[1] = b[2]*x - a[1]*y; + return y; +} + +void VQF::filterVec(const vqf_real_t x[], size_t N, vqf_real_t tau, vqf_real_t Ts, const double b[3], + const double a[2], double state[], vqf_real_t out[]) +{ + assert(N>=2); + + // to avoid depending on a single sample, average the first samples (for duration tau) + // and then use this average to calculate the filter initial state + if (isnan(state[0])) { // initialization phase + if (isnan(state[1])) { // first sample + state[1] = 0; // state[1] is used to store the sample count + for(size_t i = 0; i < N; i++) { + state[2+i] = 0; // state[2+i] is used to store the sum + } + } + state[1]++; + for (size_t i = 0; i < N; i++) { + state[2+i] += x[i]; + out[i] = state[2+i]/state[1]; + } + if (state[1]*Ts >= tau) { + for(size_t i = 0; i < N; i++) { + filterInitialState(out[i], b, a, state+2*i); + } + } + return; + } + + for (size_t i = 0; i < N; i++) { + out[i] = filterStep(x[i], b, a, state+2*i); + } +} + +#ifndef VQF_NO_MOTION_BIAS_ESTIMATION +void VQF::matrix3SetToScaledIdentity(vqf_real_t scale, vqf_real_t out[9]) +{ + out[0] = scale; + out[1] = 0.0; + out[2] = 0.0; + out[3] = 0.0; + out[4] = scale; + out[5] = 0.0; + out[6] = 0.0; + out[7] = 0.0; + out[8] = scale; +} + +void VQF::matrix3Multiply(const vqf_real_t in1[9], const vqf_real_t in2[9], vqf_real_t out[9]) +{ + vqf_real_t tmp[9]; + tmp[0] = in1[0]*in2[0] + in1[1]*in2[3] + in1[2]*in2[6]; + tmp[1] = in1[0]*in2[1] + in1[1]*in2[4] + in1[2]*in2[7]; + tmp[2] = in1[0]*in2[2] + in1[1]*in2[5] + in1[2]*in2[8]; + tmp[3] = in1[3]*in2[0] + in1[4]*in2[3] + in1[5]*in2[6]; + tmp[4] = in1[3]*in2[1] + in1[4]*in2[4] + in1[5]*in2[7]; + tmp[5] = in1[3]*in2[2] + in1[4]*in2[5] + in1[5]*in2[8]; + tmp[6] = in1[6]*in2[0] + in1[7]*in2[3] + in1[8]*in2[6]; + tmp[7] = in1[6]*in2[1] + in1[7]*in2[4] + in1[8]*in2[7]; + tmp[8] = in1[6]*in2[2] + in1[7]*in2[5] + in1[8]*in2[8]; + std::copy(tmp, tmp+9, out); +} + +void VQF::matrix3MultiplyTpsFirst(const vqf_real_t in1[9], const vqf_real_t in2[9], vqf_real_t out[9]) +{ + vqf_real_t tmp[9]; + tmp[0] = in1[0]*in2[0] + in1[3]*in2[3] + in1[6]*in2[6]; + tmp[1] = in1[0]*in2[1] + in1[3]*in2[4] + in1[6]*in2[7]; + tmp[2] = in1[0]*in2[2] + in1[3]*in2[5] + in1[6]*in2[8]; + tmp[3] = in1[1]*in2[0] + in1[4]*in2[3] + in1[7]*in2[6]; + tmp[4] = in1[1]*in2[1] + in1[4]*in2[4] + in1[7]*in2[7]; + tmp[5] = in1[1]*in2[2] + in1[4]*in2[5] + in1[7]*in2[8]; + tmp[6] = in1[2]*in2[0] + in1[5]*in2[3] + in1[8]*in2[6]; + tmp[7] = in1[2]*in2[1] + in1[5]*in2[4] + in1[8]*in2[7]; + tmp[8] = in1[2]*in2[2] + in1[5]*in2[5] + in1[8]*in2[8]; + std::copy(tmp, tmp+9, out); +} + +void VQF::matrix3MultiplyTpsSecond(const vqf_real_t in1[9], const vqf_real_t in2[9], vqf_real_t out[9]) +{ + vqf_real_t tmp[9]; + tmp[0] = in1[0]*in2[0] + in1[1]*in2[1] + in1[2]*in2[2]; + tmp[1] = in1[0]*in2[3] + in1[1]*in2[4] + in1[2]*in2[5]; + tmp[2] = in1[0]*in2[6] + in1[1]*in2[7] + in1[2]*in2[8]; + tmp[3] = in1[3]*in2[0] + in1[4]*in2[1] + in1[5]*in2[2]; + tmp[4] = in1[3]*in2[3] + in1[4]*in2[4] + in1[5]*in2[5]; + tmp[5] = in1[3]*in2[6] + in1[4]*in2[7] + in1[5]*in2[8]; + tmp[6] = in1[6]*in2[0] + in1[7]*in2[1] + in1[8]*in2[2]; + tmp[7] = in1[6]*in2[3] + in1[7]*in2[4] + in1[8]*in2[5]; + tmp[8] = in1[6]*in2[6] + in1[7]*in2[7] + in1[8]*in2[8]; + std::copy(tmp, tmp+9, out); +} + +bool VQF::matrix3Inv(const vqf_real_t in[9], vqf_real_t out[9]) +{ + // in = [a b c; d e f; g h i] + double A = in[4]*in[8] - in[5]*in[7]; // (e*i - f*h) + double D = in[2]*in[7] - in[1]*in[8]; // -(b*i - c*h) + double G = in[1]*in[5] - in[2]*in[4]; // (b*f - c*e) + double B = in[5]*in[6] - in[3]*in[8]; // -(d*i - f*g) + double E = in[0]*in[8] - in[2]*in[6]; // (a*i - c*g) + double H = in[2]*in[3] - in[0]*in[5]; // -(a*f - c*d) + double C = in[3]*in[7] - in[4]*in[6]; // (d*h - e*g) + double F = in[1]*in[6] - in[0]*in[7]; // -(a*h - b*g) + double I = in[0]*in[4] - in[1]*in[3]; // (a*e - b*d) + + double det = in[0]*A + in[1]*B + in[2]*C; // a*A + b*B + c*C; + + if (det >= -EPS && det <= EPS) { + std::fill(out, out+9, 0); + return false; + } + + // out = [A D G; B E H; C F I]/det + out[0] = A/det; + out[1] = D/det; + out[2] = G/det; + out[3] = B/det; + out[4] = E/det; + out[5] = H/det; + out[6] = C/det; + out[7] = F/det; + out[8] = I/det; + + return true; +} +#endif + +void VQF::setup() +{ + assert(coeffs.gyrTs > 0); + assert(coeffs.accTs > 0); + assert(coeffs.magTs > 0); + + filterCoeffs(params.tauAcc, coeffs.accTs, coeffs.accLpB, coeffs.accLpA); + + coeffs.kMag = gainFromTau(params.tauMag, coeffs.magTs); + + coeffs.biasP0 = square(params.biasSigmaInit*100.0); + // the system noise increases the variance from 0 to (0.1 °/s)^2 in biasForgettingTime seconds + coeffs.biasV = square(0.1*100.0)*coeffs.accTs/params.biasForgettingTime; + +#ifndef VQF_NO_MOTION_BIAS_ESTIMATION + vqf_real_t pMotion = square(params.biasSigmaMotion*100.0); + coeffs.biasMotionW = square(pMotion) / coeffs.biasV + pMotion; + coeffs.biasVerticalW = coeffs.biasMotionW / std::max(params.biasVerticalForgettingFactor, vqf_real_t(1e-10)); +#endif + + vqf_real_t pRest = square(params.biasSigmaRest*100.0); + coeffs.biasRestW = square(pRest) / coeffs.biasV + pRest; + + filterCoeffs(params.restFilterTau, coeffs.gyrTs, coeffs.restGyrLpB, coeffs.restGyrLpA); + filterCoeffs(params.restFilterTau, coeffs.accTs, coeffs.restAccLpB, coeffs.restAccLpA); + + coeffs.kMagRef = gainFromTau(params.magRefTau, coeffs.magTs); + if (params.magCurrentTau > 0) { + filterCoeffs(params.magCurrentTau, coeffs.magTs, coeffs.magNormDipLpB, coeffs.magNormDipLpA); + } else { + std::fill(coeffs.magNormDipLpB, coeffs.magNormDipLpB + 3, NaN); + std::fill(coeffs.magNormDipLpA, coeffs.magNormDipLpA + 2, NaN); + } + + resetState(); +} diff --git a/lib/vqf/vqf.h b/lib/vqf/vqf.h new file mode 100644 index 0000000..cda5cc1 --- /dev/null +++ b/lib/vqf/vqf.h @@ -0,0 +1,1018 @@ +// SPDX-FileCopyrightText: 2021 Daniel Laidig +// +// SPDX-License-Identifier: MIT + +// Modified to add timestamps in: updateGyr(const vqf_real_t gyr[3], double gyrTs) +// Removed batch update functions + +#ifndef VQF_HPP +#define VQF_HPP + +#include + +#define VQF_SINGLE_PRECISION +#define VQF_NO_MOTION_BIAS_ESTIMATION +#define M_PI 3.14159265358979323846 +#define M_SQRT2 1.41421356237309504880 + +/** + * @brief Typedef for the floating-point data type used for most operations. + * + * By default, all floating-point calculations are performed using `double`. Set the `VQF_SINGLE_PRECISION` define to + * change this type to `float`. Note that the Butterworth filter implementation will always use double precision as + * using floats can cause numeric issues. + */ +#ifndef VQF_SINGLE_PRECISION +typedef double vqf_real_t; +#else +typedef float vqf_real_t; +#endif + +/** + * @brief Struct containing all tuning parameters used by the VQF class. + * + * The parameters influence the behavior of the algorithm and are independent of the sampling rate of the IMU data. The + * constructor sets all parameters to the default values. + * + * The parameters #motionBiasEstEnabled, #restBiasEstEnabled, and #magDistRejectionEnabled can be used to enable/disable + * the main features of the VQF algorithm. The time constants #tauAcc and #tauMag can be tuned to change the trust on + * the accelerometer and magnetometer measurements, respectively. The remaining parameters influence bias estimation + * and magnetometer rejection. + */ +struct VQFParams +{ + /** + * @brief Constructor that initializes the struct with the default parameters. + */ + VQFParams(); + + /** + * @brief Time constant \f$\tau_\mathrm{acc}\f$ for accelerometer low-pass filtering in seconds. + * + * Small values for \f$\tau_\mathrm{acc}\f$ imply trust on the accelerometer measurements and while large values of + * \f$\tau_\mathrm{acc}\f$ imply trust on the gyroscope measurements. + * + * The time constant \f$\tau_\mathrm{acc}\f$ corresponds to the cutoff frequency \f$f_\mathrm{c}\f$ of the + * second-order Butterworth low-pass filter as follows: \f$f_\mathrm{c} = \frac{\sqrt{2}}{2\pi\tau_\mathrm{acc}}\f$. + * + * Default value: 3.0 s + */ + vqf_real_t tauAcc; + /** + * @brief Time constant \f$\tau_\mathrm{mag}\f$ for magnetometer update in seconds. + * + * Small values for \f$\tau_\mathrm{mag}\f$ imply trust on the magnetometer measurements and while large values of + * \f$\tau_\mathrm{mag}\f$ imply trust on the gyroscope measurements. + * + * The time constant \f$\tau_\mathrm{mag}\f$ corresponds to the cutoff frequency \f$f_\mathrm{c}\f$ of the + * first-order low-pass filter for the heading correction as follows: + * \f$f_\mathrm{c} = \frac{1}{2\pi\tau_\mathrm{mag}}\f$. + * + * Default value: 9.0 s + */ + vqf_real_t tauMag; + +#ifndef VQF_NO_MOTION_BIAS_ESTIMATION + /** + * @brief Enables gyroscope bias estimation during motion phases. + * + * If set to true (default), gyroscope bias is estimated based on the inclination correction only, i.e. without + * using magnetometer measurements. + */ + bool motionBiasEstEnabled; +#endif + /** + * @brief Enables rest detection and gyroscope bias estimation during rest phases. + * + * If set to true (default), phases in which the IMU is at rest are detected. During rest, the gyroscope bias + * is estimated from the low-pass filtered gyroscope readings. + */ + bool restBiasEstEnabled; + /** + * @brief Enables magnetic disturbance detection and magnetic disturbance rejection. + * + * If set to true (default), the magnetic field is analyzed. For short disturbed phases, the magnetometer-based + * correction is disabled totally. If the magnetic field is always regarded as disturbed or if the duration of + * the disturbances exceeds #magMaxRejectionTime, magnetometer-based updates are performed, but with an increased + * time constant. + */ + bool magDistRejectionEnabled; + + /** + * @brief Standard deviation of the initial bias estimation uncertainty (in degrees per second). + * + * Default value: 0.5 °/s + */ + vqf_real_t biasSigmaInit; + /** + * @brief Time in which the bias estimation uncertainty increases from 0 °/s to 0.1 °/s (in seconds). + * + * This value determines the system noise assumed by the Kalman filter. + * + * Default value: 100.0 s + */ + vqf_real_t biasForgettingTime; + /** + * @brief Maximum expected gyroscope bias (in degrees per second). + * + * This value is used to clip the bias estimate and the measurement error in the bias estimation update step. It is + * further used by the rest detection algorithm in order to not regard measurements with a large but constant + * angular rate as rest. + * + * Default value: 2.0 °/s + */ + vqf_real_t biasClip; +#ifndef VQF_NO_MOTION_BIAS_ESTIMATION + /** + * @brief Standard deviation of the converged bias estimation uncertainty during motion (in degrees per second). + * + * This value determines the trust on motion bias estimation updates. A small value leads to fast convergence. + * + * Default value: 0.1 °/s + */ + vqf_real_t biasSigmaMotion; + /** + * @brief Forgetting factor for unobservable bias in vertical direction during motion. + * + * As magnetometer measurements are deliberately not used during motion bias estimation, gyroscope bias is not + * observable in vertical direction. This value is the relative weight of an artificial zero measurement that + * ensures that the bias estimate in the unobservable direction will eventually decay to zero. + * + * Default value: 0.0001 + */ + vqf_real_t biasVerticalForgettingFactor; +#endif + /** + * @brief Standard deviation of the converged bias estimation uncertainty during rest (in degrees per second). + * + * This value determines the trust on rest bias estimation updates. A small value leads to fast convergence. + * + * Default value: 0.03 ° + */ + vqf_real_t biasSigmaRest; + + /** + * @brief Time threshold for rest detection (in seconds). + * + * Rest is detected when the measurements have been close to the low-pass filtered reference for the given time. + * + * Default value: 1.5 s + */ + vqf_real_t restMinT; + /** + * @brief Time constant for the low-pass filter used in rest detection (in seconds). + * + * This time constant characterizes a second-order Butterworth low-pass filter used to obtain the reference for + * rest detection. + * + * Default value: 0.5 s + */ + vqf_real_t restFilterTau; + /** + * @brief Angular velocity threshold for rest detection (in °/s). + * + * For rest to be detected, the norm of the deviation between measurement and reference must be below the given + * threshold. (Furthermore, the absolute value of each component must be below #biasClip). + * + * Default value: 2.0 °/s + */ + vqf_real_t restThGyr; + /** + * @brief Acceleration threshold for rest detection (in m/s²). + * + * For rest to be detected, the norm of the deviation between measurement and reference must be below the given + * threshold. + * + * Default value: 0.5 m/s² + */ + vqf_real_t restThAcc; + + /** + * @brief Time constant for current norm/dip value in magnetic disturbance detection (in seconds). + * + * This (very fast) low-pass filter is intended to provide additional robustness when the magnetometer measurements + * are noisy or not sampled perfectly in sync with the gyroscope measurements. Set to -1 to disable the low-pass + * filter and directly use the magnetometer measurements. + * + * Default value: 0.05 s + */ + vqf_real_t magCurrentTau; + /** + * @brief Time constant for the adjustment of the magnetic field reference (in seconds). + * + * This adjustment allows the reference estimate to converge to the observed undisturbed field. + * + * Default value: 20.0 s + */ + vqf_real_t magRefTau; + /** + * @brief Relative threshold for the magnetic field strength for magnetic disturbance detection. + * + * This value is relative to the reference norm. + * + * Default value: 0.1 (10%) + */ + vqf_real_t magNormTh; + /** + * @brief Threshold for the magnetic field dip angle for magnetic disturbance detection (in degrees). + * + * Default vaule: 10 ° + */ + vqf_real_t magDipTh; + /** + * @brief Duration after which to accept a different homogeneous magnetic field (in seconds). + * + * A different magnetic field reference is accepted as the new field when the measurements are within the thresholds + * #magNormTh and #magDipTh for the given time. Additionally, only phases with sufficient movement, specified by + * #magNewMinGyr, count. + * + * Default value: 20.0 + */ + vqf_real_t magNewTime; + /** + * @brief Duration after which to accept a homogeneous magnetic field for the first time (in seconds). + * + * This value is used instead of #magNewTime when there is no current estimate in order to allow for the initial + * magnetic field reference to be obtained faster. + * + * Default value: 5.0 + */ + vqf_real_t magNewFirstTime; + /** + * @brief Minimum angular velocity needed in order to count time for new magnetic field acceptance (in °/s). + * + * Durations for which the angular velocity norm is below this threshold do not count towards reaching #magNewTime. + * + * Default value: 20.0 °/s + */ + vqf_real_t magNewMinGyr; + /** + * @brief Minimum duration within thresholds after which to regard the field as undisturbed again (in seconds). + * + * Default value: 0.5 s + */ + vqf_real_t magMinUndisturbedTime; + /** + * @brief Maximum duration of full magnetic disturbance rejection (in seconds). + * + * For magnetic disturbances up to this duration, heading correction is fully disabled and heading changes are + * tracked by gyroscope only. After this duration (or for many small disturbed phases without sufficient time in the + * undisturbed field in between), the heading correction is performed with an increased time constant (see + * #magRejectionFactor). + * + * Default value: 60.0 s + */ + vqf_real_t magMaxRejectionTime; + /** + * @brief Factor by which to slow the heading correction during long disturbed phases. + * + * After #magMaxRejectionTime of full magnetic disturbance rejection, heading correction is performed with an + * increased time constant. This parameter (approximately) specifies the factor of the increase. + * + * Furthermore, after spending #magMaxRejectionTime/#magRejectionFactor seconds in an undisturbed magnetic field, + * the time is reset and full magnetic disturbance rejection will be performed for up to #magMaxRejectionTime again. + * + * Default value: 2.0 + */ + vqf_real_t magRejectionFactor; +}; + +/** + * @brief Struct containing the filter state of the VQF class. + * + * The relevant parts of the state can be accessed via functions of the VQF class, e.g. VQF::getQuat6D(), + * VQF::getQuat9D(), VQF::getGyrBiasEstimate(), VQF::setGyrBiasEstimate(), VQF::getRestDetected() and + * VQF::getMagDistDetected(). To reset the state to the initial values, use VQF::resetState(). + * + * Direct access to the full state is typically not needed but can be useful in some cases, e.g. for debugging. For this + * purpose, the state can be accessed by VQF::getState() and set by VQF::setState(). + */ +struct VQFState { + /** + * @brief Angular velocity strapdown integration quaternion \f$^{\mathcal{S}_i}_{\mathcal{I}_i}\mathbf{q}\f$. + */ + vqf_real_t gyrQuat[4]; + /** + * @brief Inclination correction quaternion \f$^{\mathcal{I}_i}_{\mathcal{E}_i}\mathbf{q}\f$. + */ + vqf_real_t accQuat[4]; + /** + * @brief Heading difference \f$\delta\f$ between \f$\mathcal{E}_i\f$ and \f$\mathcal{E}\f$. + * + * \f$^{\mathcal{E}_i}_{\mathcal{E}}\mathbf{q} = \begin{bmatrix}\cos\frac{\delta}{2} & 0 & 0 & + * \sin\frac{\delta}{2}\end{bmatrix}^T\f$. + */ + vqf_real_t delta; + /** + * @brief True if it has been detected that the IMU is currently at rest. + * + * Used to switch between rest and motion gyroscope bias estimation. + */ + bool restDetected; + /** + * @brief True if magnetic disturbances have been detected. + */ + bool magDistDetected; + + /** + * @brief Last low-pass filtered acceleration in the \f$\mathcal{I}_i\f$ frame. + */ + vqf_real_t lastAccLp[3]; + /** + * @brief Internal low-pass filter state for #lastAccLp. + */ + double accLpState[3*2]; + /** + * @brief Last inclination correction angular rate. + * + * Change to inclination correction quaternion \f$^{\mathcal{I}_i}_{\mathcal{E}_i}\mathbf{q}\f$ performed in the + * last accelerometer update, expressed as an angular rate (in rad/s). + */ + vqf_real_t lastAccCorrAngularRate; + + /** + * @brief Gain used for heading correction to ensure fast initial convergence. + * + * This value is used as the gain for heading correction in the beginning if it is larger than the normal filter + * gain. It is initialized to 1 and then updated to 0.5, 0.33, 0.25, ... After VQFParams::tauMag seconds, it is + * set to zero. + */ + vqf_real_t kMagInit; + /** + * @brief Last heading disagreement angle. + * + * Disagreement between the heading \f$\hat\delta\f$ estimated from the last magnetometer sample and the state + * \f$\delta\f$ (in rad). + */ + vqf_real_t lastMagDisAngle; + /** + * @brief Last heading correction angular rate. + * + * Change to heading \f$\delta\f$ performed in the last magnetometer update, + * expressed as an angular rate (in rad/s). + */ + vqf_real_t lastMagCorrAngularRate; + + /** + * @brief Current gyroscope bias estimate (in rad/s). + */ + vqf_real_t bias[3]; +#ifndef VQF_NO_MOTION_BIAS_ESTIMATION + /** + * @brief Covariance matrix of the gyroscope bias estimate. + * + * The 3x3 matrix is stored in row-major order. Note that for numeric reasons the internal unit used is 0.01 °/s, + * i.e. to get the standard deviation in degrees per second use \f$\sigma = \frac{\sqrt{p_{ii}}}{100}\f$. + */ + vqf_real_t biasP[9]; +#else + // If only rest gyr bias estimation is enabled, P and K of the KF are always diagonal + // and matrix inversion is not needed. If motion bias estimation is disabled at compile + // time, storing the full P matrix is not necessary. + vqf_real_t biasP; +#endif + +#ifndef VQF_NO_MOTION_BIAS_ESTIMATION + /** + * @brief Internal state of the Butterworth low-pass filter for the rotation matrix coefficients used in motion + * bias estimation. + */ + double motionBiasEstRLpState[9*2]; + /** + * @brief Internal low-pass filter state for the rotated bias estimate used in motion bias estimation. + */ + double motionBiasEstBiasLpState[2*2]; +#endif + /** + * @brief Last (squared) deviations from the reference of the last sample used in rest detection. + * + * Looking at those values can be useful to understand how rest detection is working and which thresholds are + * suitable. The array contains the last values for gyroscope and accelerometer in the respective + * units. Note that the values are squared. + * + * The method VQF::getRelativeRestDeviations() provides an easier way to obtain and interpret those values. + */ + vqf_real_t restLastSquaredDeviations[2]; + /** + * @brief The current duration for which all sensor readings are within the rest detection thresholds. + * + * Rest is detected if this value is larger or equal to VQFParams::restMinT. + */ + vqf_real_t restT; + /** + * @brief Last low-pass filtered gyroscope measurement used as the reference for rest detection. + * + * Note that this value is also used for gyroscope bias estimation when rest is detected. + */ + vqf_real_t restLastGyrLp[3]; + /** + * @brief Internal low-pass filter state for #restLastGyrLp. + */ + double restGyrLpState[3*2]; + /** + * @brief Last low-pass filtered accelerometer measurement used as the reference for rest detection. + */ + vqf_real_t restLastAccLp[3]; + /** + * @brief Internal low-pass filter state for #restLastAccLp. + */ + double restAccLpState[3*2]; + + /** + * @brief Norm of the currently accepted magnetic field reference. + * + * A value of -1 indicates that no homogeneous field is found yet. + */ + vqf_real_t magRefNorm; + /** + * @brief Dip angle of the currently accepted magnetic field reference. + */ + vqf_real_t magRefDip; + /** + * @brief The current duration for which the current norm and dip are close to the reference. + * + * The magnetic field is regarded as undisturbed when this value reaches VQFParams::magMinUndisturbedTime. + */ + vqf_real_t magUndisturbedT; + /** + * @brief The current duration for which the magnetic field was rejected. + * + * If the magnetic field is disturbed and this value is smaller than VQFParams::magMaxRejectionTime, heading + * correction updates are fully disabled. + */ + vqf_real_t magRejectT; + /** + * @brief Norm of the alternative magnetic field reference currently being evaluated. + */ + vqf_real_t magCandidateNorm; + /** + * @brief Dip angle of the alternative magnetic field reference currently being evaluated. + */ + vqf_real_t magCandidateDip; + /** + * @brief The current duration for which the norm and dip are close to the candidate. + * + * If this value exceeds VQFParams::magNewTime (or VQFParams::magNewFirstTime if #magRefNorm < 0), the current + * candidate is accepted as the new reference. + */ + vqf_real_t magCandidateT; + /** + * @brief Norm and dip angle of the current magnetometer measurements. + * + * Slightly low-pass filtered, see VQFParams::magCurrentTau. + */ + vqf_real_t magNormDip[2]; + /** + * @brief Internal low-pass filter state for the current norm and dip angle. + */ + double magNormDipLpState[2*2]; +}; + +/** + * @brief Struct containing coefficients used by the VQF class. + * + * Coefficients are values that depend on the parameters and the sampling times, but do not change during update steps. + * They are calculated in VQF::setup(). + */ +struct VQFCoefficients +{ + /** + * @brief Sampling time of the gyroscope measurements (in seconds). + */ + vqf_real_t gyrTs; + /** + * @brief Sampling time of the accelerometer measurements (in seconds). + */ + vqf_real_t accTs; + /** + * @brief Sampling time of the magnetometer measurements (in seconds). + */ + vqf_real_t magTs; + + /** + * @brief Numerator coefficients of the acceleration low-pass filter. + * + * The array contains \f$\begin{bmatrix}b_0 & b_1 & b_2\end{bmatrix}\f$. + */ + double accLpB[3]; + /** + * @brief Denominator coefficients of the acceleration low-pass filter. + * + * The array contains \f$\begin{bmatrix}a_1 & a_2\end{bmatrix}\f$ and \f$a_0=1\f$. + */ + double accLpA[2]; + + /** + * @brief Gain of the first-order filter used for heading correction. + */ + vqf_real_t kMag; + + /** + * @brief Variance of the initial gyroscope bias estimate. + */ + vqf_real_t biasP0; + /** + * @brief System noise variance used in gyroscope bias estimation. + */ + vqf_real_t biasV; +#ifndef VQF_NO_MOTION_BIAS_ESTIMATION + /** + * @brief Measurement noise variance for the motion gyroscope bias estimation update. + */ + vqf_real_t biasMotionW; + /** + * @brief Measurement noise variance for the motion gyroscope bias estimation update in vertical direction. + */ + vqf_real_t biasVerticalW; +#endif + /** + * @brief Measurement noise variance for the rest gyroscope bias estimation update. + */ + vqf_real_t biasRestW; + + /** + * @brief Numerator coefficients of the gyroscope measurement low-pass filter for rest detection. + */ + double restGyrLpB[3]; + /** + * @brief Denominator coefficients of the gyroscope measurement low-pass filter for rest detection. + */ + double restGyrLpA[2]; + /** + * @brief Numerator coefficients of the accelerometer measurement low-pass filter for rest detection. + */ + double restAccLpB[3]; + /** + * @brief Denominator coefficients of the accelerometer measurement low-pass filter for rest detection. + */ + double restAccLpA[2]; + + /** + * @brief Gain of the first-order filter used for to update the magnetic field reference and candidate. + */ + vqf_real_t kMagRef; + /** + * @brief Numerator coefficients of the low-pass filter for the current magnetic norm and dip. + */ + double magNormDipLpB[3]; + /** + * @brief Denominator coefficients of the low-pass filter for the current magnetic norm and dip. + */ + double magNormDipLpA[2]; +}; + +/** + * @brief A Versatile Quaternion-based Filter for IMU Orientation Estimation. + * + * \rst + * This class implements the orientation estimation filter described in the following publication: + * + * + * D. Laidig and T. Seel. "VQF: Highly Accurate IMU Orientation Estimation with Bias Estimation and Magnetic + * Disturbance Rejection." Information Fusion 2023, 91, 187--204. + * `doi:10.1016/j.inffus.2022.10.014 `_. + * [Accepted manuscript available at `arXiv:2203.17024 `_.] + * + * The filter can perform simultaneous 6D (magnetometer-free) and 9D (gyr+acc+mag) sensor fusion and can also be used + * without magnetometer data. It performs rest detection, gyroscope bias estimation during rest and motion, and magnetic + * disturbance detection and rejection. Different sampling rates for gyroscopes, accelerometers, and magnetometers are + * supported as well. While in most cases, the defaults will be reasonable, the algorithm can be influenced via a + * number of tuning parameters. + * + * To use this C++ implementation, + * + * 1. create a instance of the class and provide the sampling time and, optionally, parameters + * 2. for every sample, call one of the update functions to feed the algorithm with IMU data + * 3. access the estimation results with :meth:`getQuat6D() `, :meth:`getQuat9D() ` and + * the other getter methods. + * + * If the full data is available in (row-major) data buffers, you can use :meth:`updateBatch() `. + * + * This class is the main C++ implementation of the algorithm. Depending on use case and programming language of choice, + * the following alternatives might be useful: + * + * +------------------------+--------------------------+--------------------------+---------------------------+ + * | | Full Version | Basic Version | Offline Version | + * | | | | | + * +========================+==========================+==========================+===========================+ + * | **C++** | **VQF (this class)** | :cpp:class:`BasicVQF` | :cpp:func:`offlineVQF` | + * +------------------------+--------------------------+--------------------------+---------------------------+ + * | **Python/C++ (fast)** | :py:class:`vqf.VQF` | :py:class:`vqf.BasicVQF` | :py:meth:`vqf.offlineVQF` | + * +------------------------+--------------------------+--------------------------+---------------------------+ + * | **Pure Python (slow)** | :py:class:`vqf.PyVQF` | -- | -- | + * +------------------------+--------------------------+--------------------------+---------------------------+ + * | **Pure Matlab (slow)** | :mat:class:`VQF.m ` | -- | -- | + * +------------------------+--------------------------+--------------------------+---------------------------+ + * \endrst + */ + +class VQF +{ +public: + /** + * Initializes the object with default parameters. + * + * In the most common case (using the default parameters and all data being sampled with the same frequency, + * create the class like this: + * \rst + * .. code-block:: c++ + * + * VQF vqf(0.01); // 0.01 s sampling time, i.e. 100 Hz + * \endrst + * + * @param gyrTs sampling time of the gyroscope measurements in seconds + * @param accTs sampling time of the accelerometer measurements in seconds (the value of `gyrTs` is used if set to -1) + * @param magTs sampling time of the magnetometer measurements in seconds (the value of `gyrTs` is used if set to -1) + * + */ + VQF(vqf_real_t gyrTs, vqf_real_t accTs=-1.0, vqf_real_t magTs=-1.0); + /** + * @brief Initializes the object with custom parameters. + * + * Example code to create an object with magnetic disturbance rejection disabled: + * \rst + * .. code-block:: c++ + * + * VQFParams params; + * params.magDistRejectionEnabled = false; + * VQF vqf(0.01); // 0.01 s sampling time, i.e. 100 Hz + * \endrst + * + * @param params VQFParams struct containing the desired parameters + * @param gyrTs sampling time of the gyroscope measurements in seconds + * @param accTs sampling time of the accelerometer measurements in seconds (the value of `gyrTs` is used if set to -1) + * @param magTs sampling time of the magnetometer measurements in seconds (the value of `gyrTs` is used if set to -1) + */ + VQF(const VQFParams& params, vqf_real_t gyrTs, vqf_real_t accTs=-1.0, vqf_real_t magTs=-1.0); + + /** + * @brief Performs gyroscope update step. + * + * It is only necessary to call this function directly if gyroscope, accelerometers and magnetometers have + * different sampling rates. Otherwise, simply use #update(). + * + * @param gyr gyroscope measurement in rad/s + */ + void updateGyr(const vqf_real_t gyr[3], double gyrTs); + /** + * @brief Performs accelerometer update step. + * + * It is only necessary to call this function directly if gyroscope, accelerometers and magnetometers have + * different sampling rates. Otherwise, simply use #update(). + * + * Should be called after #updateGyr and before #updateMag. + * + * @param acc accelerometer measurement in m/s² + */ + void updateAcc(const vqf_real_t acc[3]); + /** + * @brief Performs magnetometer update step. + * + * It is only necessary to call this function directly if gyroscope, accelerometers and magnetometers have + * different sampling rates. Otherwise, simply use #update(). + * + * Should be called after #updateAcc. + * + * @param mag magnetometer measurement in arbitrary units + */ + void updateMag(const vqf_real_t mag[3]); + + /** + * @brief Returns the angular velocity strapdown integration quaternion + * \f$^{\mathcal{S}_i}_{\mathcal{I}_i}\mathbf{q}\f$. + * @param out output array for the quaternion + */ + void getQuat3D(vqf_real_t out[4]) const; + /** + * @brief Returns the 6D (magnetometer-free) orientation quaternion + * \f$^{\mathcal{S}_i}_{\mathcal{E}_i}\mathbf{q}\f$. + * @param out output array for the quaternion + */ + void getQuat6D(vqf_real_t out[4]) const; + /** + * @brief Returns the 9D (with magnetometers) orientation quaternion + * \f$^{\mathcal{S}_i}_{\mathcal{E}}\mathbf{q}\f$. + * @param out output array for the quaternion + */ + void getQuat9D(vqf_real_t out[4]) const; + /** + * @brief Returns the heading difference \f$\delta\f$ between \f$\mathcal{E}_i\f$ and \f$\mathcal{E}\f$. + * + * \f$^{\mathcal{E}_i}_{\mathcal{E}}\mathbf{q} = \begin{bmatrix}\cos\frac{\delta}{2} & 0 & 0 & + * \sin\frac{\delta}{2}\end{bmatrix}^T\f$. + * + * @return delta angle in rad (VQFState::delta) + */ + vqf_real_t getDelta() const; + + /** + * @brief Returns the current gyroscope bias estimate and the uncertainty. + * + * The returned standard deviation sigma represents the estimation uncertainty in the worst direction and is based + * on an upper bound of the largest eigenvalue of the covariance matrix. + * + * @param out output array for the gyroscope bias estimate (rad/s) + * @return standard deviation sigma of the estimation uncertainty (rad/s) + */ + vqf_real_t getBiasEstimate(vqf_real_t out[3]) const; + /** + * @brief Sets the current gyroscope bias estimate and the uncertainty. + * + * If a value for the uncertainty sigma is given, the covariance matrix is set to a corresponding scaled identity + * matrix. + * + * @param bias gyroscope bias estimate (rad/s) + * @param sigma standard deviation of the estimation uncertainty (rad/s) - set to -1 (default) in order to not + * change the estimation covariance matrix + */ + void setBiasEstimate(vqf_real_t bias[3], vqf_real_t sigma=-1.0); + /** + * @brief Returns true if rest was detected. + */ + bool getRestDetected() const; + /** + * @brief Returns true if a disturbed magnetic field was detected. + */ + bool getMagDistDetected() const; + /** + * @brief Returns the relative deviations used in rest detection. + * + * Looking at those values can be useful to understand how rest detection is working and which thresholds are + * suitable. The output array is filled with the last values for gyroscope and accelerometer, + * relative to the threshold. In order for rest to be detected, both values must stay below 1. + * + * @param out output array of size 2 for the relative rest deviations + */ + void getRelativeRestDeviations(vqf_real_t out[2]) const; + /** + * @brief Returns the norm of the currently accepted magnetic field reference. + */ + vqf_real_t getMagRefNorm() const; + /** + * @brief Returns the dip angle of the currently accepted magnetic field reference. + */ + vqf_real_t getMagRefDip() const; + /** + * @brief Overwrites the current magnetic field reference. + * @param norm norm of the magnetic field reference + * @param dip dip angle of the magnetic field reference + */ + void setMagRef(vqf_real_t norm, vqf_real_t dip); + + /** + * @brief Sets the time constant for accelerometer low-pass filtering. + * + * For more details, see VQFParams.tauAcc. + * + * @param tauAcc time constant \f$\tau_\mathrm{acc}\f$ in seconds + */ + void setTauAcc(vqf_real_t tauAcc); + /** + * @brief Sets the time constant for the magnetometer update. + * + * For more details, see VQFParams.tauMag. + * + * @param tauMag time constant \f$\tau_\mathrm{mag}\f$ in seconds + */ + void setTauMag(vqf_real_t tauMag); +#ifndef VQF_NO_MOTION_BIAS_ESTIMATION + /** + * @brief Enables/disabled gyroscope bias estimation during motion. + */ + void setMotionBiasEstEnabled(bool enabled); +#endif + /** + * @brief Enables/disables rest detection and bias estimation during rest. + */ + void setRestBiasEstEnabled(bool enabled); + /** + * @brief Enables/disables magnetic disturbance detection and rejection. + */ + void setMagDistRejectionEnabled(bool enabled); + /** + * @brief Sets the current thresholds for rest detection. + * + * For details about the parameters, see VQFParams.restThGyr and VQFParams.restThAcc. + */ + void setRestDetectionThresholds(vqf_real_t thGyr, vqf_real_t thAcc); + + /** + * @brief Returns the current parameters. + */ + const VQFParams& getParams() const; + /** + * @brief Returns the coefficients used by the algorithm. + */ + const VQFCoefficients& getCoeffs() const; + /** + * @brief Returns the current state. + */ + const VQFState& getState() const; + /** + * @brief Overwrites the current state. + * + * This method allows to set a completely arbitrary filter state and is intended for debugging purposes. In + * combination with #getState, individual elements of the state can be modified. + * + * @param state A VQFState struct containing the new state + */ + void setState(const VQFState& state); + /** + * @brief Resets the state to the default values at initialization. + * + * Resetting the state is equivalent to creating a new instance of this class. + */ + void resetState(); + + /** + * @brief Performs quaternion multiplication (\f$\mathbf{q}_\mathrm{out} = \mathbf{q}_1 \otimes \mathbf{q}_2\f$). + */ + static void quatMultiply(const vqf_real_t q1[4], const vqf_real_t q2[4], vqf_real_t out[4]); + /** + * @brief Calculates the quaternion conjugate (\f$\mathbf{q}_\mathrm{out} = \mathbf{q}^*\f$). + */ + static void quatConj(const vqf_real_t q[4], vqf_real_t out[4]); + /** + * @brief Sets the output quaternion to the identity quaternion (\f$\mathbf{q}_\mathrm{out} = + * \begin{bmatrix}1 & 0 & 0 & 0\end{bmatrix}\f$). + */ + static void quatSetToIdentity(vqf_real_t out[4]); + /** + * @brief Applies a heading rotation by the angle delta (in rad) to a quaternion. + * + * \f$\mathbf{q}_\mathrm{out} = \begin{bmatrix}\cos\frac{\delta}{2} & 0 & 0 & + * \sin\frac{\delta}{2}\end{bmatrix} \otimes \mathbf{q}\f$ + */ + static void quatApplyDelta(vqf_real_t q[4], vqf_real_t delta, vqf_real_t out[4]); + /** + * @brief Rotates a vector with a given quaternion. + * + * \f$\begin{bmatrix}0 & \mathbf{v}_\mathrm{out}\end{bmatrix} = + * \mathbf{q} \otimes \begin{bmatrix}0 & \mathbf{v}\end{bmatrix} \otimes \mathbf{q}^*\f$ + */ + static void quatRotate(const vqf_real_t q[4], const vqf_real_t v[3], vqf_real_t out[3]); + /** + * @brief Calculates the Euclidean norm of a vector. + * @param vec pointer to an array of N elements + * @param N number of elements + */ + static vqf_real_t norm(const vqf_real_t vec[], size_t N); + /** + * @brief Normalizes a vector in-place. + * @param vec pointer to an array of N elements that will be normalized + * @param N number of elements + */ + static void normalize(vqf_real_t vec[], size_t N); + /** + * @brief Clips a vector in-place. + * @param vec pointer to an array of N elements that will be clipped + * @param N number of elements + * @param min smallest allowed value + * @param max largest allowed value + */ + static void clip(vqf_real_t vec[], size_t N, vqf_real_t min, vqf_real_t max); + /** + * @brief Calculates the gain for a first-order low-pass filter from the 1/e time constant. + * + * \f$k = 1 - \exp\left(-\frac{T_\mathrm{s}}{\tau}\right)\f$ + * + * The cutoff frequency of the resulting filter is \f$f_\mathrm{c} = \frac{1}{2\pi\tau}\f$. + * + * @param tau time constant \f$\tau\f$ in seconds - use -1 to disable update (\f$k=0\f$) or 0 to obtain + * unfiltered values (\f$k=1\f$) + * @param Ts sampling time \f$T_\mathrm{s}\f$ in seconds + * @return filter gain *k* + */ + static vqf_real_t gainFromTau(vqf_real_t tau, vqf_real_t Ts); + /** + * @brief Calculates coefficients for a second-order Butterworth low-pass filter. + * + * The filter is parametrized via the time constant of the dampened, non-oscillating part of step response and the + * resulting cutoff frequency is \f$f_\mathrm{c} = \frac{\sqrt{2}}{2\pi\tau}\f$. + * + * @param tau time constant \f$\tau\f$ in seconds + * @param Ts sampling time \f$T_\mathrm{s}\f$ in seconds + * @param outB output array for numerator coefficients + * @param outA output array for denominator coefficients (without \f$a_0=1\f$) + */ + static void filterCoeffs(vqf_real_t tau, vqf_real_t Ts, double outB[3], double outA[2]); + /** + * @brief Calculates the initial filter state for a given steady-state value. + * @param x0 steady state value + * @param b numerator coefficients + * @param a denominator coefficients (without \f$a_0=1\f$) + * @param out output array for filter state + */ + static void filterInitialState(vqf_real_t x0, const double b[], const double a[], double out[2]); + /** + * @brief Adjusts the filter state when changing coefficients. + * + * This function assumes that the filter is currently in a steady state, i.e. the last input values and the last + * output values are all equal. Based on this, the filter state is adjusted to new filter coefficients so that the + * output does not jump. + * + * @param last_y last filter output values (array of size N) + * @param N number of values in vector-valued signal + * @param b_old previous numerator coefficients + * @param a_old previous denominator coefficients (without \f$a_0=1\f$) + * @param b_new new numerator coefficients + * @param a_new new denominator coefficients (without \f$a_0=1\f$) + * @param state filter state (array of size N*2, will be modified) + */ + static void filterAdaptStateForCoeffChange(vqf_real_t last_y[], size_t N, const double b_old[3], + const double a_old[2], const double b_new[3], + const double a_new[2], double state[]); + /** + * @brief Performs a filter step for a scalar value. + * @param x input value + * @param b numerator coefficients + * @param a denominator coefficients (without \f$a_0=1\f$) + * @param state filter state array (will be modified) + * @return filtered value + */ + static vqf_real_t filterStep(vqf_real_t x, const double b[3], const double a[2], double state[2]); + /** + * @brief Performs filter step for vector-valued signal with averaging-based initialization. + * + * During the first \f$\tau\f$ seconds, the filter output is the mean of the previous samples. At \f$t=\tau\f$, the + * initial conditions for the low-pass filter are calculated based on the current mean value and from then on, + * regular filtering with the rational transfer function described by the coefficients b and a is performed. + * + * @param x input values (array of size N) + * @param N number of values in vector-valued signal + * @param tau filter time constant \f$\tau\f$ in seconds (used for initialization) + * @param Ts sampling time \f$T_\mathrm{s}\f$ in seconds (used for initialization) + * @param b numerator coefficients + * @param a denominator coefficients (without \f$a_0=1\f$) + * @param state filter state (array of size N*2, will be modified) + * @param out output array for filtered values (size N) + */ + static void filterVec(const vqf_real_t x[], size_t N, vqf_real_t tau, vqf_real_t Ts, const double b[3], + const double a[2], double state[], vqf_real_t out[]); +#ifndef VQF_NO_MOTION_BIAS_ESTIMATION + /** + * @brief Sets a 3x3 matrix to a scaled version of the identity matrix. + * @param scale value of diagonal elements + * @param out output array of size 9 (3x3 matrix stored in row-major order) + */ + static void matrix3SetToScaledIdentity(vqf_real_t scale, vqf_real_t out[9]); + /** + * @brief Performs 3x3 matrix multiplication (\f$\mathbf{M}_\mathrm{out} = \mathbf{M}_1\mathbf{M}_2\f$). + * @param in1 input 3x3 matrix \f$\mathbf{M}_1\f$ (stored in row-major order) + * @param in2 input 3x3 matrix \f$\mathbf{M}_2\f$ (stored in row-major order) + * @param out output 3x3 matrix \f$\mathbf{M}_\mathrm{out}\f$ (stored in row-major order) + */ + static void matrix3Multiply(const vqf_real_t in1[9], const vqf_real_t in2[9], vqf_real_t out[9]); + /** + * @brief Performs 3x3 matrix multiplication after transposing the first matrix + * (\f$\mathbf{M}_\mathrm{out} = \mathbf{M}_1^T\mathbf{M}_2\f$). + * @param in1 input 3x3 matrix \f$\mathbf{M}_1\f$ (stored in row-major order) + * @param in2 input 3x3 matrix \f$\mathbf{M}_2\f$ (stored in row-major order) + * @param out output 3x3 matrix \f$\mathbf{M}_\mathrm{out}\f$ (stored in row-major order) + */ + static void matrix3MultiplyTpsFirst(const vqf_real_t in1[9], const vqf_real_t in2[9], vqf_real_t out[9]); + /** + * @brief Performs 3x3 matrix multiplication after transposing the second matrix + * (\f$\mathbf{M}_\mathrm{out} = \mathbf{M}_1\mathbf{M}_2^T\f$). + * @param in1 input 3x3 matrix \f$\mathbf{M}_1\f$ (stored in row-major order) + * @param in2 input 3x3 matrix \f$\mathbf{M}_2\f$ (stored in row-major order) + * @param out output 3x3 matrix \f$\mathbf{M}_\mathrm{out}\f$ (stored in row-major order) + */ + static void matrix3MultiplyTpsSecond(const vqf_real_t in1[9], const vqf_real_t in2[9], vqf_real_t out[9]); + /** + * @brief Calculates the inverse of a 3x3 matrix (\f$\mathbf{M}_\mathrm{out} = \mathbf{M}^{-1}\f$). + * @param in input 3x3 matrix \f$\mathbf{M}\f$ (stored in row-major order) + * @param out output 3x3 matrix \f$\mathbf{M}_\mathrm{out}\f$ (stored in row-major order) + */ + static bool matrix3Inv(const vqf_real_t in[9], vqf_real_t out[9]); +#endif + +protected: + /** + * @brief Calculates coefficients based on parameters and sampling rates. + */ + void setup(); + + /** + * @brief Contains the current parameters. + * + * See #getParams. To set parameters, pass them to the constructor. Part of the parameters can be changed with + * #setTauAcc, #setTauMag, #setMotionBiasEstEnabled, #setRestBiasEstEnabled, #setMagDistRejectionEnabled, and + * #setRestDetectionThresholds. + */ + VQFParams params; + /** + * @brief Contains the current state. + * + * See #getState, #getState and #resetState. + */ + VQFState state; + /** + * @brief Contains the current coefficients (calculated in #setup). + * + * See #getCoeffs. + */ + VQFCoefficients coeffs; +}; + +#endif // VQF_HPP diff --git a/platformio.ini b/platformio.ini index 396cc57..803d1b6 100644 --- a/platformio.ini +++ b/platformio.ini @@ -35,8 +35,9 @@ build_flags = ; Enable -O2 GCC optimization -O2 + -std=gnu++17 -build_unflags = -Os +build_unflags = -Os -std=gnu++11 ; If you want to enable OTA Updates, uncomment and set OTA password here and in credentials.h ; You can set upload_port to device's ip after it's set up for the first time @@ -62,9 +63,9 @@ upload_speed = 921600 ; Uncomment below if you want to build for esp32 ; Check your board name at https://docs.platformio.org/en/latest/platforms/espressif32.html#boards -;[env:esp32] -;platform = espressif32 @ 5.1.1 -;board = esp32dev +; [env:esp32] +; platform = espressif32 @ 5.1.1 +; board = esp32dev ; Comment out this line below if you have any trouble uploading the firmware - and if it has a CP2102 on it (a square chip next to the usb port): change to 3000000 (3 million) for even faster upload speed ;upload_speed = 921600 diff --git a/src/LEDManager.cpp b/src/LEDManager.cpp index 7484236..f184d55 100644 --- a/src/LEDManager.cpp +++ b/src/LEDManager.cpp @@ -202,9 +202,6 @@ namespace SlimeVR on(); m_CurrentStage = ON; break; - on(); - m_CurrentStage = ON; - break; } } else diff --git a/src/configuration/CalibrationConfig.h b/src/configuration/CalibrationConfig.h index 9de08a9..5b2a385 100644 --- a/src/configuration/CalibrationConfig.h +++ b/src/configuration/CalibrationConfig.h @@ -33,6 +33,10 @@ namespace SlimeVR { float A_B[3]; float A_Ainv[3][3]; + // magnetometer offsets and correction matrix + float M_B[3]; + float M_Ainv[3][3]; + // raw offsets, determined from gyro at rest float G_off[3]; diff --git a/src/configuration/Configuration.cpp b/src/configuration/Configuration.cpp index 845d004..a0264d4 100644 --- a/src/configuration/Configuration.cpp +++ b/src/configuration/Configuration.cpp @@ -27,6 +27,9 @@ #include "consts.h" #include "utils.h" +#define DIR_CALIBRATIONS "/calibrations" +#define DIR_TEMPERATURE_CALIBRATIONS "/tempcalibrations" + namespace SlimeVR { namespace Configuration { CalibrationConfig Configuration::m_EmptyCalibration = {NONE}; @@ -99,7 +102,7 @@ namespace SlimeVR { } char path[17]; - sprintf(path, "/calibrations/%d", i); + sprintf(path, DIR_CALIBRATIONS"/%d", i); m_Logger.trace("Saving calibration data for %d", i); @@ -156,34 +159,34 @@ namespace SlimeVR { void Configuration::loadCalibrations() { #ifdef ESP32 { - File calibrations = LittleFS.open("/calibrations"); + File calibrations = LittleFS.open(DIR_CALIBRATIONS); if (!calibrations) { m_Logger.warn("No calibration data found, creating new directory..."); - if (!LittleFS.mkdir("/calibrations")) { - m_Logger.error("Failed to create directory: /calibrations"); + if (!LittleFS.mkdir(DIR_CALIBRATIONS)) { + m_Logger.error("Failed to create directory: " DIR_CALIBRATIONS); return; } - calibrations = LittleFS.open("/calibrations"); + calibrations = LittleFS.open(DIR_CALIBRATIONS); } if (!calibrations.isDirectory()) { calibrations.close(); - m_Logger.warn("Found file instead of directory: /calibrations"); + m_Logger.warn("Found file instead of directory: " DIR_CALIBRATIONS); - if (!LittleFS.remove("/calibrations")) { - m_Logger.error("Failed to remove directory: /calibrations"); + if (!LittleFS.remove(DIR_CALIBRATIONS)) { + m_Logger.error("Failed to remove directory: " DIR_CALIBRATIONS); return; } - if (!LittleFS.mkdir("/calibrations")) { - m_Logger.error("Failed to create directory: /calibrations"); + if (!LittleFS.mkdir(DIR_CALIBRATIONS)) { + m_Logger.error("Failed to create directory: " DIR_CALIBRATIONS); return; } - calibrations = LittleFS.open("/calibrations"); + calibrations = LittleFS.open(DIR_CALIBRATIONS); } m_Logger.debug("Found calibration data directory"); @@ -192,9 +195,10 @@ namespace SlimeVR { if (f.isDirectory()) { continue; } - uint8_t sensorId = strtoul(f.name(), nullptr, 10); + m_Logger.trace("Found calibration data file: %s", f.name()); + uint8_t sensorId = strtoul(f.name(), nullptr, 10); CalibrationConfig calibrationConfig; f.read((uint8_t*)&calibrationConfig, sizeof(CalibrationConfig)); f.close(); @@ -208,11 +212,11 @@ namespace SlimeVR { } #else { - if (!LittleFS.exists("/calibrations")) { + if (!LittleFS.exists(DIR_CALIBRATIONS)) { m_Logger.warn("No calibration data found, creating new directory..."); - if (!LittleFS.mkdir("/calibrations")) { - m_Logger.error("Failed to create directory: /calibrations"); + if (!LittleFS.mkdir(DIR_CALIBRATIONS)) { + m_Logger.error("Failed to create directory: " DIR_CALIBRATIONS); return; } @@ -220,7 +224,7 @@ namespace SlimeVR { return; } - Dir calibrations = LittleFS.openDir("/calibrations"); + Dir calibrations = LittleFS.openDir(DIR_CALIBRATIONS); while (calibrations.next()) { File f = calibrations.openFile("r"); if (!f.isFile()) { @@ -239,6 +243,145 @@ namespace SlimeVR { #endif } + bool Configuration::loadTemperatureCalibration(uint8_t sensorId, GyroTemperatureCalibrationConfig& config) { +#ifdef ESP32 + { + File calibrations = LittleFS.open(DIR_TEMPERATURE_CALIBRATIONS); + if (!calibrations) { + m_Logger.warn("No temperature calibration data found, creating new directory..."); + + if (!LittleFS.mkdir(DIR_TEMPERATURE_CALIBRATIONS)) { + m_Logger.error("Failed to create directory: " DIR_TEMPERATURE_CALIBRATIONS); + return false; + } + + calibrations = LittleFS.open(DIR_TEMPERATURE_CALIBRATIONS); + } + + if (!calibrations.isDirectory()) { + calibrations.close(); + + m_Logger.warn("Found file instead of directory: " DIR_TEMPERATURE_CALIBRATIONS); + + if (!LittleFS.remove(DIR_TEMPERATURE_CALIBRATIONS)) { + m_Logger.error("Failed to remove directory: " DIR_TEMPERATURE_CALIBRATIONS); + return false; + } + + if (!LittleFS.mkdir(DIR_TEMPERATURE_CALIBRATIONS)) { + m_Logger.error("Failed to create directory: " DIR_TEMPERATURE_CALIBRATIONS); + return false; + } + + return false; + } + + calibrations.close(); + + m_Logger.debug("Found temperature calibration data directory"); + + char path[32]; + sprintf(path, DIR_TEMPERATURE_CALIBRATIONS"/%d", sensorId); + if (LittleFS.exists(path)) { + File f = LittleFS.open(path, "r"); + if (f.isDirectory()) { + return false; + } + + if (f.size() == sizeof(GyroTemperatureCalibrationConfig)) { + CalibrationConfigType storedConfigType; + f.read((uint8_t*)&storedConfigType, sizeof(CalibrationConfigType)); + if (storedConfigType != config.type) { + f.close(); + m_Logger.debug( + "Found incompatible sensor temperature calibration (expected %s, found %s) sensorId:%d, skipping", + calibrationConfigTypeToString(config.type), + calibrationConfigTypeToString(storedConfigType), + sensorId + ); + return false; + } + + f.seek(0); + f.read((uint8_t*)&config, sizeof(GyroTemperatureCalibrationConfig)); + f.close(); + m_Logger.debug("Found sensor temperature calibration for %s sensorId:%d", calibrationConfigTypeToString(config.type), sensorId); + return true; + } else { + m_Logger.debug("Found incompatible sensor temperature calibration (size mismatch) sensorId:%d, skipping", sensorId); + } + } + return false; + } +#else + { + if (!LittleFS.exists(DIR_TEMPERATURE_CALIBRATIONS)) { + m_Logger.warn("No temperature calibration data found, creating new directory..."); + + if (!LittleFS.mkdir(DIR_TEMPERATURE_CALIBRATIONS)) { + m_Logger.error("Failed to create directory: " DIR_TEMPERATURE_CALIBRATIONS); + return false; + } + + // There's no calibrations here, so we're done + return false; + } + + char path[32]; + sprintf(path, DIR_TEMPERATURE_CALIBRATIONS"/%d", sensorId); + if (LittleFS.exists(path)) { + File f = LittleFS.open(path, "r"); + if (!f.isFile()) { + return false; + } + + if (f.size() == sizeof(GyroTemperatureCalibrationConfig)) { + CalibrationConfigType storedConfigType; + f.read((uint8_t*)&storedConfigType, sizeof(CalibrationConfigType)); + if (storedConfigType != config.type) { + f.close(); + m_Logger.debug( + "Found incompatible sensor temperature calibration (expected %s, found %s) sensorId:%d, skipping", + calibrationConfigTypeToString(config.type), + calibrationConfigTypeToString(storedConfigType), + sensorId + ); + return false; + } + + f.seek(0); + f.read((uint8_t*)&config, sizeof(GyroTemperatureCalibrationConfig)); + f.close(); + m_Logger.debug("Found sensor temperature calibration for %s sensorId:%d", calibrationConfigTypeToString(config.type), sensorId); + return true; + } else { + m_Logger.debug("Found incompatible sensor temperature calibration (size mismatch) sensorId:%d, skipping", sensorId); + } + } + + return false; + } +#endif + } + + bool Configuration::saveTemperatureCalibration(uint8_t sensorId, const GyroTemperatureCalibrationConfig& config) { + if (config.type == CalibrationConfigType::NONE) { + return false; + } + + char path[32]; + sprintf(path, DIR_TEMPERATURE_CALIBRATIONS"/%d", sensorId); + + m_Logger.trace("Saving temperature calibration data for sensorId:%d", sensorId); + + File file = LittleFS.open(path, "w"); + file.write((uint8_t*)&config, sizeof(GyroTemperatureCalibrationConfig)); + file.close(); + + m_Logger.debug("Saved temperature calibration data for sensorId:%i", sensorId); + return true; + } + bool Configuration::runMigrations(int32_t version) { return true; } diff --git a/src/configuration/Configuration.h b/src/configuration/Configuration.h index 2bdeb74..7b191bd 100644 --- a/src/configuration/Configuration.h +++ b/src/configuration/Configuration.h @@ -28,6 +28,7 @@ #include "DeviceConfig.h" #include "logging/Logger.h" +#include "../motionprocessing/GyroTemperatureCalibrator.h" namespace SlimeVR { namespace Configuration { @@ -46,6 +47,9 @@ namespace SlimeVR { CalibrationConfig getCalibration(size_t sensorID) const; void setCalibration(size_t sensorID, const CalibrationConfig& config); + bool loadTemperatureCalibration(uint8_t sensorId, GyroTemperatureCalibrationConfig& config); + bool saveTemperatureCalibration(uint8_t sensorId, const GyroTemperatureCalibrationConfig& config); + private: void loadCalibrations(); bool runMigrations(int32_t version); diff --git a/src/consts.h b/src/consts.h index d36726f..14a4b83 100644 --- a/src/consts.h +++ b/src/consts.h @@ -65,6 +65,15 @@ #define DEG_180 PI #define DEG_270 PI / 2 +#define CONST_EARTH_GRAVITY 9.80665 + +#define ACCEL_CALIBRATION_METHOD_SKIP 0 +#define ACCEL_CALIBRATION_METHOD_ROTATION 1 +#define ACCEL_CALIBRATION_METHOD_6POINT 2 + +#define BMI160_MAG_TYPE_HMC 1 +#define BMI160_MAG_TYPE_QMC 2 + #ifdef ESP8266 #define HARDWARE_MCU 1 #elif defined(ESP32) diff --git a/src/defines_bmi160.h b/src/defines_bmi160.h new file mode 100644 index 0000000..2f788b2 --- /dev/null +++ b/src/defines_bmi160.h @@ -0,0 +1,85 @@ +/* + SlimeVR Code is placed under the MIT license + Copyright (c) 2022 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. +*/ +#ifndef BMI160_DEFINES_H +#define BMI160_DEFINES_H + +// BMI160 gyro/accel axis remapping. +#define BMI160_REMAP_AXIS_X(x, y, z) (sensorId == 0 ? x : x) +#define BMI160_REMAP_AXIS_Y(x, y, z) (sensorId == 0 ? y : y) +#define BMI160_REMAP_AXIS_Z(x, y, z) (sensorId == 0 ? z : z) +// Additional magnetometer remapping (if present and enabled in ). +#define BMI160_MAG_REMAP_AXIS_X(x, y, z) (sensorId == 0 ? x : x) +#define BMI160_MAG_REMAP_AXIS_Y(x, y, z) (sensorId == 0 ? y : y) +#define BMI160_MAG_REMAP_AXIS_Z(x, y, z) (sensorId == 0 ? z : z) + +// BMI160 magnetometer type, applies to both main and aux trackers, mixed types are not supported currently. +// If only 1 out of 2 trackers has a mag, tracker without a mag should still function normally. +// NOT USED if USE_6_AXIS == true +// Pick one: +#define BMI160_MAG_TYPE BMI160_MAG_TYPE_HMC +// #define BMI160_MAG_TYPE BMI160_MAG_TYPE_QMC + +// Use VQF instead of mahony sensor fusion. +// Features: rest bias estimation, magnetic distortion rejection. +#define BMI160_USE_VQF true + +// Use BasicVQF instead of VQF (if BMI160_USE_VQF == true). +// Disables the features above. +#define BMI160_USE_BASIC_VQF false + +// Use temperature calibration. +#define BMI160_USE_TEMPCAL true + +// How long to run gyro calibration for. +// Disables this calibration step if value is 0. +// Default: 5 +#define BMI160_CALIBRATION_GYRO_SECONDS 5 + +// Calibration method options: +// - Skip: disable this calibration step; +// - Rotation: rotate the device in hand; +// - 6 point: put the device in 6 unique orientations. +// Default: ACCEL_CALIBRATION_METHOD_6POINT +// #define BMI160_ACCEL_CALIBRATION_METHOD ACCEL_CALIBRATION_METHOD_SKIP +// #define BMI160_ACCEL_CALIBRATION_METHOD ACCEL_CALIBRATION_METHOD_ROTATION +#define BMI160_ACCEL_CALIBRATION_METHOD ACCEL_CALIBRATION_METHOD_6POINT + +// How long to run magnetometer calibration for, if enabled and you have added a magnetometer. +// Magnetometer not be used until you calibrate it. +// Disables this calibration step if value is 0. +// NOT USED if USE_6_AXIS == true +// Default: 20 +#define BMI160_CALIBRATION_MAG_SECONDS 20 + +// Send temperature to the server as AXXYY, +// where XX is calibration progress from 0 to 60, and YY is temperature, +// A is 1: not in calibration mode or 2: calibration in progress. +#define BMI160_TEMPCAL_DEBUG false + +// Print debug info every second. +#define BMI160_DEBUG false + +// Use sensitivity calibration. +#define BMI160_USE_SENSCAL true + +#endif \ No newline at end of file diff --git a/src/defines_sensitivity.h b/src/defines_sensitivity.h new file mode 100644 index 0000000..77c5ac0 --- /dev/null +++ b/src/defines_sensitivity.h @@ -0,0 +1,60 @@ +/* + SlimeVR Code is placed under the MIT license + Copyright (c) 2022 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. +*/ + +#ifndef DEFINES_SENSITIVITY_H +#define DEFINES_SENSITIVITY_H + +// ========================== +// Sensitivity calibration +// Only for: BMI160 +// ========================== + +// This type of calibration attempts to reduce "extra degrees measures", +// useful if you do 360 spins + +// Trackers are identified by sensor id and MAC address of the device, +// which is displayed in serial when you upload firmware + +// Number of spins determines calibration accuracy + +// 1. Put the axis you want to calibrate vertically +// 2. Put the tracker into an orientation that you can repeat later +// 3. Do a full reset, rotation must now be 0 0 0 +// 4. Spin the tracker clockwise <.spins> times around the axis +// 5. Put the tracker back into the reset position +// 6. Write down the resulting Y (yaw) value into the table below +// Reflash, do the same rotation and check if the resulting angle is now 0 + +#define SENSORID_PRIMARY 0 +#define SENSORID_AUX 1 +struct SensitivityOffsetXYZ { const char* mac; unsigned char sensorId; double spins; double x; double y; double z; }; +const SensitivityOffsetXYZ sensitivityOffsets[] = { + // example values + { "A4:E5:7C:B6:00:01", SENSORID_PRIMARY, .spins = 10, .x = 2.63, .y = 37.82, .z = 31.11 }, + { "A4:E5:7C:B6:00:02", SENSORID_PRIMARY, .spins = 10, .x = -2.38, .y = -26.8, .z = -42.78 }, + { "A4:E5:7C:B6:00:03", SENSORID_PRIMARY, .spins = 10, .x = 11, .y = 2.2, .z = -1 }, + { "A4:E5:7C:B6:00:04", SENSORID_PRIMARY, .spins = 10, .x = -7, .y = -53.7, .z = -57 }, + { "A4:E5:7C:B6:00:05", SENSORID_PRIMARY, .spins = 10, .x = -10.63, .y = -8.25, .z = -18.6 }, +}; + +#endif \ No newline at end of file diff --git a/src/globals.h b/src/globals.h index 20eb48f..8667ef1 100644 --- a/src/globals.h +++ b/src/globals.h @@ -27,6 +27,8 @@ #include "consts.h" #include "debug.h" #include "defines.h" +#include "defines_bmi160.h" +#include "defines_sensitivity.h" #ifndef SECOND_IMU #define SECOND_IMU IMU diff --git a/src/main.cpp b/src/main.cpp index 5156850..6234454 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -71,7 +71,7 @@ void setup() SerialCommands::setUp(); -#if IMU == IMU_MPU6500 || IMU == IMU_MPU6050 || IMU == IMU_MPU9250 || IMU == IMU_BNO055 || IMU == IMU_ICM20948 +#if IMU == IMU_MPU6500 || IMU == IMU_MPU6050 || IMU == IMU_MPU9250 || IMU == IMU_BNO055 || IMU == IMU_ICM20948 || IMU == IMU_BMI160 I2CSCAN::clearBus(PIN_IMU_SDA, PIN_IMU_SCL); // Make sure the bus isn't stuck when resetting ESP without powering it down // Fixes I2C issues for certain IMUs. Only has been tested on IMUs above. Testing advised when adding other IMUs. #endif diff --git a/src/motionprocessing/GyroTemperatureCalibrator.cpp b/src/motionprocessing/GyroTemperatureCalibrator.cpp new file mode 100644 index 0000000..6fe8d4d --- /dev/null +++ b/src/motionprocessing/GyroTemperatureCalibrator.cpp @@ -0,0 +1,211 @@ +/* + SlimeVR Code is placed under the MIT license + Copyright (c) 2022 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. +*/ + +#include "GyroTemperatureCalibrator.h" +#include "GlobalVars.h" + +void GyroTemperatureCalibrator::resetCurrentTemperatureState() { + if (!state.numSamples) return; + state.numSamples = 0; + state.tSum = 0; + state.xSum = 0; + state.ySum = 0; + state.zSum = 0; +} + +// must be called for every raw gyro sample +void GyroTemperatureCalibrator::updateGyroTemperatureCalibration(const float temperature, const bool restDetected, int16_t x, int16_t y, int16_t z) { + if (!restDetected) { + return resetCurrentTemperatureState(); + } + + if (temperature < TEMP_CALIBRATION_MIN && !calibrationRunning) { + calibrationRunning = true; + configSaved = false; + config.reset(); + } + if (temperature > TEMP_CALIBRATION_MAX && calibrationRunning) { + auto coeffs = poly.computeCoefficients(); + for (uint32_t i = 0; i < poly.numCoefficients; i++) { + config.cx[i] = coeffs[0][i]; + config.cy[i] = coeffs[1][i]; + config.cz[i] = coeffs[2][i]; + } + config.hasCoeffs = true; + bst = 0.0f; + bsx = 0; + bsy = 0; + bsz = 0; + bn = 0; + lastTemp = 0; + calibrationRunning = false; + if (!configSaveFailed && !configSaved) { + saveConfig(); + } + } + if (calibrationRunning) { + if (fabs(lastTemp - temperature) > 0.03f) { + const double avgt = (double)bst / bn; + const double avgValues[] = { + (double)bsx / bn, + (double)bsy / bn, + (double)bsz / bn, + }; + if (bn > 0) poly.update(avgt, avgValues); + bst = 0.0f; + bsx = 0; + bsy = 0; + bsz = 0; + bn = 0; + lastTemp = temperature; + } else { + bst += temperature; + bsx += x; + bsy += y; + bsz += z; + bn++; + } + } + + const int16_t idx = TEMP_CALIBRATION_TEMP_TO_IDX(temperature); + + if (idx < 0 || idx >= TEMP_CALIBRATION_BUFFER_SIZE) return; + + bool currentTempAlreadyCalibrated = config.samples[idx].t != 0.0f; + if (currentTempAlreadyCalibrated) return; + + if (state.temperatureCurrentIdx != idx) { + state.temperatureCurrentIdx = idx; + resetCurrentTemperatureState(); + } + + float temperatureStepBoundsMin = TEMP_CALIBRATION_IDX_TO_TEMP(idx) - TEMP_CALIBRATION_MAX_DEVIATION_FROM_STEP; + float temperatureStepBoundsMax = TEMP_CALIBRATION_IDX_TO_TEMP(idx) + TEMP_CALIBRATION_MAX_DEVIATION_FROM_STEP; + bool isTemperatureOutOfDeviationRange = + temperature < temperatureStepBoundsMin || temperature > temperatureStepBoundsMax; + if (isTemperatureOutOfDeviationRange) { + return resetCurrentTemperatureState(); + } + + state.numSamples++; + state.tSum += temperature; + state.xSum += x; + state.ySum += y; + state.zSum += z; + if (state.numSamples > samplesPerStep) { + bool currentTempAlreadyCalibrated = config.samples[idx].t != 0.0f; + if (!currentTempAlreadyCalibrated) { + config.samplesTotal++; + } + config.samples[idx].t = state.tSum / state.numSamples; + config.samples[idx].x = ((double)state.xSum / state.numSamples); + config.samples[idx].y = ((double)state.ySum / state.numSamples); + config.samples[idx].z = ((double)state.zSum / state.numSamples); + + config.minTemperatureRange = + min(config.samples[idx].t, config.minTemperatureRange); + config.maxTemperatureRange = + max(config.samples[idx].t, config.maxTemperatureRange); + config.minCalibratedIdx = + TEMP_CALIBRATION_TEMP_TO_IDX(config.minTemperatureRange); + config.maxCalibratedIdx = + TEMP_CALIBRATION_TEMP_TO_IDX(config.maxTemperatureRange); + resetCurrentTemperatureState(); + } +} + +bool GyroTemperatureCalibrator::approximateOffset(const float temperature, float GOxyz[3]) { + if (!config.hasData()) return false; + if (config.hasCoeffs) { + if (lastApproximatedTemperature != 0.0f && temperature == lastApproximatedTemperature) { + GOxyz[0] = lastApproximatedOffsets[0]; + GOxyz[1] = lastApproximatedOffsets[1]; + GOxyz[2] = lastApproximatedOffsets[2]; + } else { + float offsets[3] = { config.cx[3], config.cy[3], config.cz[3] }; + for (int32_t i = 2; i >= 0; i--) { + offsets[0] = offsets[0] * temperature + config.cx[i]; + offsets[1] = offsets[1] * temperature + config.cy[i]; + offsets[2] = offsets[2] * temperature + config.cz[i]; + } + lastApproximatedTemperature = temperature; + lastApproximatedOffsets[0] = GOxyz[0] = offsets[0]; + lastApproximatedOffsets[1] = GOxyz[1] = offsets[1]; + lastApproximatedOffsets[2] = GOxyz[2] = offsets[2]; + } + return true; + } + + const float constrainedTemperature = constrain(temperature, + config.minTemperatureRange, + config.maxTemperatureRange + ); + + const int16_t idx = + TEMP_CALIBRATION_TEMP_TO_IDX(constrainedTemperature); + + if (idx < 0 || idx >= TEMP_CALIBRATION_BUFFER_SIZE) return false; + + bool isCurrentTempCalibrated = config.samples[idx].t != 0.0f; + if (isCurrentTempCalibrated) { + GOxyz[0] = config.samples[idx].x; + GOxyz[1] = config.samples[idx].y; + GOxyz[2] = config.samples[idx].z; + return true; + } + + return false; +} + +bool GyroTemperatureCalibrator::loadConfig(float newSensitivity) { + bool ok = configuration.loadTemperatureCalibration(sensorId, config); + if (ok) { + config.rescaleSamples(newSensitivity); + if (config.fullyCalibrated()) { + configSaved = true; + } + } else { + m_Logger.warn("No temperature calibration data found for sensor %d, ignoring...", sensorId); + // m_Logger.info("Temperature calibration is advised"); + m_Logger.info("Temperature calibration is a work-in-progress feature; any changes to its parameters or updates will render the saved temperature curve invalid and unloadable."); + } + return configSaved; +} + +bool GyroTemperatureCalibrator::saveConfig() { + if (configuration.saveTemperatureCalibration(sensorId, config)) { + m_Logger.info("Saved temperature calibration config (%0.1f%) for sensorId:%i", + config.getCalibrationDonePercent(), + sensorId + ); + if (config.fullyCalibrated()) { + configSaved = true; + } else { + m_Logger.info("Calibration will resume from this checkpoint after reboot"); + } + } else { + configSaveFailed = true; + m_Logger.error("Something went wrong"); + } + return configSaved; +} diff --git a/src/motionprocessing/GyroTemperatureCalibrator.h b/src/motionprocessing/GyroTemperatureCalibrator.h new file mode 100644 index 0000000..eb27f7f --- /dev/null +++ b/src/motionprocessing/GyroTemperatureCalibrator.h @@ -0,0 +1,219 @@ +/* + SlimeVR Code is placed under the MIT license + Copyright (c) 2022 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. +*/ + +#ifndef GYRO_TEMPERATURE_CALIBRATOR_H +#define GYRO_TEMPERATURE_CALIBRATOR_H + +#include +#include +#include "debug.h" +#include "../logging/Logger.h" +#include "../configuration/CalibrationConfig.h" +#include "OnlinePolyfit.h" + + +// Degrees C +// default: 15.0f +#define TEMP_CALIBRATION_MIN 15.0f + +// Degrees C +// default: 45.0f +#define TEMP_CALIBRATION_MAX 45.0f + +// Snap calibration to every 1/2 of degree: 20.00, 20.50, 21.00, etc +// default: 0.5f +#define TEMP_CALIBRATION_STEP 0.5f + +// Record debug samples if current temperature is off by no more than this value; +// if snapping point is 20.00 - samples will be recorded in range of 19.80 - 20.20 +// default: 0.2f +#define TEMP_CALIBRATION_MAX_DEVIATION_FROM_STEP 0.2f + +// How long to average gyro samples for before saving a data point +// default: 0.2f +#define TEMP_CALIBRATION_SECONDS_PER_STEP 0.2f + +#if IMU == IMU_ICM20948 + // 16 bit 333 lsb/K, ~0.00508 degrees per bit + // already calibrated by DMP? +#elif IMU == IMU_MPU6500 || IMU == IMU_MPU6050 + // 16 bit 340 lsb/K, ~0.00518 degrees per bit +#elif IMU == IMU_MPU9250 + // 16 bit 333 lsb/K, ~0.00508 degrees per bit +#elif IMU == IMU_BMI160 + // 16 bit 128 lsb/K, ~0.00195 degrees per bit +#endif + + +constexpr uint16_t TEMP_CALIBRATION_BUFFER_SIZE = + (uint16_t)((TEMP_CALIBRATION_MAX - TEMP_CALIBRATION_MIN) * (1/TEMP_CALIBRATION_STEP)); + +#define TEMP_CALIBRATION_TEMP_TO_IDX(temperature) (uint16_t)( \ + (temperature + TEMP_CALIBRATION_STEP/2.0f) * (1/TEMP_CALIBRATION_STEP) - \ + TEMP_CALIBRATION_MIN * (1/TEMP_CALIBRATION_STEP) \ +) + +#define TEMP_CALIBRATION_IDX_TO_TEMP(idx) (float)( \ + ((float)idx / (1.0f/TEMP_CALIBRATION_STEP)) + \ + TEMP_CALIBRATION_MIN \ +) + +struct GyroTemperatureCalibrationState { + uint16_t temperatureCurrentIdx; + uint32_t numSamples; + float tSum; + int32_t xSum; + int32_t ySum; + int32_t zSum; + + GyroTemperatureCalibrationState(): temperatureCurrentIdx(-1), numSamples(0), tSum(0.0f), xSum(0), ySum(0), zSum(0) + { }; +}; + +struct GyroTemperatureOffsetSample { + float t; + float x; + float y; + float z; + + GyroTemperatureOffsetSample(): t(0.0f), x(0), y(0), z(0) + { } +}; + +struct GyroTemperatureCalibrationConfig { + SlimeVR::Configuration::CalibrationConfigType type; + + float sensitivityLSB; + float minTemperatureRange; + float maxTemperatureRange; + uint16_t minCalibratedIdx = 0; + uint16_t maxCalibratedIdx = 0; + GyroTemperatureOffsetSample samples[TEMP_CALIBRATION_BUFFER_SIZE]; + uint16_t samplesTotal = 0; + float cx[4] = {0.0}; + float cy[4] = {0.0}; + float cz[4] = {0.0}; + bool hasCoeffs = false; + + GyroTemperatureCalibrationConfig(SlimeVR::Configuration::CalibrationConfigType _type, float _sensitivityLSB) : + type(_type), + sensitivityLSB(_sensitivityLSB), + minTemperatureRange(1000), + maxTemperatureRange(-1000) + { } + + bool hasData() { + return minTemperatureRange != 1000; + } + + bool fullyCalibrated() { + return samplesTotal >= TEMP_CALIBRATION_BUFFER_SIZE && hasCoeffs; + } + + float getCalibrationDonePercent() { + return (float)samplesTotal / TEMP_CALIBRATION_BUFFER_SIZE * 100.0f; + } + + void rescaleSamples(float newSensitivityLSB) { + if (sensitivityLSB == newSensitivityLSB) return; + float mul = newSensitivityLSB / sensitivityLSB; + for (int i = 0; i < TEMP_CALIBRATION_BUFFER_SIZE; i++) { + if (samples[i].t == 0) continue; + samples[i].x *= mul; + samples[i].y *= mul; + samples[i].z *= mul; + } + sensitivityLSB = newSensitivityLSB; + } + + void reset() { + minTemperatureRange = 1000; + maxTemperatureRange = -1000; + samplesTotal = 0; + for (int i = 0; i < TEMP_CALIBRATION_BUFFER_SIZE; i++) { + samples[i].t = 0; + samples[i].x = 0; + samples[i].y = 0; + samples[i].z = 0; + } + hasCoeffs = false; + } +}; + +class GyroTemperatureCalibrator { +public: + uint8_t sensorId; + GyroTemperatureCalibrationConfig config; + + // set when config is fully calibrated is saved OR on startup when loaded config is fully calibrated; + // left unset when sending saving command over serial so it can continue calibration and autosave later + bool configSaved = false; + bool configSaveFailed = false; + + GyroTemperatureCalibrator(SlimeVR::Configuration::CalibrationConfigType _configType, uint8_t _sensorId, float sensitivity, uint32_t _samplesPerStep): + sensorId(_sensorId), + config(_configType, sensitivity), + samplesPerStep(_samplesPerStep), + m_Logger(SlimeVR::Logging::Logger("GyroTemperatureCalibrator")) + { + char buf[4]; + sprintf(buf, "%u", _sensorId); + m_Logger.setTag(buf); + } + + void updateGyroTemperatureCalibration(const float temperature, const bool restDetected, int16_t x, int16_t y, int16_t z); + bool approximateOffset(const float temperature, float GOxyz[3]); + bool loadConfig(float newSensitivity); + bool saveConfig(); + + void reset() { + config.reset(); + configSaved = false; + configSaveFailed = false; + } + + bool isCalibrating() { + return calibrationRunning; + } + +private: + GyroTemperatureCalibrationState state; + uint32_t samplesPerStep; + SlimeVR::Logging::Logger m_Logger; + + float lastApproximatedTemperature = 0.0f; + float lastApproximatedOffsets[3]; + + bool calibrationRunning = false; + OnlineVectorPolyfit<3, 3, (uint64_t)1e9> poly; + float bst = 0.0f; + int32_t bsx = 0; + int32_t bsy = 0; + int32_t bsz = 0; + int32_t bn = 0; + float lastTemp = 0; + + void resetCurrentTemperatureState(); +}; + +#endif \ No newline at end of file diff --git a/src/motionprocessing/OnlinePolyfit.h b/src/motionprocessing/OnlinePolyfit.h new file mode 100644 index 0000000..8ee7119 --- /dev/null +++ b/src/motionprocessing/OnlinePolyfit.h @@ -0,0 +1,125 @@ +/* + SlimeVR Code is placed under the MIT license + Copyright (c) 2022 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. +*/ +#include + +#ifndef ONLINE_POLYFIT_H +#define ONLINE_POLYFIT_H + +template +class OnlineVectorPolyfit { +public: + constexpr static int32_t numDimensions = dimensions; + constexpr static int32_t numCoefficients = degree + 1; + constexpr static double forgettingFactor = std::exp(-1.0/forgettingFactorNumSamples); + + OnlineVectorPolyfit() { + reset(); + } + + void reset() { + std::fill(Rb[0], Rb[0] + rows * cols, 0.0); + std::fill(coeffs[0], coeffs[0] + numDimensions * rows, 0.0f); + } + + // Recursive least squares update using QR decomposition by Givens transformations + void update(double xValue, const double yValues[numDimensions]) { + double xin[cols]; + xin[0] = 1; + for (int32_t i = 1; i < cols - numDimensions; i++) { + xin[i] = xin[i - 1] * xValue; + } + for (int32_t i = 0; i < numDimensions; i++) { + xin[cols - numDimensions + i] = yValues[i]; + } + + // degree = 3, dimensions = 3, yValues = [x, y, z] + // B I I I Ix Iy Iz R R R R bx by bz + // . B I I Ix Iy Iz === . R R R bx by bz + // . . B I Ix Iy Iz === . . R R bx by bz + // . . . B Ix Iy Iz . . . R bx by bz + + // https://www.eecs.harvard.edu/~htk/publication/1981-matrix-triangularization-by-systolic-arrays.pdf + + for (int32_t y = 0; y < rows; y++) { + double c = 1, s = 0; + if (xin[y] != 0.0) { + Rb[y][y] *= forgettingFactor; + const double norm = sqrt(Rb[y][y] * Rb[y][y] + xin[y] * xin[y]); + c = Rb[y][y] * (1.0 / norm); + s = xin[y] * (1.0 / norm); + Rb[y][y] = norm; + } + for (int32_t x = y + 1; x < cols; x++) { + Rb[y][x] *= forgettingFactor; + const double xout = (c * xin[x] - s * Rb[y][x]); + Rb[y][x] = (s * xin[x] + c * Rb[y][x]); + xin[x] = xout; + } + } + } + + // Back solves upper triangular system + // Returns float[numDimensions][numCoefficients] coefficients from lowest to highest power + const auto computeCoefficients() { + // https://en.wikipedia.org/wiki/Triangular_matrix#Forward_and_back_substitution + for (int32_t d = 0; d < numDimensions; d++) { + for (int32_t y = rows - 1; y >= 0; y--) { + const int32_t bColumn = cols - numDimensions + d; + coeffs[d][y] = Rb[y][bColumn]; + if (Rb[y][y] == 0.0) continue; + for (int32_t x = y + 1; x < rows; x++) { + coeffs[d][y] -= coeffs[d][x] * Rb[y][x]; + } + coeffs[d][y] /= Rb[y][y]; + } + } + return coeffs; + } + + float predict(int32_t d, float x) { + if (d >= numDimensions) return 0.0; + // https://en.wikipedia.org/wiki/Horner%27s_method + float y = coeffs[d][numCoefficients - 1]; + for (int32_t i = numCoefficients - 2; i >= 0; i--) { + y = y * x + coeffs[d][i]; + } + return y; + } + + std::pair tangentAt(float x) { + float intercept = coeffs[0]; + float slope = coeffs[1]; + for (uint32_t i = 2; i < degree + 1; i++) { + intercept -= coeffs[i] * (i - 1) * pow(x, i); + slope += coeffs[i] * i * pow(x, i - 1); + } + return std::make_pair(slope, intercept); + } +private: + constexpr static int32_t rows = numCoefficients; + constexpr static int32_t cols = numCoefficients + 3; + double Rb[rows][cols]; + float coeffs[numDimensions][rows]; +}; + +#endif \ No newline at end of file diff --git a/src/motionprocessing/RestDetection.h b/src/motionprocessing/RestDetection.h new file mode 100644 index 0000000..c0215ba --- /dev/null +++ b/src/motionprocessing/RestDetection.h @@ -0,0 +1,262 @@ +// SPDX-FileCopyrightText: 2021 Daniel Laidig +// SPDX-FileCopyrightText: 2022 SlimeVR Contributors +// +// SPDX-License-Identifier: MIT + +// Separated and modified from VQF + +#ifndef REST_DETECTION_H +#define REST_DETECTION_H + +// #define REST_DETECTION_DISABLE_LPF + +#include +#include +#include +#include "types.h" + +#define NaN std::numeric_limits::quiet_NaN() + +struct RestDetectionParams { + sensor_real_t biasClip; + sensor_real_t biasSigmaRest; + uint32_t restMinTimeMicros; + sensor_real_t restFilterTau; + sensor_real_t restThGyr; + sensor_real_t restThAcc; + RestDetectionParams(): + biasClip(2.0f), + biasSigmaRest(0.03f), + restMinTimeMicros(1.5 * 1e6), + restFilterTau(0.5f), + restThGyr(2.0f), + restThAcc(0.5f) + { } +}; + +inline sensor_real_t square(sensor_real_t x) { return x * x; } + +class RestDetection { +public: + RestDetection(sensor_real_t gyrTs, sensor_real_t accTs) { + this->gyrTs = gyrTs; + this->accTs = accTs; + setup(); + } + RestDetection(const RestDetectionParams ¶ms, sensor_real_t gyrTs, sensor_real_t accTs) { + this->params = params; + this->gyrTs = gyrTs; + this->accTs = accTs; + setup(); + } + +#ifndef REST_DETECTION_DISABLE_LPF + void filterInitialState(sensor_real_t x0, const double b[3], const double a[2], double out[]) + { + // initial state for steady state (equivalent to scipy.signal.lfilter_zi, obtained by setting y=x=x0 in the filter + // update equation) + out[0] = x0*(1 - b[0]); + out[1] = x0*(b[2] - a[1]); + } + + sensor_real_t filterStep(sensor_real_t x, const double b[3], const double a[2], double state[2]) + { + // difference equations based on scipy.signal.lfilter documentation + // assumes that a0 == 1.0 + double y = b[0]*x + state[0]; + state[0] = b[1]*x - a[0]*y + state[1]; + state[1] = b[2]*x - a[1]*y; + return y; + } + + + void filterVec(const sensor_real_t x[], size_t N, sensor_real_t tau, sensor_real_t Ts, const double b[3], + const double a[2], double state[], sensor_real_t out[]) + { + assert(N>=2); + + // to avoid depending on a single sample, average the first samples (for duration tau) + // and then use this average to calculate the filter initial state + if (isnan(state[0])) { // initialization phase + if (isnan(state[1])) { // first sample + state[1] = 0; // state[1] is used to store the sample count + for(size_t i = 0; i < N; i++) { + state[2+i] = 0; // state[2+i] is used to store the sum + } + } + state[1]++; + for (size_t i = 0; i < N; i++) { + state[2+i] += x[i]; + out[i] = state[2+i]/state[1]; + } + if (state[1]*Ts >= tau) { + for(size_t i = 0; i < N; i++) { + filterInitialState(out[i], b, a, state+2*i); + } + } + return; + } + + for (size_t i = 0; i < N; i++) { + out[i] = filterStep(x[i], b, a, state+2*i); + } + } +#endif + + void updateGyr(uint32_t dtMicros, sensor_real_t gyr[3]) { +#ifdef REST_DETECTION_DISABLE_LPF + gyrLastSquaredDeviation = + square(gyr[0] - lastSample.gyr[0]) + + square(gyr[1] - lastSample.gyr[1]) + + square(gyr[2] - lastSample.gyr[2]); + + sensor_real_t biasClip = params.biasClip*sensor_real_t(M_PI/180.0); + if (gyrLastSquaredDeviation >= square(params.restThGyr*sensor_real_t(M_PI/180.0)) + || fabs(lastSample.gyr[0]) > biasClip || fabs(lastSample.gyr[1]) > biasClip + || fabs(lastSample.gyr[2]) > biasClip) { + restTimeMicros = 0; + restDetected = false; + } + + lastSample.gyr[0] = gyr[0]; + lastSample.gyr[1] = gyr[1]; + lastSample.gyr[2] = gyr[2]; +#else + filterVec(gyr, 3, params.restFilterTau, gyrTs, restGyrLpB, restGyrLpA, + restGyrLpState, restLastGyrLp); + + gyrLastSquaredDeviation = + square(gyr[0] - restLastGyrLp[0]) + + square(gyr[1] - restLastGyrLp[1]) + + square(gyr[2] - restLastGyrLp[2]); + + sensor_real_t biasClip = params.biasClip*sensor_real_t(M_PI/180.0); + if (gyrLastSquaredDeviation >= square(params.restThGyr*sensor_real_t(M_PI/180.0)) + || fabs(restLastGyrLp[0]) > biasClip || fabs(restLastGyrLp[1]) > biasClip + || fabs(restLastGyrLp[2]) > biasClip) { + restTimeMicros = 0; + restDetected = false; + } +#endif + } + + void updateAcc(uint32_t dtMicros, sensor_real_t acc[3]) { + if (acc[0] == sensor_real_t(0.0) && acc[1] == sensor_real_t(0.0) && acc[2] == sensor_real_t(0.0)) { + return; + } + +#ifdef REST_DETECTION_DISABLE_LPF + accLastSquaredDeviation = + square(acc[0] - lastSample.acc[0]) + + square(acc[1] - lastSample.acc[1]) + + square(acc[2] - lastSample.acc[2]); + + if (accLastSquaredDeviation >= square(params.restThAcc)) { + restTimeMicros = 0; + restDetected = false; + } else { + restTimeMicros += dtMicros; + if (restTimeMicros >= params.restMinTimeMicros) { + restDetected = true; + } + } + + lastSample.acc[0] = acc[0]; + lastSample.acc[1] = acc[1]; + lastSample.acc[2] = acc[2]; +#else + filterVec(acc, 3, params.restFilterTau, accTs, restAccLpB, restAccLpA, + restAccLpState, restLastAccLp); + + accLastSquaredDeviation = + square(acc[0] - restLastAccLp[0]) + + square(acc[1] - restLastAccLp[1]) + + square(acc[2] - restLastAccLp[2]); + + if (accLastSquaredDeviation >= square(params.restThAcc)) { + restTimeMicros = 0; + restDetected = false; + } else { + restTimeMicros += dtMicros; + if (restTimeMicros >= params.restMinTimeMicros) { + restDetected = true; + } + } +#endif + } + + bool getRestDetected() { + return restDetected; + } + +#ifndef REST_DETECTION_DISABLE_LPF + void resetState() { + restDetected = false; + + gyrLastSquaredDeviation = 0.0; + accLastSquaredDeviation = 0.0; + restTimeMicros = 0.0; + std::fill(restLastGyrLp, restLastGyrLp + 3, 0.0); + std::fill(restGyrLpState, restGyrLpState + 3*2, NaN); + std::fill(restLastAccLp, restLastAccLp + 3, 0.0); + std::fill(restAccLpState, restAccLpState + 3*2, NaN); + } + + void filterCoeffs(sensor_real_t tau, sensor_real_t Ts, double outB[], double outA[]) { + assert(tau > 0); + assert(Ts > 0); + // second order Butterworth filter based on https://stackoverflow.com/a/52764064 + double fc = (M_SQRT2 / (2.0*M_PI))/double(tau); // time constant of dampened, non-oscillating part of step response + double C = tan(M_PI*fc*double(Ts)); + double D = C*C + sqrt(2)*C + 1; + double b0 = C*C/D; + outB[0] = b0; + outB[1] = 2*b0; + outB[2] = b0; + // a0 = 1.0 + outA[0] = 2*(C*C-1)/D; // a1 + outA[1] = (1-sqrt(2)*C+C*C)/D; // a2 + } +#endif + + void setup() { +#ifndef REST_DETECTION_DISABLE_LPF + assert(gyrTs > 0); + assert(accTs > 0); + + filterCoeffs(params.restFilterTau, gyrTs, restGyrLpB, restGyrLpA); + filterCoeffs(params.restFilterTau, accTs, restAccLpB, restAccLpA); + + resetState(); +#endif + } + + +private: + RestDetectionParams params; + bool restDetected; + uint32_t restTimeMicros; + sensor_real_t gyrLastSquaredDeviation = 0; + sensor_real_t accLastSquaredDeviation = 0; + + sensor_real_t gyrTs; + sensor_real_t accTs; +#ifndef REST_DETECTION_DISABLE_LPF + sensor_real_t restLastGyrLp[3]; + double restGyrLpState[3*2]; + double restGyrLpB[3]; + double restGyrLpA[2]; + sensor_real_t restLastAccLp[3]; + double restAccLpState[3*2]; + double restAccLpB[3]; + double restAccLpA[2]; +#else + struct { + float gyr[3]; + float acc[3]; + } lastSample; +#endif +}; + + +#endif \ No newline at end of file diff --git a/src/motionprocessing/types.h b/src/motionprocessing/types.h new file mode 100644 index 0000000..f9bf383 --- /dev/null +++ b/src/motionprocessing/types.h @@ -0,0 +1,10 @@ +#ifndef MOTIONPROCESSING_TYPES_H +#define MOTIONPROCESSING_TYPES_H + +#if not defined(VQF_SINGLE_PRECISION) && BMI160_USE_VQF + typedef double sensor_real_t; +#else + typedef float sensor_real_t; +#endif + +#endif \ No newline at end of file diff --git a/src/sensors/bmi160sensor.cpp b/src/sensors/bmi160sensor.cpp index 7a362d3..4fee5cb 100644 --- a/src/sensors/bmi160sensor.cpp +++ b/src/sensors/bmi160sensor.cpp @@ -24,46 +24,96 @@ #include "bmi160sensor.h" #include "network/network.h" #include "GlobalVars.h" +#include +#include +#include -// Typical sensitivity at 25C -// See p. 9 of https://www.mouser.com/datasheet/2/783/BST-BMI160-DS000-1509569.pdf -// 65.6 LSB/deg/s = 500 deg/s -#define TYPICAL_SENSITIVITY_LSB 65.6 +void BMI160Sensor::initHMC(BMI160MagRate magRate) { + /* Configure MAG interface and setup mode */ + /* Set MAG interface normal power mode */ + imu.setRegister(BMI160_RA_CMD, BMI160_CMD_MAG_MODE_NORMAL); + delay(60); -// Scale conversion steps: LSB/°/s -> °/s -> step/°/s -> step/rad/s -constexpr float GSCALE = ((32768. / TYPICAL_SENSITIVITY_LSB) / 32768.) * (PI / 180.0); + imu.setRegister(BMI160_RA_CMD, BMI160_EN_PULL_UP_REG_1); + imu.setRegister(BMI160_RA_CMD, BMI160_EN_PULL_UP_REG_2); + imu.setRegister(BMI160_RA_CMD, BMI160_EN_PULL_UP_REG_3); + imu.setRegister(BMI160_7F, BMI160_EN_PULL_UP_REG_4); + imu.setRegister(BMI160_7F, BMI160_EN_PULL_UP_REG_5); -#define ACCEL_SENSITIVITY_4G 8192.0f + /* Enable MAG interface */ + imu.setRegister(BMI160_RA_IF_CONF, BMI160_IF_CONF_MODE_PRI_AUTO_SEC_MAG); + delay(1); -// Accel scale conversion steps: LSB/G -> G -> m/s^2 -constexpr float ASCALE_4G = ((32768. / ACCEL_SENSITIVITY_4G) / 32768.) * EARTH_GRAVITY; + imu.setMagDeviceAddress(HMC_DEVADDR); + delay(3); + imu.setRegister(BMI160_RA_MAG_IF_1_MODE, BMI160_MAG_SETUP_MODE); + delay(3); -// LSB change per temperature step map. -// These values were calculated for 500 deg/s sensitivity -// Step quantization - 5 degrees per step -const float LSB_COMP_PER_TEMP_X_MAP[13] = { - 0.77888f, 1.01376f, 0.83848f, 0.39416f, // 15, 20, 25, 30 - -0.08792f, -0.01576f, -0.1018f, 0.22208f, // 35, 40, 45, 50 - 0.22208f, 0.22208f, 0.22208f, 0.2316f, // 55, 60, 65, 70 - 0.53416f // 75 -}; -const float LSB_COMP_PER_TEMP_Y_MAP[13] = { - 0.10936f, 0.24392f, 0.28816f, 0.24096f, - 0.05376f, -0.1464f, -0.22664f, -0.23864f, - -0.25064f, -0.26592f, -0.28064f, -0.30224f, - -0.31608f -}; -const float LSB_COMP_PER_TEMP_Z_MAP[13] = { - 0.15136f, 0.04472f, 0.02528f, -0.07056f, - 0.03184f, -0.002f, -0.03888f, -0.14f, - -0.14488f, -0.14976f, -0.15656f, -0.16108f, - -0.1656f -}; + /* Configure HMC5883L Sensor */ + imu.setMagRegister(HMC_RA_CFGA, HMC_CFGA_DATA_RATE_75 | HMC_CFGA_AVG_SAMPLES_8 | HMC_CFGA_BIAS_NORMAL); + imu.setMagRegister(HMC_RA_CFGB, HMC_CFGB_GAIN_1_30); + imu.setMagRegister(HMC_RA_MODE, HMC_MODE_HIGHSPEED | HMC_MODE_READ_CONTINUOUS); + + imu.setRegister(BMI160_RA_MAG_IF_2_READ_RA, HMC_RA_DATA); + imu.setRegister(BMI160_RA_MAG_CONF, magRate); + delay(3); + imu.setRegister(BMI160_RA_MAG_IF_1_MODE, BMI160_MAG_DATA_MODE_6); +} + +void BMI160Sensor::initQMC(BMI160MagRate magRate) { + /* Configure MAG interface and setup mode */ + /* Set MAG interface normal power mode */ + imu.setRegister(BMI160_RA_CMD, BMI160_CMD_MAG_MODE_NORMAL); + delay(60); + + imu.setRegister(BMI160_RA_CMD, BMI160_EN_PULL_UP_REG_1); + imu.setRegister(BMI160_RA_CMD, BMI160_EN_PULL_UP_REG_2); + imu.setRegister(BMI160_RA_CMD, BMI160_EN_PULL_UP_REG_3); + imu.setRegister(BMI160_7F, BMI160_EN_PULL_UP_REG_4); + imu.setRegister(BMI160_7F, BMI160_EN_PULL_UP_REG_5); + + /* Enable MAG interface */ + imu.setRegister(BMI160_RA_IF_CONF, BMI160_IF_CONF_MODE_PRI_AUTO_SEC_MAG); + delay(1); + + imu.setMagDeviceAddress(QMC_DEVADDR); + delay(3); + imu.setRegister(BMI160_RA_MAG_IF_1_MODE, BMI160_MAG_SETUP_MODE); + delay(3); + + /* Configure QMC5883L Sensor */ + imu.setMagRegister(QMC_RA_RESET, 1); + delay(3); + imu.setMagRegister(QMC_RA_CONTROL, QMC_CFG_MODE_CONTINUOUS | QMC_CFG_ODR_200HZ | QMC_CFG_RNG_8G | QMC_CFG_OSR_512); + + imu.setRegister(BMI160_RA_MAG_IF_2_READ_RA, QMC_RA_DATA); + imu.setRegister(BMI160_RA_MAG_CONF, magRate); + delay(3); + imu.setRegister(BMI160_RA_MAG_IF_1_MODE, BMI160_MAG_DATA_MODE_6); +} void BMI160Sensor::motionSetup() { // initialize device - imu.initialize(addr); - if(!imu.testConnection()) { + imu.initialize( + addr, + BMI160_GYRO_RATE, + BMI160_GYRO_RANGE, + BMI160_GYRO_FILTER_MODE, + BMI160_ACCEL_RATE, + BMI160_ACCEL_RANGE, + BMI160_ACCEL_FILTER_MODE + ); + #if !USE_6_AXIS + #if BMI160_MAG_TYPE == BMI160_MAG_TYPE_HMC + initHMC(BMI160_MAG_RATE); + #elif BMI160_MAG_TYPE == BMI160_MAG_TYPE_QMC + initQMC(BMI160_MAG_RATE); + #else + static_assert(false, "Mag is enabled but BMI160_MAG_TYPE not set in defines"); + #endif + #endif + + if (!imu.testConnection()) { m_Logger.fatal("Can't connect to BMI160 (reported device ID 0x%02x) at address 0x%02x", imu.getDeviceID(), addr); ledManager.pattern(50, 50, 200); return; @@ -71,25 +121,6 @@ void BMI160Sensor::motionSetup() { m_Logger.info("Connected to BMI160 (reported device ID 0x%02x) at address 0x%02x", imu.getDeviceID(), addr); - int16_t ax, ay, az; - imu.getAcceleration(&ax, &ay, &az); - float g_az = (float)az / 8192; // For 4G sensitivity - if(g_az < -0.75f) { - ledManager.on(); - - m_Logger.info("Flip front to confirm start calibration"); - delay(5000); - imu.getAcceleration(&ax, &ay, &az); - g_az = (float)az / 8192; - if(g_az > 0.75f) - { - m_Logger.debug("Starting calibration..."); - startCalibration(0); - } - - ledManager.off(); - } - // Initialize the configuration { SlimeVR::Configuration::CalibrationConfig sensorCalibration = configuration.getCalibration(sensorId); @@ -110,203 +141,659 @@ void BMI160Sensor::motionSetup() { } } + int16_t ax, ay, az; + getRemappedAcceleration(&ax, &ay, &az); + float g_az = (float)az / BMI160_ACCEL_TYPICAL_SENSITIVITY_LSB; + if (g_az < -0.75f) { + ledManager.on(); + + m_Logger.info("Flip front to confirm start calibration"); + delay(5000); + getRemappedAcceleration(&ax, &ay, &az); + g_az = (float)az / BMI160_ACCEL_TYPICAL_SENSITIVITY_LSB; + if (g_az > 0.75f) { + m_Logger.debug("Starting calibration..."); + startCalibration(0); + } + + ledManager.off(); + } + + { + #define IS_INT16_CLIPPED(value) (value == INT16_MIN || value == INT16_MAX) + const bool anyClipped = IS_INT16_CLIPPED(ax) || IS_INT16_CLIPPED(ay) || IS_INT16_CLIPPED(az); + const bool anyZero = ax == 0 || ay == 0 || az == 0; + if (anyClipped || anyZero) { + m_Logger.warn("---------------- WARNING -----------------"); + m_Logger.warn("One or more accelerometer axes may be dead"); + m_Logger.warn("Acceleration: %i %i %i (Z = %f G)", + ax, ay, az, (float)az / BMI160_ACCEL_TYPICAL_SENSITIVITY_LSB); + m_Logger.warn("---------------- WARNING -----------------"); + } + } + + // allocate temperature memory after calibration because OOM + gyroTempCalibrator = new GyroTemperatureCalibrator( + SlimeVR::Configuration::CalibrationConfigType::BMI160, + sensorId, + BMI160_GYRO_TYPICAL_SENSITIVITY_LSB, + BMI160_TEMP_CALIBRATION_REQUIRED_SAMPLES_PER_STEP + ); + + #if BMI160_USE_TEMPCAL + gyroTempCalibrator->loadConfig(BMI160_GYRO_TYPICAL_SENSITIVITY_LSB); + if (gyroTempCalibrator->config.hasCoeffs) { + float GOxyzAtTemp[3]; + gyroTempCalibrator->approximateOffset(m_Calibration.temperature, GOxyzAtTemp); + for (uint32_t i = 0; i < 3; i++) { + GOxyzStaticTempCompensated[i] = m_Calibration.G_off[i] - GOxyzAtTemp[i]; + } + } + #endif + + #if BMI160_USE_SENSCAL + { + String localDevice = WiFi.macAddress(); + for (auto const& offsets : sensitivityOffsets) { + if (!localDevice.equals(offsets.mac)) continue; + if (offsets.sensorId != sensorId) continue; + + #define BMI160_CALCULATE_SENSITIVTY_MUL(degrees) (1.0 / (1.0 - ((degrees)/(360.0 * offsets.spins)))) + + gscaleX = BMI160_GSCALE * BMI160_CALCULATE_SENSITIVTY_MUL(offsets.x); + gscaleY = BMI160_GSCALE * BMI160_CALCULATE_SENSITIVTY_MUL(offsets.y); + gscaleZ = BMI160_GSCALE * BMI160_CALCULATE_SENSITIVTY_MUL(offsets.z); + m_Logger.debug("Custom sensitivity offset enabled: %s %s", + offsets.mac, + offsets.sensorId == SENSORID_PRIMARY ? "primary" : "aux" + ); + } + } + #endif + + isGyroCalibrated = hasGyroCalibration(); + isAccelCalibrated = hasAccelCalibration(); + #if !USE_6_AXIS + isMagCalibrated = hasMagCalibration(); + #endif + m_Logger.info("Calibration data for gyro: %s", isGyroCalibrated ? "found" : "not found"); + m_Logger.info("Calibration data for accel: %s", isAccelCalibrated ? "found" : "not found"); + #if !USE_6_AXIS + m_Logger.info("Calibration data for mag: %s", isMagCalibrated ? "found" : "not found"); + #endif + + imu.setFIFOHeaderModeEnabled(true); + imu.setGyroFIFOEnabled(true); + imu.setAccelFIFOEnabled(true); + #if !USE_6_AXIS + imu.setMagFIFOEnabled(true); + #endif + delay(4); + imu.resetFIFO(); + delay(2); + + uint8_t err; + if (imu.getErrReg(&err)) { + if (err & BMI160_ERR_MASK_CHIP_NOT_OPERABLE) { + m_Logger.fatal("Fatal error: chip not operable"); + return; + } else if (err & BMI160_ERR_MASK_ERROR_CODE) { + m_Logger.error("Error code 0x%02x", err); + } else { + m_Logger.info("Initialized"); + } + } else { + m_Logger.error("Failed to get error register value"); + } + working = true; } void BMI160Sensor::motionLoop() { -#if ENABLE_INSPECTION + #if ENABLE_INSPECTION { int16_t rX, rY, rZ, aX, aY, aZ; - imu.getRotation(&rX, &rY, &rZ); - imu.getAcceleration(&aX, &aY, &aZ); + getRemappedRotation(&rX, &rY, &rZ); + getRemappedAcceleration(&aX, &aY, &aZ); Network::sendInspectionRawIMUData(sensorId, rX, rY, rZ, 255, aX, aY, aZ, 255, 0, 0, 0, 255); } -#endif + #endif - now = micros(); - deltat = now - last; //seconds since last update - last = now; - - float Gxyz[3] = {0}; - float Axyz[3] = {0}; - getScaledValues(Gxyz, Axyz); - - mahonyQuaternionUpdate(q, Axyz[0], Axyz[1], Axyz[2], Gxyz[0], Gxyz[1], Gxyz[2], deltat * 1.0e-6f); - quaternion.set(-q[2], q[1], q[3], q[0]); - -#if SEND_ACCELERATION { - // Use the same mapping as in quaternion.set(-q[2], q[1], q[3], q[0]); - this->acceleration[0] = -Axyz[1]; - this->acceleration[1] = Axyz[0]; - this->acceleration[2] = Axyz[2]; + uint32_t now = micros(); + constexpr uint32_t BMI160_TARGET_SYNC_INTERVAL_MICROS = 25000; + uint32_t elapsed = now - lastClockPollTime; + if (elapsed >= BMI160_TARGET_SYNC_INTERVAL_MICROS) { + lastClockPollTime = now - (elapsed - BMI160_TARGET_SYNC_INTERVAL_MICROS); - // get the component of the acceleration that is gravity - VectorFloat gravity; - gravity.x = 2 * (this->quaternion.x * this->quaternion.z - this->quaternion.w * this->quaternion.y); - gravity.y = 2 * (this->quaternion.w * this->quaternion.x + this->quaternion.y * this->quaternion.z); - gravity.z = this->quaternion.w * this->quaternion.w - this->quaternion.x * this->quaternion.x - this->quaternion.y * this->quaternion.y + this->quaternion.z * this->quaternion.z; - - // subtract gravity from the acceleration vector - this->acceleration[0] -= gravity.x * ACCEL_SENSITIVITY_4G; - this->acceleration[1] -= gravity.y * ACCEL_SENSITIVITY_4G; - this->acceleration[2] -= gravity.z * ACCEL_SENSITIVITY_4G; + const uint32_t nextLocalTime1 = micros(); + uint32_t rawSensorTime; + if (imu.getSensorTime(&rawSensorTime)) { + localTime0 = localTime1; + localTime1 = nextLocalTime1; + syncLatencyMicros = (micros() - localTime1) * 0.3; + sensorTime0 = sensorTime1; + sensorTime1 = rawSensorTime; + if ((sensorTime0 > 0 || localTime0 > 0) && (sensorTime1 > 0 || sensorTime1 > 0)) { + // handle 24 bit overflow + double remoteDt = + sensorTime1 >= sensorTime0 ? + sensorTime1 - sensorTime0 : + (sensorTime1 + 0xFFFFFF) - sensorTime0; + double localDt = localTime1 - localTime0; + const double nextSensorTimeRatio = localDt / (remoteDt * BMI160_TIMESTAMP_RESOLUTION_MICROS); + + // handle sdk lags and time travel + if (round(nextSensorTimeRatio) == 1.0) { + sensorTimeRatio = nextSensorTimeRatio; - // finally scale the acceleration values to mps2 - this->acceleration[0] *= ASCALE_4G; - this->acceleration[1] *= ASCALE_4G; - this->acceleration[2] *= ASCALE_4G; + if (round(sensorTimeRatioEma) != 1.0) { + sensorTimeRatioEma = sensorTimeRatio; + } + + constexpr double EMA_APPROX_SECONDS = 1.0; + constexpr uint32_t EMA_SAMPLES = (EMA_APPROX_SECONDS / 3 * 1e6) / BMI160_TARGET_SYNC_INTERVAL_MICROS; + sensorTimeRatioEma -= sensorTimeRatioEma / EMA_SAMPLES; + sensorTimeRatioEma += sensorTimeRatio / EMA_SAMPLES; + + sampleDtMicros = BMI160_ODR_GYR_MICROS * sensorTimeRatioEma; + samplesSinceClockSync = 0; + } + } + } + + getTemperature(&temperature); + optimistic_yield(100); + } } -#endif - - quaternion *= sensorOffset; - -#if ENABLE_INSPECTION + { - Network::sendInspectionFusedIMUData(sensorId, quaternion); + uint32_t now = micros(); + constexpr uint32_t BMI160_TARGET_POLL_INTERVAL_MICROS = 6000; + uint32_t elapsed = now - lastPollTime; + if (elapsed >= BMI160_TARGET_POLL_INTERVAL_MICROS) { + lastPollTime = now - (elapsed - BMI160_TARGET_POLL_INTERVAL_MICROS); + + #if BMI160_DEBUG + uint32_t start = micros(); + readFIFO(); + uint32_t end = micros(); + cpuUsageMicros += end - start; + if (!lastCpuUsagePrinted) lastCpuUsagePrinted = end; + if (end - lastCpuUsagePrinted > 1e6) { + bool restDetected = false; + #if BMI160_VQF_REST_DETECTION_AVAILABLE + restDetected = vqf.getRestDetected(); + #else + restDetected = restDetection.getRestDetected(); + #endif + + #if BMI160_USE_VQF + #define BMI160_FUSION_TYPE "vqf" + #else + #define BMI160_FUSION_TYPE "mahony" + #endif + m_Logger.debug("readFIFO took %0.4f ms, read gyr %i acc %i mag %i rest %i resets %i readerrs %i type " BMI160_FUSION_TYPE, + ((float)cpuUsageMicros / 1e3f), + gyrReads, + accReads, + magReads, + restDetected, + numFIFODropped, + numFIFOFailedReads + ); + + cpuUsageMicros = 0; + lastCpuUsagePrinted = end; + gyrReads = 0; + accReads = 0; + magReads = 0; + } + #else + readFIFO(); + #endif + optimistic_yield(100); + if (!fusionUpdated) return; + fusionUpdated = false; + } } -#endif - if (!OPTIMIZE_UPDATES || !lastQuatSent.equalsWithEpsilon(quaternion)) { - newData = true; - lastQuatSent = quaternion; + uint32_t now = micros(); + constexpr float maxSendRateHz = 2.0f; + constexpr uint32_t sendInterval = 1.0f/maxSendRateHz * 1e6; + uint32_t elapsed = now - lastTemperaturePacketSent; + if (elapsed >= sendInterval) { + lastTemperaturePacketSent = now - (elapsed - sendInterval); + #if BMI160_TEMPCAL_DEBUG + uint32_t isCalibrating = gyroTempCalibrator->isCalibrating() ? 10000 : 0; + Network::sendTemperature(isCalibrating + 10000 + (gyroTempCalibrator->config.samplesTotal * 100) + temperature, sensorId); + #else + Network::sendTemperature(temperature, sensorId); + #endif + optimistic_yield(100); + } + } + + { + uint32_t now = micros(); + constexpr float maxSendRateHz = 120.0f; + constexpr uint32_t sendInterval = 1.0f/maxSendRateHz * 1e6; + uint32_t elapsed = now - lastRotationPacketSent; + if (elapsed >= sendInterval) { + lastRotationPacketSent = now - (elapsed - sendInterval); + + #if BMI160_USE_VQF + #if USE_6_AXIS + vqf.getQuat6D(qwxyz); + #else + vqf.getQuat9D(qwxyz); + #endif + #endif + + if (isnan(qwxyz[0]) || isnan(qwxyz[1]) || isnan(qwxyz[2]) || isnan(qwxyz[3])) { + qwxyz[0] = 1; + qwxyz[1] = 0; + qwxyz[2] = 0; + qwxyz[3] = 0; + return; + } + + quaternion.set(qwxyz[1], qwxyz[2], qwxyz[3], qwxyz[0]); + + const Quat q = quaternion; + sensor_real_t vecGravity[3]; + vecGravity[0] = 2 * (q.x * q.z - q.w * q.y); + vecGravity[1] = 2 * (q.w * q.x + q.y * q.z); + vecGravity[2] = q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z; + + VectorFloat linAccel; + linAccel.x = lastAxyz[0] - vecGravity[0] * CONST_EARTH_GRAVITY; + linAccel.y = lastAxyz[1] - vecGravity[1] * CONST_EARTH_GRAVITY; + linAccel.z = lastAxyz[2] - vecGravity[2] * CONST_EARTH_GRAVITY; + + linearAcceleration[0] = linAccel.x; + linearAcceleration[1] = linAccel.y; + linearAcceleration[2] = linAccel.z; + + quaternion *= sensorOffset; + + #if ENABLE_INSPECTION + { + Network::sendInspectionFusedIMUData(sensorId, quaternion); + } + #endif + + if (!OPTIMIZE_UPDATES || !lastQuatSent.equalsWithEpsilon(quaternion)) + { + newData = true; + lastQuatSent = quaternion; + } + + optimistic_yield(100); + } } } -float BMI160Sensor::getTemperature() -{ +void BMI160Sensor::readFIFO() { + if (!imu.getFIFOCount(&fifo.length)) { + #if BMI160_DEBUG + numFIFOFailedReads++; + #endif + return; + } + + if (fifo.length <= 1) return; + if (fifo.length > sizeof(fifo.data)) { + #if BMI160_DEBUG + numFIFODropped++; + #endif + imu.resetFIFO(); + return; + } + std::fill(fifo.data, fifo.data + fifo.length, 0); + if (!imu.getFIFOBytes(fifo.data, fifo.length)) { + #if BMI160_DEBUG + numFIFOFailedReads++; + #endif + return; + } + + optimistic_yield(100); + + int16_t gx, gy, gz; + int16_t ax, ay, az; + bool gnew, anew; + #if !USE_6_AXIS + int16_t mx, my, mz; + bool mnew; + #endif + + uint8_t header; + for (uint32_t i = 0; i < fifo.length;) { + #define BMI160_FIFO_FRAME_ENSURE_BYTES_AVAILABLE(len) { if (i + len > fifo.length) break; } + BMI160_FIFO_FRAME_ENSURE_BYTES_AVAILABLE(1); + + // ignore interrupt tags in header + header = fifo.data[i] & 0b11111100; + i++; + + if (header == BMI160_FIFO_HEADER_CTL_SKIP_FRAME) { + BMI160_FIFO_FRAME_ENSURE_BYTES_AVAILABLE(BMI160_FIFO_SKIP_FRAME_LEN); + break; + } else if (header == BMI160_FIFO_HEADER_CTL_SENSOR_TIME) { + BMI160_FIFO_FRAME_ENSURE_BYTES_AVAILABLE(BMI160_FIFO_SENSOR_TIME_LEN); + i += BMI160_FIFO_SENSOR_TIME_LEN; + } else if (header == BMI160_FIFO_HEADER_CTL_INPUT_CONFIG) { + BMI160_FIFO_FRAME_ENSURE_BYTES_AVAILABLE(BMI160_FIFO_INPUT_CONFIG_LEN); + i += BMI160_FIFO_INPUT_CONFIG_LEN; + } else if (header & BMI160_FIFO_HEADER_DATA_FRAME_BASE) { + if (!(header & BMI160_FIFO_HEADER_DATA_FRAME_MASK_HAS_DATA)) { + break; + } + gnew = false; + anew = false; + #if !USE_6_AXIS + mnew = false; + #endif + + // mag + if (header & BMI160_FIFO_HEADER_DATA_FRAME_FLAG_M) { + BMI160_FIFO_FRAME_ENSURE_BYTES_AVAILABLE(BMI160_FIFO_M_LEN); + #if !USE_6_AXIS + getMagnetometerXYZFromBuffer(&fifo.data[i], &mx, &my, &mz); + mnew = true; + #endif + i += BMI160_FIFO_M_LEN; + } + + // bmi160 -> 0 lsb 1 msb + // gyro + if (header & BMI160_FIFO_HEADER_DATA_FRAME_FLAG_G) { + BMI160_FIFO_FRAME_ENSURE_BYTES_AVAILABLE(BMI160_FIFO_G_LEN); + gx = ((int16_t)fifo.data[i + 1] << 8) | fifo.data[i + 0]; + gy = ((int16_t)fifo.data[i + 3] << 8) | fifo.data[i + 2]; + gz = ((int16_t)fifo.data[i + 5] << 8) | fifo.data[i + 4]; + gnew = true; + i += BMI160_FIFO_G_LEN; + } + + // bmi160 -> 0 lsb 1 msb + // accel + if (header & BMI160_FIFO_HEADER_DATA_FRAME_FLAG_A) { + BMI160_FIFO_FRAME_ENSURE_BYTES_AVAILABLE(BMI160_FIFO_A_LEN); + ax = ((int16_t)fifo.data[i + 1] << 8) | fifo.data[i + 0]; + ay = ((int16_t)fifo.data[i + 3] << 8) | fifo.data[i + 2]; + az = ((int16_t)fifo.data[i + 5] << 8) | fifo.data[i + 4]; + anew = true; + i += BMI160_FIFO_A_LEN; + } + + // gyro callback updates fusion and must be last + #if !USE_6_AXIS + if (mnew) onMagRawSample(BMI160_ODR_MAG_MICROS, mx, my, mz); + #endif + if (anew) onAccelRawSample(BMI160_ODR_ACC_MICROS, ax, ay, az); + if (gnew) { + constexpr uint32_t alignmentBitmask = ~(0xFFFFFFFF << (16 - BMI160_GYRO_RATE)); + uint32_t alignmentOffset = + (sensorTime1 & alignmentBitmask) * BMI160_TIMESTAMP_RESOLUTION_MICROS * sensorTimeRatioEma; + + timestamp0 = timestamp1; + timestamp1 = (localTime1 - alignmentOffset - syncLatencyMicros) + + (++samplesSinceClockSync) * sampleDtMicros; + int32_t dtMicros = timestamp1 - timestamp0; + + constexpr float invPeriod = 1.0f / BMI160_ODR_GYR_MICROS; + int32_t sampleOffset = round((float)dtMicros * invPeriod) - 1; + if (abs(sampleOffset) > 3) { + dtMicros = sampleDtMicros; + } else if (sampleOffset != 0) { + dtMicros -= sampleOffset * sampleDtMicros; + } + + onGyroRawSample(dtMicros, gx, gy, gz); + } + } else { + break; + } + } +} + +void BMI160Sensor::onGyroRawSample(uint32_t dtMicros, int16_t x, int16_t y, int16_t z) { + #if BMI160_DEBUG + gyrReads++; + #endif + + #if BMI160_USE_TEMPCAL + bool restDetected = false; + #if BMI160_VQF_REST_DETECTION_AVAILABLE + restDetected = vqf.getRestDetected(); + #else + restDetected = restDetection.getRestDetected(); + #endif + gyroTempCalibrator->updateGyroTemperatureCalibration(temperature, restDetected, x, y, z); + #endif + + sensor_real_t gyroCalibratedStatic[3]; + gyroCalibratedStatic[0] = (sensor_real_t)((((double)x - m_Calibration.G_off[0]) * gscaleX)); + gyroCalibratedStatic[1] = (sensor_real_t)((((double)y - m_Calibration.G_off[1]) * gscaleY)); + gyroCalibratedStatic[2] = (sensor_real_t)((((double)z - m_Calibration.G_off[2]) * gscaleZ)); + + #if BMI160_USE_TEMPCAL + float GOxyz[3]; + if (gyroTempCalibrator->approximateOffset(temperature, GOxyz)) { + Gxyz[0] = (sensor_real_t)((((double)x - GOxyz[0] - GOxyzStaticTempCompensated[0]) * gscaleX)); + Gxyz[1] = (sensor_real_t)((((double)y - GOxyz[1] - GOxyzStaticTempCompensated[1]) * gscaleY)); + Gxyz[2] = (sensor_real_t)((((double)z - GOxyz[2] - GOxyzStaticTempCompensated[2]) * gscaleZ)); + } + else + #endif + { + Gxyz[0] = gyroCalibratedStatic[0]; + Gxyz[1] = gyroCalibratedStatic[1]; + Gxyz[2] = gyroCalibratedStatic[2]; + } + remapGyroAccel(&Gxyz[0], &Gxyz[1], &Gxyz[2]); + + #if !BMI160_VQF_REST_DETECTION_AVAILABLE + restDetection.updateGyr(dtMicros, Gxyz); + #endif + #if USE_6_AXIS + #if BMI160_USE_VQF + vqf.updateGyr(Gxyz, (double)dtMicros * 1.0e-6); + #else + mahony.updateInto(qwxyz, Axyz[0], Axyz[1], Axyz[2], Gxyz[0], Gxyz[1], Gxyz[2], ((float)dtMicros) * 1.0e-6); + #endif + #else + #if BMI160_USE_VQF + vqf.updateGyr(Gxyz, (double)dtMicros * 1.0e-6); + #else + mahonyQuaternionUpdate(qwxyz, Axyz[0], Axyz[1], Axyz[2], Gxyz[0], Gxyz[1], Gxyz[2], Mxyz[0], Mxyz[1], Mxyz[2], ((float)dtMicros) * 1.0e-6f); + #endif + #endif + Axyz[0] = 0; + Axyz[1] = 0; + Axyz[2] = 0; + + fusionUpdated = true; + optimistic_yield(100); +} +void BMI160Sensor::onAccelRawSample(uint32_t dtMicros, int16_t x, int16_t y, int16_t z) { + #if BMI160_DEBUG + accReads++; + #endif + + Axyz[0] = (sensor_real_t)x; + Axyz[1] = (sensor_real_t)y; + Axyz[2] = (sensor_real_t)z; + applyAccelCalibrationAndScale(Axyz); + remapGyroAccel(&Axyz[0], &Axyz[1], &Axyz[2]); + lastAxyz[0] = Axyz[0]; + lastAxyz[1] = Axyz[1]; + lastAxyz[2] = Axyz[2]; + #if !BMI160_VQF_REST_DETECTION_AVAILABLE + restDetection.updateAcc(dtMicros, Axyz); + #endif + #if BMI160_USE_VQF + vqf.updateAcc(Axyz); + #endif + optimistic_yield(100); +} +void BMI160Sensor::onMagRawSample(uint32_t dtMicros, int16_t x, int16_t y, int16_t z) { + #if BMI160_DEBUG + magReads++; + #endif + + Mxyz[0] = (sensor_real_t)x; + Mxyz[1] = (sensor_real_t)y; + Mxyz[2] = (sensor_real_t)z; + #if !USE_6_AXIS + applyMagCalibrationAndScale(Mxyz); + #endif + remapMagnetometer(&Mxyz[0], &Mxyz[1], &Mxyz[2]); + #if !USE_6_AXIS + #if BMI160_USE_VQF + vqf.updateMag(Mxyz); + #endif + #endif +} + +void BMI160Sensor::printTemperatureCalibrationState() { + const auto degCtoF = [](float degC) { return (degC * 9.0f/5.0f) + 32.0f; }; + + m_Logger.info("Sensor %i temperature calibration state:", sensorId); + m_Logger.info(" current temp: %0.4f C (%0.4f F)", temperature, degCtoF(temperature)); + auto printTemperatureRange = [&](const char* label, float min, float max) { + m_Logger.info(" %s: min %0.4f C max %0.4f C (min %0.4f F max %0.4f F)", + label, min, max, degCtoF(min), degCtoF(max) + ); + }; + printTemperatureRange("total range", + TEMP_CALIBRATION_MIN, + TEMP_CALIBRATION_MAX + ); + printTemperatureRange("calibrated range", + gyroTempCalibrator->config.minTemperatureRange, + gyroTempCalibrator->config.maxTemperatureRange + ); + m_Logger.info(" done: %0.1f%", gyroTempCalibrator->config.getCalibrationDonePercent()); +} +void BMI160Sensor::printDebugTemperatureCalibrationState() { + m_Logger.info("Sensor %i gyro odr %f hz, sensitivity %f lsb", + sensorId, + BMI160_ODR_GYR_HZ, + BMI160_GYRO_TYPICAL_SENSITIVITY_LSB + ); + m_Logger.info("Sensor %i temperature calibration matrix (tempC x y z):", sensorId); + m_Logger.info("BUF %i %i", sensorId, TEMP_CALIBRATION_BUFFER_SIZE); + m_Logger.info("SENS %i %f", sensorId, BMI160_GYRO_TYPICAL_SENSITIVITY_LSB); + m_Logger.info("DATA %i", sensorId); + for (int i = 0; i < TEMP_CALIBRATION_BUFFER_SIZE; i++) { + m_Logger.info("%f %f %f %f", + gyroTempCalibrator->config.samples[i].t, + gyroTempCalibrator->config.samples[i].x, + gyroTempCalibrator->config.samples[i].y, + gyroTempCalibrator->config.samples[i].z + ); + } + m_Logger.info("END %i", sensorId); + m_Logger.info("y = %f + (%fx) + (%fxx) + (%fxxx)", UNPACK_VECTOR_ARRAY(gyroTempCalibrator->config.cx), gyroTempCalibrator->config.cx[3]); + m_Logger.info("y = %f + (%fx) + (%fxx) + (%fxxx)", UNPACK_VECTOR_ARRAY(gyroTempCalibrator->config.cy), gyroTempCalibrator->config.cy[3]); + m_Logger.info("y = %f + (%fx) + (%fxx) + (%fxxx)", UNPACK_VECTOR_ARRAY(gyroTempCalibrator->config.cz), gyroTempCalibrator->config.cz[3]); +} +void BMI160Sensor::saveTemperatureCalibration() { + gyroTempCalibrator->saveConfig(); +} + +bool BMI160Sensor::getTemperature(float* out) { // Middle value is 23 degrees C (0x0000) - #define TEMP_ZERO 23 + #define BMI160_ZERO_TEMP_OFFSET 23 // Temperature per step from -41 + 1/2^9 degrees C (0x8001) to 87 - 1/2^9 degrees C (0x7FFF) constexpr float TEMP_STEP = 128. / 65535; - return (imu.getTemperature() * TEMP_STEP) + TEMP_ZERO; + int16_t temp; + if (imu.getTemperature(&temp)) { + *out = (temp * TEMP_STEP) + BMI160_ZERO_TEMP_OFFSET; + return true; + } + return false; } -void BMI160Sensor::getScaledValues(float Gxyz[3], float Axyz[3]) -{ -#if ENABLE_INSPECTION - { - int16_t rX, rY, rZ, aX, aY, aZ, mX, mY, mZ; - imu.getRotation(&rX, &rY, &rZ); - imu.getAcceleration(&aX, &aY, &aZ); - - Network::sendInspectionRawIMUData(sensorId, rX, rY, rZ, 255, aX, aY, aZ, 255, 0, 0, 0, 255); - } -#endif - - float temperature = getTemperature(); - float tempDiff = temperature - m_Calibration.temperature; - uint8_t quant = map(temperature, 15, 75, 0, 12); - - int16_t ax, ay, az; - int16_t gx, gy, gz; - // TODO: Read from FIFO? - imu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); - - // TODO: Sensitivity over temp compensation? - // TODO: Cross-axis sensitivity compensation? - Gxyz[0] = ((float)gx - (m_Calibration.G_off[0] + (tempDiff * LSB_COMP_PER_TEMP_X_MAP[quant]))) * GSCALE; - Gxyz[1] = ((float)gy - (m_Calibration.G_off[1] + (tempDiff * LSB_COMP_PER_TEMP_Y_MAP[quant]))) * GSCALE; - Gxyz[2] = ((float)gz - (m_Calibration.G_off[2] + (tempDiff * LSB_COMP_PER_TEMP_Z_MAP[quant]))) * GSCALE; - - Axyz[0] = (float)ax; - Axyz[1] = (float)ay; - Axyz[2] = (float)az; +void BMI160Sensor::applyAccelCalibrationAndScale(sensor_real_t Axyz[3]) { //apply offsets (bias) and scale factors from Magneto - #if useFullCalibrationMatrix == true - float temp[3]; - for (uint8_t i = 0; i < 3; i++) - temp[i] = (Axyz[i] - m_Calibration.A_B[i]); - Axyz[0] = m_Calibration.A_Ainv[0][0] * temp[0] + m_Calibration.A_Ainv[0][1] * temp[1] + m_Calibration.A_Ainv[0][2] * temp[2]; - Axyz[1] = m_Calibration.A_Ainv[1][0] * temp[0] + m_Calibration.A_Ainv[1][1] * temp[1] + m_Calibration.A_Ainv[1][2] * temp[2]; - Axyz[2] = m_Calibration.A_Ainv[2][0] * temp[0] + m_Calibration.A_Ainv[2][1] * temp[1] + m_Calibration.A_Ainv[2][2] * temp[2]; - #else - for (uint8_t i = 0; i < 3; i++) - Axyz[i] = (Axyz[i] - calibration->A_B[i]); + if (isAccelCalibrated) { + #if useFullCalibrationMatrix == true + float tmp[3]; + for (uint8_t i = 0; i < 3; i++) + tmp[i] = (Axyz[i] - m_Calibration.A_B[i]); + Axyz[0] = m_Calibration.A_Ainv[0][0] * tmp[0] + m_Calibration.A_Ainv[0][1] * tmp[1] + m_Calibration.A_Ainv[0][2] * tmp[2]; + Axyz[1] = m_Calibration.A_Ainv[1][0] * tmp[0] + m_Calibration.A_Ainv[1][1] * tmp[1] + m_Calibration.A_Ainv[1][2] * tmp[2]; + Axyz[2] = m_Calibration.A_Ainv[2][0] * tmp[0] + m_Calibration.A_Ainv[2][1] * tmp[1] + m_Calibration.A_Ainv[2][2] * tmp[2]; + #else + for (uint8_t i = 0; i < 3; i++) + Axyz[i] = (Axyz[i] - calibration->A_B[i]); + #endif + } + Axyz[0] *= BMI160_ASCALE; + Axyz[1] *= BMI160_ASCALE; + Axyz[2] *= BMI160_ASCALE; +} + +void BMI160Sensor::applyMagCalibrationAndScale(sensor_real_t Mxyz[3]) { + #if !USE_6_AXIS + //apply offsets and scale factors from Magneto + #if useFullCalibrationMatrix == true + float temp[3]; + for (uint8_t i = 0; i < 3; i++) + temp[i] = (Mxyz[i] - m_Calibration.M_B[i]); + Mxyz[0] = m_Calibration.M_Ainv[0][0] * temp[0] + m_Calibration.M_Ainv[0][1] * temp[1] + m_Calibration.M_Ainv[0][2] * temp[2]; + Mxyz[1] = m_Calibration.M_Ainv[1][0] * temp[0] + m_Calibration.M_Ainv[1][1] * temp[1] + m_Calibration.M_Ainv[1][2] * temp[2]; + Mxyz[2] = m_Calibration.M_Ainv[2][0] * temp[0] + m_Calibration.M_Ainv[2][1] * temp[1] + m_Calibration.M_Ainv[2][2] * temp[2]; + #else + for (i = 0; i < 3; i++) + Mxyz[i] = (Mxyz[i] - m_Calibration.M_B[i]); + #endif #endif } +bool BMI160Sensor::hasGyroCalibration() { + for (int i = 0; i < 3; i++) { + if (m_Calibration.G_off[i] != 0.0) + return true; + } + return false; +} + +bool BMI160Sensor::hasAccelCalibration() { + for (int i = 0; i < 3; i++) { + if (m_Calibration.A_B[i] != 0.0 || + m_Calibration.A_Ainv[0][i] != 0.0 || + m_Calibration.A_Ainv[1][i] != 0.0 || + m_Calibration.A_Ainv[2][i] != 0.0) + return true; + } + return false; +} + +bool BMI160Sensor::hasMagCalibration() { + for (int i = 0; i < 3; i++) { + if (m_Calibration.M_B[i] != 0.0 || + m_Calibration.M_Ainv[0][i] != 0.0 || + m_Calibration.M_Ainv[1][i] != 0.0 || + m_Calibration.M_Ainv[2][i] != 0.0) + return true; + } + return false; +} + void BMI160Sensor::startCalibration(int calibrationType) { ledManager.on(); - m_Logger.debug("Gathering raw data for device calibration..."); - - // Wait for sensor to calm down before calibration - m_Logger.info("Put down the device and wait for baseline gyro reading calibration"); - delay(2000); - float temperature = getTemperature(); - m_Calibration.temperature = temperature; - uint16_t gyroCalibrationSamples = 2500; - float rawGxyz[3] = {0}; - -#ifdef DEBUG_SENSOR - m_Logger.trace("Calibration temperature: %f", temperature); -#endif - - for (int i = 0; i < gyroCalibrationSamples; i++) - { - ledManager.on(); - - int16_t gx, gy, gz; - imu.getRotation(&gx, &gy, &gz); - rawGxyz[0] += float(gx); - rawGxyz[1] += float(gy); - rawGxyz[2] += float(gz); - - ledManager.off(); - } - m_Calibration.G_off[0] = rawGxyz[0] / gyroCalibrationSamples; - m_Calibration.G_off[1] = rawGxyz[1] / gyroCalibrationSamples; - m_Calibration.G_off[2] = rawGxyz[2] / gyroCalibrationSamples; - -#ifdef DEBUG_SENSOR - m_Logger.trace("Gyro calibration results: %f %f %f", UNPACK_VECTOR_ARRAY(m_Calibration.G_off)); -#endif - - // Blink calibrating led before user should rotate the sensor - m_Logger.info("After 3 seconds, Gently rotate the device while it's gathering accelerometer data"); - ledManager.on(); - delay(1500); - ledManager.off(); - delay(1500); - m_Logger.debug("Gathering accelerometer data..."); - - MagnetoCalibration *magneto = new MagnetoCalibration(); - - uint16_t accelCalibrationSamples = 300; - for (int i = 0; i < accelCalibrationSamples; i++) - { - ledManager.on(); - - int16_t ax, ay, az; - imu.getAcceleration(&ax, &ay, &az); - magneto->sample(ax, ay, az); - - ledManager.off(); - delay(100); - } - ledManager.off(); - m_Logger.debug("Calculating calibration data..."); - - float A_BAinv[4][3]; - magneto->current_calibration(A_BAinv); - delete magneto; - - m_Logger.debug("Finished Calculate Calibration data"); - m_Logger.debug("Accelerometer calibration matrix:"); - m_Logger.debug("{"); - for (int i = 0; i < 3; i++) - { - m_Calibration.A_B[i] = A_BAinv[0][i]; - m_Calibration.A_Ainv[0][i] = A_BAinv[1][i]; - m_Calibration.A_Ainv[1][i] = A_BAinv[2][i]; - m_Calibration.A_Ainv[2][i] = A_BAinv[3][i]; - m_Logger.debug(" %f, %f, %f, %f", A_BAinv[0][i], A_BAinv[1][i], A_BAinv[2][i], A_BAinv[3][i]); - } - m_Logger.debug("}"); - + maybeCalibrateGyro(); + maybeCalibrateAccel(); + maybeCalibrateMag(); + m_Logger.debug("Saving the calibration data"); SlimeVR::Configuration::CalibrationConfig calibration; @@ -317,6 +804,313 @@ void BMI160Sensor::startCalibration(int calibrationType) { m_Logger.debug("Saved the calibration data"); - m_Logger.info("Calibration data gathered"); - delay(5000); + m_Logger.info("Calibration data gathered, exiting calibration mode in..."); + constexpr uint8_t POST_CALIBRATION_DELAY_SEC = 3; + ledManager.on(); + for (uint8_t i = POST_CALIBRATION_DELAY_SEC; i > 0; i--) { + m_Logger.info("%i...", i); + delay(1000); + } } + +void BMI160Sensor::maybeCalibrateGyro() { + #ifndef BMI160_CALIBRATION_GYRO_SECONDS + static_assert(false, "BMI160_CALIBRATION_GYRO_SECONDS not set in defines"); + #endif + + #if BMI160_CALIBRATION_GYRO_SECONDS == 0 + m_Logger.debug("Skipping gyro calibration"); + return; + #endif + + // Wait for sensor to calm down before calibration + constexpr uint8_t GYRO_CALIBRATION_DELAY_SEC = 3; + constexpr float GYRO_CALIBRATION_DURATION_SEC = BMI160_CALIBRATION_GYRO_SECONDS; + m_Logger.info("Put down the device and wait for baseline gyro reading calibration (%.1f seconds)", GYRO_CALIBRATION_DURATION_SEC); + ledManager.on(); + for (uint8_t i = GYRO_CALIBRATION_DELAY_SEC; i > 0; i--) { + m_Logger.info("%i...", i); + delay(1000); + } + ledManager.off(); + + if (!getTemperature(&temperature)) { + m_Logger.error("Error: can't read temperature"); + } + m_Calibration.temperature = temperature; + + #ifdef DEBUG_SENSOR + m_Logger.trace("Calibration temperature: %f", temperature); + #endif + + if (!imu.getGyroDrdy()) { + m_Logger.error("Fatal error: gyroscope drdy = 0 (dead?)"); + return; + } + + ledManager.pattern(100, 100, 3); + ledManager.on(); + m_Logger.info("Gyro calibration started..."); + + constexpr uint16_t gyroCalibrationSamples = + GYRO_CALIBRATION_DURATION_SEC / (BMI160_ODR_GYR_MICROS / 1e6); + int32_t rawGxyz[3] = {0}; + for (int i = 0; i < gyroCalibrationSamples; i++) { + imu.waitForGyroDrdy(); + + int16_t gx, gy, gz; + imu.getRotation(&gx, &gy, &gz); + rawGxyz[0] += gx; + rawGxyz[1] += gy; + rawGxyz[2] += gz; + } + ledManager.off(); + m_Calibration.G_off[0] = ((double)rawGxyz[0]) / gyroCalibrationSamples; + m_Calibration.G_off[1] = ((double)rawGxyz[1]) / gyroCalibrationSamples; + m_Calibration.G_off[2] = ((double)rawGxyz[2]) / gyroCalibrationSamples; + + #ifdef DEBUG_SENSOR + m_Logger.trace("Gyro calibration results: %f %f %f", UNPACK_VECTOR_ARRAY(m_Calibration.G_off)); + #endif +} + +void BMI160Sensor::maybeCalibrateAccel() { + #ifndef BMI160_ACCEL_CALIBRATION_METHOD + static_assert(false, "BMI160_ACCEL_CALIBRATION_METHOD not set in defines"); + #endif + + #if BMI160_ACCEL_CALIBRATION_METHOD == ACCEL_CALIBRATION_METHOD_SKIP + m_Logger.debug("Skipping accelerometer calibration"); + return; + #endif + + MagnetoCalibration* magneto = new MagnetoCalibration(); + + // Blink calibrating led before user should rotate the sensor + #if BMI160_ACCEL_CALIBRATION_METHOD == ACCEL_CALIBRATION_METHOD_ROTATION + m_Logger.info("After 3 seconds, Gently rotate the device while it's gathering data"); + #elif BMI160_ACCEL_CALIBRATION_METHOD == ACCEL_CALIBRATION_METHOD_6POINT + m_Logger.info("Put the device into 6 unique orientations (all sides), leave it still and do not hold/touch for 3 seconds each"); + #endif + constexpr uint8_t ACCEL_CALIBRATION_DELAY_SEC = 3; + ledManager.on(); + for (uint8_t i = ACCEL_CALIBRATION_DELAY_SEC; i > 0; i--) { + m_Logger.info("%i...", i); + delay(1000); + } + ledManager.off(); + + #if BMI160_ACCEL_CALIBRATION_METHOD == ACCEL_CALIBRATION_METHOD_ROTATION + uint16_t accelCalibrationSamples = 200; + ledManager.pattern(100, 100, 6); + delay(100); + ledManager.on(); + m_Logger.debug("Gathering accelerometer data..."); + for (int i = 0; i < accelCalibrationSamples; i++) + { + int16_t ax, ay, az; + imu.getAcceleration(&ax, &ay, &az); + magneto->sample(ax, ay, az); + + delay(100); + } + ledManager.off(); + m_Logger.debug("Calculating accelerometer calibration data..."); + #elif BMI160_ACCEL_CALIBRATION_METHOD == ACCEL_CALIBRATION_METHOD_6POINT + RestDetectionParams calibrationRestDetectionParams; + calibrationRestDetectionParams.restMinTimeMicros = 3 * 1e6; + calibrationRestDetectionParams.restThAcc = 0.25f; + RestDetection calibrationRestDetection( + calibrationRestDetectionParams, + BMI160_ODR_GYR_MICROS / 1e6f, + BMI160_ODR_ACC_MICROS / 1e6f + ); + + constexpr uint16_t expectedPositions = 6; + constexpr uint16_t numSamplesPerPosition = 96; + + uint16_t numPositionsRecorded = 0; + uint16_t numCurrentPositionSamples = 0; + bool waitForMotion = true; + + float* accelCalibrationChunk = new float[numSamplesPerPosition * 3]; + ledManager.pattern(100, 100, 6); + ledManager.on(); + m_Logger.info("Gathering accelerometer data..."); + m_Logger.info("Waiting for position %i, you can leave the device as is...", numPositionsRecorded + 1); + while (true) { + int16_t ax, ay, az; + imu.getAcceleration(&ax, &ay, &az); + sensor_real_t scaled[3]; + scaled[0] = ax * BMI160_ASCALE; + scaled[1] = ay * BMI160_ASCALE; + scaled[2] = az * BMI160_ASCALE; + + calibrationRestDetection.updateAcc(BMI160_ODR_ACC_MICROS, scaled); + + if (waitForMotion) { + if (!calibrationRestDetection.getRestDetected()) { + waitForMotion = false; + } + delayMicroseconds(BMI160_ODR_ACC_MICROS); + continue; + } + + if (calibrationRestDetection.getRestDetected()) { + const uint16_t i = numCurrentPositionSamples * 3; + accelCalibrationChunk[i + 0] = ax; + accelCalibrationChunk[i + 1] = ay; + accelCalibrationChunk[i + 2] = az; + numCurrentPositionSamples++; + + if (numCurrentPositionSamples >= numSamplesPerPosition) { + for (int i = 0; i < numSamplesPerPosition; i++) { + magneto->sample( + accelCalibrationChunk[i * 3 + 0], + accelCalibrationChunk[i * 3 + 1], + accelCalibrationChunk[i * 3 + 2] + ); + } + numPositionsRecorded++; + numCurrentPositionSamples = 0; + if (numPositionsRecorded < expectedPositions) { + ledManager.pattern(50, 50, 2); + ledManager.on(); + m_Logger.info("Recorded, waiting for position %i...", numPositionsRecorded + 1); + waitForMotion = true; + } + } + } else { + numCurrentPositionSamples = 0; + } + + if (numPositionsRecorded >= expectedPositions) break; + + delayMicroseconds(BMI160_ODR_ACC_MICROS); + } + ledManager.off(); + m_Logger.debug("Calculating accelerometer calibration data..."); + delete[] accelCalibrationChunk; + #endif + + float A_BAinv[4][3]; + magneto->current_calibration(A_BAinv); + delete magneto; + + m_Logger.debug("Finished calculating accelerometer calibration"); + m_Logger.debug("Accelerometer calibration matrix:"); + m_Logger.debug("{"); + for (int i = 0; i < 3; i++) { + m_Calibration.A_B[i] = A_BAinv[0][i]; + m_Calibration.A_Ainv[0][i] = A_BAinv[1][i]; + m_Calibration.A_Ainv[1][i] = A_BAinv[2][i]; + m_Calibration.A_Ainv[2][i] = A_BAinv[3][i]; + m_Logger.debug(" %f, %f, %f, %f", A_BAinv[0][i], A_BAinv[1][i], A_BAinv[2][i], A_BAinv[3][i]); + } + m_Logger.debug("}"); +} + +void BMI160Sensor::maybeCalibrateMag() { +#if !USE_6_AXIS + #ifndef BMI160_CALIBRATION_MAG_SECONDS + static_assert(false, "BMI160_CALIBRATION_MAG_SECONDS not set in defines"); + #endif + + #if BMI160_CALIBRATION_MAG_SECONDS == 0 + m_Logger.debug("Skipping magnetometer calibration"); + return; + #endif + + MagnetoCalibration* magneto = new MagnetoCalibration(); + + constexpr uint8_t MAG_CALIBRATION_DELAY_SEC = 3; + constexpr float MAG_CALIBRATION_DURATION_SEC = BMI160_CALIBRATION_MAG_SECONDS; + m_Logger.info("After 3 seconds, rotate the device in figure 8 pattern while it's gathering data (%.1f seconds)", MAG_CALIBRATION_DURATION_SEC); + for (uint8_t i = MAG_CALIBRATION_DELAY_SEC; i > 0; i--) { + m_Logger.info("%i...", i); + delay(1000); + } + ledManager.pattern(100, 100, 9); + delay(100); + ledManager.on(); + m_Logger.debug("Gathering magnetometer data..."); + + constexpr float SAMPLE_DELAY_MS = 100.0f; + constexpr uint16_t magCalibrationSamples = + MAG_CALIBRATION_DURATION_SEC / (SAMPLE_DELAY_MS / 1e3f); + + uint8_t magdata[6]; + for (int i = 0; i < magCalibrationSamples; i++) { + ledManager.on(); + + int16_t mx, my, mz; + imu.getMagnetometerXYZBuffer(magdata); + getMagnetometerXYZFromBuffer(magdata, &mx, &my, &mz); + magneto->sample(mx, my, mz); + + ledManager.off(); + delay(SAMPLE_DELAY_MS); + } + ledManager.off(); + m_Logger.debug("Calculating magnetometer calibration data..."); + + float M_BAinv[4][3]; + magneto->current_calibration(M_BAinv); + delete magneto; + + m_Logger.debug("[INFO] Magnetometer calibration matrix:"); + m_Logger.debug("{"); + for (int i = 0; i < 3; i++) { + m_Calibration.M_B[i] = M_BAinv[0][i]; + m_Calibration.M_Ainv[0][i] = M_BAinv[1][i]; + m_Calibration.M_Ainv[1][i] = M_BAinv[2][i]; + m_Calibration.M_Ainv[2][i] = M_BAinv[3][i]; + m_Logger.debug(" %f, %f, %f, %f", M_BAinv[0][i], M_BAinv[1][i], M_BAinv[2][i], M_BAinv[3][i]); + } + m_Logger.debug("}"); +#endif +} + +void BMI160Sensor::remapGyroAccel(sensor_real_t* x, sensor_real_t* y, sensor_real_t* z) { + sensor_real_t gx = *x, gy = *y, gz = *z; + *x = BMI160_REMAP_AXIS_X(gx, gy, gz); + *y = BMI160_REMAP_AXIS_Y(gx, gy, gz); + *z = BMI160_REMAP_AXIS_Z(gx, gy, gz); +} +void BMI160Sensor::remapMagnetometer(sensor_real_t* x, sensor_real_t* y, sensor_real_t* z) { + sensor_real_t mx = *x, my = *y, mz = *z; + *x = BMI160_MAG_REMAP_AXIS_X(mx, my, mz); + *y = BMI160_MAG_REMAP_AXIS_Y(mx, my, mz); + *z = BMI160_MAG_REMAP_AXIS_Z(mx, my, mz); +} + +void BMI160Sensor::getRemappedRotation(int16_t* x, int16_t* y, int16_t* z) { + int16_t gx, gy, gz; + imu.getRotation(&gx, &gy, &gz); + *x = BMI160_REMAP_AXIS_X(gx, gy, gz); + *y = BMI160_REMAP_AXIS_Y(gx, gy, gz); + *z = BMI160_REMAP_AXIS_Z(gx, gy, gz); +} +void BMI160Sensor::getRemappedAcceleration(int16_t* x, int16_t* y, int16_t* z) { + int16_t ax, ay, az; + imu.getAcceleration(&ax, &ay, &az); + *x = BMI160_REMAP_AXIS_X(ax, ay, az); + *y = BMI160_REMAP_AXIS_Y(ax, ay, az); + *z = BMI160_REMAP_AXIS_Z(ax, ay, az); +} + +void BMI160Sensor::getMagnetometerXYZFromBuffer(uint8_t* data, int16_t* x, int16_t* y, int16_t* z) { + #if BMI160_MAG_TYPE == BMI160_MAG_TYPE_HMC + // hmc5883l -> 0 msb 1 lsb + // XZY order + *x = ((int16_t)data[0] << 8) | data[1]; + *z = ((int16_t)data[2] << 8) | data[3]; + *y = ((int16_t)data[4] << 8) | data[5]; + #elif BMI160_MAG_TYPE == BMI160_MAG_TYPE_QMC + // qmc5883l -> 0 lsb 1 msb + // XYZ order + *x = ((int16_t)data[1] << 8) | data[0]; + *y = ((int16_t)data[3] << 8) | data[2]; + *z = ((int16_t)data[5] << 8) | data[4]; + #endif +} \ No newline at end of file diff --git a/src/sensors/bmi160sensor.h b/src/sensors/bmi160sensor.h index 1bc198e..e312e1d 100644 --- a/src/sensors/bmi160sensor.h +++ b/src/sensors/bmi160sensor.h @@ -1,6 +1,6 @@ /* SlimeVR Code is placed under the MIT license - Copyright (c) 2021 Eiren Rain & SlimeVR contributors + Copyright (c) 2022 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 @@ -29,22 +29,250 @@ #include "magneto1.4.h" #include +#include +#include +#include "../motionprocessing/types.h" + +#include "../motionprocessing/GyroTemperatureCalibrator.h" +#include "../motionprocessing/RestDetection.h" + +#if BMI160_USE_VQF + #if USE_6_AXIS + #define BMI160_GYRO_RATE BMI160_GYRO_RATE_400HZ + #else + #define BMI160_GYRO_RATE BMI160_GYRO_RATE_200HZ + #endif +#else + #if USE_6_AXIS + #define BMI160_GYRO_RATE BMI160_GYRO_RATE_800HZ + #else + #define BMI160_GYRO_RATE BMI160_GYRO_RATE_400HZ + #endif +#endif +#define BMI160_GYRO_RANGE BMI160_GYRO_RANGE_1000 +#define BMI160_GYRO_FILTER_MODE BMI160_DLPF_MODE_NORM + +#define BMI160_ACCEL_RATE BMI160_ACCEL_RATE_100HZ +#define BMI160_ACCEL_RANGE BMI160_ACCEL_RANGE_4G +#define BMI160_ACCEL_FILTER_MODE BMI160_DLPF_MODE_NORM + +// note: if changing ODR or filter modes - adjust rest detection params and buffer size + +#define BMI160_TIMESTAMP_RESOLUTION_MICROS 39.0625f +// #define BMI160_TIMESTAMP_RESOLUTION_MICROS 39.0f +#define BMI160_MAP_ODR_MICROS(micros) ((uint16_t)((micros) / BMI160_TIMESTAMP_RESOLUTION_MICROS) * BMI160_TIMESTAMP_RESOLUTION_MICROS) +constexpr float BMI160_ODR_GYR_HZ = 25.0f * (1 << (BMI160_GYRO_RATE - 6)); +constexpr float BMI160_ODR_ACC_HZ = 12.5f * (1 << (BMI160_ACCEL_RATE - 5)); +constexpr float BMI160_ODR_GYR_MICROS = BMI160_MAP_ODR_MICROS(1.0f / BMI160_ODR_GYR_HZ * 1e6f); +constexpr float BMI160_ODR_ACC_MICROS = BMI160_MAP_ODR_MICROS(1.0f / BMI160_ODR_ACC_HZ * 1e6f); +#if !USE_6_AXIS +// note: this value only sets polling and fusion update rate - HMC is internally sampled at 75hz, QMC at 200hz +#define BMI160_MAG_RATE BMI160_MAG_RATE_50HZ +constexpr float BMI160_ODR_MAG_HZ = (25.0f/32.0f) * (1 << (BMI160_MAG_RATE - 1)); +constexpr float BMI160_ODR_MAG_MICROS = BMI160_MAP_ODR_MICROS(1.0f / BMI160_ODR_MAG_HZ * 1e6f); +#else +constexpr float BMI160_ODR_MAG_HZ = 0; +constexpr float BMI160_ODR_MAG_MICROS = 0; +#endif + +constexpr uint16_t BMI160_SETTINGS_MAX_ODR_HZ = max(max(BMI160_ODR_GYR_HZ, BMI160_ODR_ACC_HZ), BMI160_ODR_MAG_HZ); +constexpr uint16_t BMI160_SETTINGS_MAX_ODR_MICROS = BMI160_MAP_ODR_MICROS(1.0f / BMI160_SETTINGS_MAX_ODR_HZ * 1e6f); + +constexpr float BMI160_FIFO_AVG_DATA_FRAME_LENGTH = ( + BMI160_SETTINGS_MAX_ODR_HZ * 1 + + BMI160_ODR_GYR_HZ * BMI160_FIFO_G_LEN + + BMI160_ODR_ACC_HZ * BMI160_FIFO_A_LEN + + BMI160_ODR_MAG_HZ * BMI160_FIFO_M_LEN +) / BMI160_SETTINGS_MAX_ODR_HZ; +constexpr float BMI160_FIFO_READ_BUFFER_SIZE_MICROS = 30000; +constexpr float BMI160_FIFO_READ_BUFFER_SIZE_SAMPLES = + BMI160_SETTINGS_MAX_ODR_HZ * BMI160_FIFO_READ_BUFFER_SIZE_MICROS / 1e6f; +constexpr uint16_t BMI160_FIFO_MAX_LENGTH = 1024; +constexpr uint16_t BMI160_FIFO_READ_BUFFER_SIZE_BYTES = min( + (float)BMI160_FIFO_MAX_LENGTH - 64, + BMI160_FIFO_READ_BUFFER_SIZE_SAMPLES * BMI160_FIFO_AVG_DATA_FRAME_LENGTH * 1.25f +); + +// Typical sensitivity at 25C +// See p. 9 of https://www.mouser.com/datasheet/2/783/BST-BMI160-DS000-1509569.pdf +// #define BMI160_GYRO_TYPICAL_SENSITIVITY_LSB 16.4f // 2000 deg 0 +// #define BMI160_GYRO_TYPICAL_SENSITIVITY_LSB 32.8f // 1000 deg 1 +// #define BMI160_GYRO_TYPICAL_SENSITIVITY_LSB 65.6f // 500 deg 2 +// #define BMI160_GYRO_TYPICAL_SENSITIVITY_LSB 131.2f // 250 deg 3 +// #define BMI160_GYRO_TYPICAL_SENSITIVITY_LSB 262.4f // 125 deg 4 +constexpr double BMI160_GYRO_TYPICAL_SENSITIVITY_LSB = (16.4f * (1 << BMI160_GYRO_RANGE)); + +constexpr std::pair BMI160_ACCEL_SENSITIVITY_LSB_MAP[] = { + {BMI160_ACCEL_RANGE_2G, 16384.0f}, + {BMI160_ACCEL_RANGE_4G, 8192.0f}, + {BMI160_ACCEL_RANGE_8G, 4096.0f}, + {BMI160_ACCEL_RANGE_16G, 2048.0f} +}; +constexpr double BMI160_ACCEL_TYPICAL_SENSITIVITY_LSB = BMI160_ACCEL_SENSITIVITY_LSB_MAP[BMI160_ACCEL_RANGE / 4].second; +constexpr double BMI160_ASCALE = CONST_EARTH_GRAVITY / BMI160_ACCEL_TYPICAL_SENSITIVITY_LSB; + +// Scale conversion steps: LSB/°/s -> °/s -> step/°/s -> step/rad/s +constexpr double BMI160_GSCALE = ((32768. / BMI160_GYRO_TYPICAL_SENSITIVITY_LSB) / 32768.) * (PI / 180.0); + +constexpr float targetSampleRateMs = 10.0f; +constexpr uint32_t targetSampleRateMicros = (uint32_t)targetSampleRateMs * 1e3; + +constexpr uint32_t BMI160_TEMP_CALIBRATION_REQUIRED_SAMPLES_PER_STEP = + TEMP_CALIBRATION_SECONDS_PER_STEP / (BMI160_ODR_GYR_MICROS / 1e6); +static_assert(0x7FFF * BMI160_TEMP_CALIBRATION_REQUIRED_SAMPLES_PER_STEP < 0x7FFFFFFF, "Temperature calibration sum overflow"); + +#define BMI160_VQF_REST_DETECTION_AVAILABLE (BMI160_USE_VQF && !BMI160_USE_BASIC_VQF) + +#if BMI160_USE_VQF +#if !BMI160_USE_BASIC_VQF +struct BMI160VQFParams: VQFParams { + BMI160VQFParams() : VQFParams() { + #ifndef VQF_NO_MOTION_BIAS_ESTIMATION + motionBiasEstEnabled = false; + #endif + tauAcc = 2.0f; + restMinT = 2.0f; + restThGyr = 0.6f; // 400 norm + restThAcc = 0.06f; // 100 norm + } +}; +#endif +#endif + +struct BMI160RestDetectionParams: RestDetectionParams { + BMI160RestDetectionParams() : RestDetectionParams() { + restMinTimeMicros = 2.0f * 1e6; + restThGyr = 0.6f; // 400 norm + restThAcc = 0.06f; // 100 norm + } +}; class BMI160Sensor : public Sensor { public: - BMI160Sensor(uint8_t id, uint8_t address, float rotation) : Sensor("BMI160Sensor", IMU_BMI160, id, address, rotation){}; + BMI160Sensor(uint8_t id, uint8_t address, float rotation) : + Sensor("BMI160Sensor", IMU_BMI160, id, address, rotation) +#if !BMI160_VQF_REST_DETECTION_AVAILABLE + , restDetection(restDetectionParams, BMI160_ODR_GYR_MICROS / 1e6f, BMI160_ODR_ACC_MICROS / 1e6f) +#endif + +#if BMI160_USE_VQF +#if !BMI160_USE_BASIC_VQF + , vqf(vqfParams, BMI160_ODR_GYR_MICROS / 1e6f, BMI160_ODR_ACC_MICROS / 1e6f, BMI160_ODR_MAG_MICROS / 1e6f) +#else + , vqf(BMI160_ODR_GYR_MICROS / 1e6f, BMI160_ODR_ACC_MICROS / 1e6f, BMI160_ODR_MAG_MICROS / 1e6f) +#endif +#endif + { + }; ~BMI160Sensor(){}; + void initHMC(BMI160MagRate magRate); + void initQMC(BMI160MagRate magRate); + void motionSetup() override final; void motionLoop() override final; void startCalibration(int calibrationType) override final; - void getScaledValues(float Gxyz[3], float Axyz[3]); - float getTemperature(); + void maybeCalibrateGyro(); + void maybeCalibrateAccel(); + void maybeCalibrateMag(); + + void printTemperatureCalibrationState() override final; + void printDebugTemperatureCalibrationState() override final; + void resetTemperatureCalibrationState() override final { + gyroTempCalibrator->reset(); + m_Logger.info("Temperature calibration state has been reset for sensorId:%i", sensorId); + }; + void saveTemperatureCalibration() override final; + + void applyAccelCalibrationAndScale(sensor_real_t Axyz[3]); + void applyMagCalibrationAndScale(sensor_real_t Mxyz[3]); + + bool hasGyroCalibration(); + bool hasAccelCalibration(); + bool hasMagCalibration(); + + void onGyroRawSample(uint32_t dtMicros, int16_t x, int16_t y, int16_t z); + void onAccelRawSample(uint32_t dtMicros, int16_t x, int16_t y, int16_t z); + void onMagRawSample(uint32_t dtMicros, int16_t x, int16_t y, int16_t z); + void readFIFO(); + + void getMagnetometerXYZFromBuffer(uint8_t* data, int16_t* x, int16_t* y, int16_t* z); + + void remapGyroAccel(sensor_real_t* x, sensor_real_t* y, sensor_real_t* z); + void remapMagnetometer(sensor_real_t* x, sensor_real_t* y, sensor_real_t* z); + void getRemappedRotation(int16_t* x, int16_t* y, int16_t* z); + void getRemappedAcceleration(int16_t* x, int16_t* y, int16_t* z); + + bool getTemperature(float* out); private: BMI160 imu {}; - float q[4] {1.0f, 0.0f, 0.0f, 0.0f}; - // Loop timing globals - uint32_t now = 0, last = 0; //micros() timers - float deltat = 0; //loop time in seconds + + Mahony mahony; +#if !BMI160_VQF_REST_DETECTION_AVAILABLE + BMI160RestDetectionParams restDetectionParams {}; + RestDetection restDetection; +#endif +#if BMI160_USE_VQF +#if !BMI160_USE_BASIC_VQF + BMI160VQFParams vqfParams {}; + VQF vqf; +#else + BasicVQF vqf; +#endif +#endif + + sensor_real_t qwxyz[4] {1.0f, 0.0f, 0.0f, 0.0f}; + Quaternion quat{}; + + // clock sync and sample timestamping + uint32_t sensorTime0 = 0; + uint32_t sensorTime1 = 0; + uint32_t localTime0 = 0; + uint32_t localTime1 = 0; + double sensorTimeRatio = 1; + double sensorTimeRatioEma = 1; + double sampleDtMicros = BMI160_ODR_GYR_MICROS; + uint32_t syncLatencyMicros = 0; + uint32_t samplesSinceClockSync = 0; + uint32_t timestamp0 = 0; + uint32_t timestamp1 = 0; + + // scheduling + uint32_t lastPollTime = micros(); + uint32_t lastClockPollTime = micros(); + #if BMI160_DEBUG + uint32_t cpuUsageMicros = 0; + uint32_t lastCpuUsagePrinted = 0; + uint32_t gyrReads = 0; + uint32_t accReads = 0; + uint32_t magReads = 0; + uint16_t numFIFODropped = 0; + uint16_t numFIFOFailedReads = 0; + #endif + + uint32_t lastRotationPacketSent = 0; + uint32_t lastTemperaturePacketSent = 0; + + struct BMI160FIFO { + uint8_t data[BMI160_FIFO_READ_BUFFER_SIZE_BYTES]; + uint16_t length; + } fifo {}; + float temperature = 0; + GyroTemperatureCalibrator* gyroTempCalibrator = nullptr; + sensor_real_t Gxyz[3] = {0}; + sensor_real_t Axyz[3] = {0}; + sensor_real_t Mxyz[3] = {0}; + sensor_real_t lastAxyz[3] = {0}; + bool fusionUpdated = false; + + double gscaleX = BMI160_GSCALE; + double gscaleY = BMI160_GSCALE; + double gscaleZ = BMI160_GSCALE; + + double GOxyzStaticTempCompensated[3]; + + bool isGyroCalibrated = false; + bool isAccelCalibrated = false; + bool isMagCalibrated = false; SlimeVR::Configuration::BMI160CalibrationConfig m_Calibration; }; diff --git a/src/sensors/bno055sensor.cpp b/src/sensors/bno055sensor.cpp index 2ef7a85..28a7544 100644 --- a/src/sensors/bno055sensor.cpp +++ b/src/sensors/bno055sensor.cpp @@ -68,9 +68,9 @@ void BNO055Sensor::motionLoop() { #if SEND_ACCELERATION { Vector3 accel = this->imu.getVector(Adafruit_BNO055::VECTOR_LINEARACCEL); - this->acceleration[0] = accel.x; - this->acceleration[1] = accel.y; - this->acceleration[2] = accel.z; + this->linearAcceleration[0] = accel.x; + this->linearAcceleration[1] = accel.y; + this->linearAcceleration[2] = accel.z; } #endif diff --git a/src/sensors/bno080sensor.cpp b/src/sensors/bno080sensor.cpp index c53bda2..628f5e3 100644 --- a/src/sensors/bno080sensor.cpp +++ b/src/sensors/bno080sensor.cpp @@ -158,7 +158,7 @@ void BNO080Sensor::motionLoop() #if SEND_ACCELERATION { uint8_t acc; - this->imu.getLinAccel(this->acceleration[0], this->acceleration[1], this->acceleration[2], acc); + this->imu.getLinAccel(this->linearAcceleration[0], this->linearAcceleration[1], this->linearAcceleration[2], acc); } #endif // SEND_ACCELERATION } // Closing new quaternion if context @@ -223,7 +223,7 @@ void BNO080Sensor::sendData() Network::sendRotationData(&quaternion, DATA_TYPE_NORMAL, calibrationAccuracy, sensorId); #if SEND_ACCELERATION - Network::sendAccel(this->acceleration, this->sensorId); + Network::sendAccel(this->linearAcceleration, this->sensorId); #endif #if !USE_6_AXIS diff --git a/src/sensors/icm20948sensor.cpp b/src/sensors/icm20948sensor.cpp index 6516441..0a0e407 100644 --- a/src/sensors/icm20948sensor.cpp +++ b/src/sensors/icm20948sensor.cpp @@ -32,7 +32,7 @@ int bias_save_periods[] = { 120, 180, 300, 600, 600 }; // 2min + 3min + 5min + 1 #define ACCEL_SENSITIVITY_4G 8192.0f // Accel scale conversion steps: LSB/G -> G -> m/s^2 -constexpr float ASCALE_4G = ((32768. / ACCEL_SENSITIVITY_4G) / 32768.) * EARTH_GRAVITY; +constexpr float ASCALE_4G = ((32768. / ACCEL_SENSITIVITY_4G) / 32768.) * CONST_EARTH_GRAVITY; void ICM20948Sensor::motionSetup() { @@ -108,7 +108,7 @@ void ICM20948Sensor::sendData() #if SEND_ACCELERATION { - Network::sendAccel(acceleration, sensorId); + Network::sendAccel(linearAcceleration, sensorId); } #endif } @@ -484,9 +484,9 @@ void ICM20948Sensor::calculateAccelerationWithoutGravity(Quat *quaternion) { if((dmpData.header & DMP_header_bitmap_Accel) > 0) { - this->acceleration[0] = (float)this->dmpData.Raw_Accel.Data.X; - this->acceleration[1] = (float)this->dmpData.Raw_Accel.Data.Y; - this->acceleration[2] = (float)this->dmpData.Raw_Accel.Data.Z; + this->linearAcceleration[0] = (float)this->dmpData.Raw_Accel.Data.X; + this->linearAcceleration[1] = (float)this->dmpData.Raw_Accel.Data.Y; + this->linearAcceleration[2] = (float)this->dmpData.Raw_Accel.Data.Z; // get the component of the acceleration that is gravity float gravity[3]; @@ -495,14 +495,14 @@ void ICM20948Sensor::calculateAccelerationWithoutGravity(Quat *quaternion) gravity[2] = quaternion->w * quaternion->w - quaternion->x * quaternion->x - quaternion->y * quaternion->y + quaternion->z * quaternion->z; // subtract gravity from the acceleration vector - this->acceleration[0] -= gravity[0] * ACCEL_SENSITIVITY_4G; - this->acceleration[1] -= gravity[1] * ACCEL_SENSITIVITY_4G; - this->acceleration[2] -= gravity[2] * ACCEL_SENSITIVITY_4G; + this->linearAcceleration[0] -= gravity[0] * ACCEL_SENSITIVITY_4G; + this->linearAcceleration[1] -= gravity[1] * ACCEL_SENSITIVITY_4G; + this->linearAcceleration[2] -= gravity[2] * ACCEL_SENSITIVITY_4G; // finally scale the acceleration values to mps2 - this->acceleration[0] *= ASCALE_4G; - this->acceleration[1] *= ASCALE_4G; - this->acceleration[2] *= ASCALE_4G; + this->linearAcceleration[0] *= ASCALE_4G; + this->linearAcceleration[1] *= ASCALE_4G; + this->linearAcceleration[2] *= ASCALE_4G; } } #endif diff --git a/src/sensors/mpu6050sensor.cpp b/src/sensors/mpu6050sensor.cpp index 1b234a1..8c18a0c 100644 --- a/src/sensors/mpu6050sensor.cpp +++ b/src/sensors/mpu6050sensor.cpp @@ -38,7 +38,7 @@ #define ACCEL_SENSITIVITY_2G 16384.0f // Accel scale conversion steps: LSB/G -> G -> m/s^2 -constexpr float ASCALE_2G = ((32768. / ACCEL_SENSITIVITY_2G) / 32768.) * EARTH_GRAVITY; +constexpr float ASCALE_2G = ((32768. / ACCEL_SENSITIVITY_2G) / 32768.) * CONST_EARTH_GRAVITY; void MPU6050Sensor::motionSetup() { @@ -156,9 +156,9 @@ void MPU6050Sensor::motionLoop() this->imu.dmpGetLinearAccel(&this->rawAccel, &this->rawAccel, &gravity); // convert acceleration to m/s^2 (implicitly casts to float) - this->acceleration[0] = this->rawAccel.x * ASCALE_2G; - this->acceleration[1] = this->rawAccel.y * ASCALE_2G; - this->acceleration[2] = this->rawAccel.z * ASCALE_2G; + this->linearAcceleration[0] = this->rawAccel.x * ASCALE_2G; + this->linearAcceleration[1] = this->rawAccel.y * ASCALE_2G; + this->linearAcceleration[2] = this->rawAccel.z * ASCALE_2G; } #endif diff --git a/src/sensors/mpu9250sensor.cpp b/src/sensors/mpu9250sensor.cpp index f62e8e4..8f391c1 100644 --- a/src/sensors/mpu9250sensor.cpp +++ b/src/sensors/mpu9250sensor.cpp @@ -42,7 +42,7 @@ constexpr float gscale = (250. / 32768.0) * (PI / 180.0); //gyro default 250 LSB #define ACCEL_SENSITIVITY_2G 16384.0f // Accel scale conversion steps: LSB/G -> G -> m/s^2 -constexpr float ASCALE_2G = ((32768. / ACCEL_SENSITIVITY_2G) / 32768.) * EARTH_GRAVITY; +constexpr float ASCALE_2G = ((32768. / ACCEL_SENSITIVITY_2G) / 32768.) * CONST_EARTH_GRAVITY; void MPU9250Sensor::motionSetup() { // initialize device @@ -198,9 +198,9 @@ void MPU9250Sensor::motionLoop() { this->imu.dmpGetLinearAccel(&this->rawAccel, &this->rawAccel, &grav); // convert acceleration to m/s^2 (implicitly casts to float) - this->acceleration[0] = this->rawAccel.x * ASCALE_2G; - this->acceleration[1] = this->rawAccel.y * ASCALE_2G; - this->acceleration[2] = this->rawAccel.z * ASCALE_2G; + this->linearAcceleration[0] = this->rawAccel.x * ASCALE_2G; + this->linearAcceleration[1] = this->rawAccel.y * ASCALE_2G; + this->linearAcceleration[2] = this->rawAccel.z * ASCALE_2G; } #endif diff --git a/src/sensors/sensor.cpp b/src/sensors/sensor.cpp index afb08f3..147ce82 100644 --- a/src/sensors/sensor.cpp +++ b/src/sensors/sensor.cpp @@ -36,7 +36,7 @@ void Sensor::sendData() { #if SEND_ACCELERATION { - Network::sendAccel(acceleration, sensorId); + Network::sendAccel(linearAcceleration, sensorId); } #endif @@ -46,6 +46,14 @@ void Sensor::sendData() { } } +void Sensor::printTemperatureCalibrationUnsupported() { + m_Logger.error("Temperature calibration not supported for IMU %s", getIMUNameByType(sensorType)); +} +void Sensor::printTemperatureCalibrationState() { printTemperatureCalibrationUnsupported(); }; +void Sensor::printDebugTemperatureCalibrationState() { printTemperatureCalibrationUnsupported(); }; +void Sensor::saveTemperatureCalibration() { printTemperatureCalibrationUnsupported(); }; +void Sensor::resetTemperatureCalibrationState() { printTemperatureCalibrationUnsupported(); }; + const char * getIMUNameByType(int imuType) { switch(imuType) { case IMU_MPU9250: diff --git a/src/sensors/sensor.h b/src/sensors/sensor.h index f7a8e8e..b6b0999 100644 --- a/src/sensors/sensor.h +++ b/src/sensors/sensor.h @@ -32,7 +32,6 @@ #include "logging/Logger.h" #include "utils.h" -#define EARTH_GRAVITY 9.80665 #define DATA_TYPE_NORMAL 1 #define DATA_TYPE_CORRECTION 2 @@ -54,6 +53,10 @@ public: virtual void sendData(); virtual void startCalibration(int calibrationType){}; virtual uint8_t getSensorState(); + virtual void printTemperatureCalibrationState(); + virtual void printDebugTemperatureCalibrationState(); + virtual void resetTemperatureCalibrationState(); + virtual void saveTemperatureCalibration(); bool isWorking() { return working; @@ -82,9 +85,12 @@ protected: Quat quaternion{}; Quat lastQuatSent{}; - float acceleration[3]{}; + float linearAcceleration[3]{}; SlimeVR::Logging::Logger m_Logger; + +private: + void printTemperatureCalibrationUnsupported(); }; const char * getIMUNameByType(int imuType); diff --git a/src/serial/serialcommands.cpp b/src/serial/serialcommands.cpp index 58eb613..bfb9063 100644 --- a/src/serial/serialcommands.cpp +++ b/src/serial/serialcommands.cpp @@ -36,7 +36,7 @@ namespace SerialCommands { SlimeVR::Logging::Logger logger("SerialCommands"); - CmdCallback<5> cmdCallbacks; + CmdCallback<6> cmdCallbacks; CmdParser cmdParser; CmdBuffer<64> cmdBuffer; @@ -192,12 +192,41 @@ namespace SerialCommands { ESP.restart(); } + void cmdTemperatureCalibration(CmdParser* parser) { + if (parser->getParamCount() > 1) { + if (parser->equalCmdParam(1, "PRINT")) { + sensorManager.getFirst()->printTemperatureCalibrationState(); + sensorManager.getSecond()->printTemperatureCalibrationState(); + return; + } else if (parser->equalCmdParam(1, "DEBUG")) { + sensorManager.getFirst()->printDebugTemperatureCalibrationState(); + sensorManager.getSecond()->printDebugTemperatureCalibrationState(); + return; + } else if (parser->equalCmdParam(1, "RESET")) { + sensorManager.getFirst()->resetTemperatureCalibrationState(); + sensorManager.getSecond()->resetTemperatureCalibrationState(); + return; + } else if (parser->equalCmdParam(1, "SAVE")) { + sensorManager.getFirst()->saveTemperatureCalibration(); + sensorManager.getSecond()->saveTemperatureCalibration(); + return; + } + } + logger.info("Usage:"); + logger.info(" TCAL PRINT: print current temperature calibration config"); + logger.info(" TCAL DEBUG: print debug values for the current temperature calibration profile"); + logger.info(" TCAL RESET: reset current temperature calibration in RAM (does not delete already saved)"); + logger.info(" TCAL SAVE: save current temperature calibration to persistent flash"); + logger.info("Note:"); + logger.info(" Temperature calibration config saves automatically when calibration percent is at 100%"); + } + void setUp() { cmdCallbacks.addCmd("SET", &cmdSet); cmdCallbacks.addCmd("GET", &cmdGet); cmdCallbacks.addCmd("FRST", &cmdFactoryReset); cmdCallbacks.addCmd("REBOOT", &cmdReboot); - + cmdCallbacks.addCmd("TCAL", &cmdTemperatureCalibration); } void update() {