mirror of
https://github.com/SlimeVR/SlimeVR-Tracker-ESP.git
synced 2026-04-06 02:01:57 +02:00
Improved BMI160 support (#220)
This commit is contained in:
0forks
GitHub
53
README.md
53
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.
|
||||
|
||||
@@ -5,4 +5,5 @@ monitor_speed = 115200
|
||||
framework = arduino
|
||||
build_flags =
|
||||
-O2
|
||||
build_unflags = -Os
|
||||
-std=gnu++17
|
||||
build_unflags = -Os -std=gnu++11
|
||||
|
||||
@@ -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);
|
||||
}
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
@@ -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;
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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<typename T>
|
||||
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_ */
|
||||
33
lib/magnetometers/hmc5883l.h
Normal file
33
lib/magnetometers/hmc5883l.h
Normal file
@@ -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
|
||||
17
lib/magnetometers/qmc5883l.h
Normal file
17
lib/magnetometers/qmc5883l.h
Normal file
@@ -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
|
||||
407
lib/vqf/basicvqf.cpp
Normal file
407
lib/vqf/basicvqf.cpp
Normal file
@@ -0,0 +1,407 @@
|
||||
// SPDX-FileCopyrightText: 2021 Daniel Laidig <laidig@control.tu-berlin.de>
|
||||
//
|
||||
// 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 <algorithm>
|
||||
#include <limits>
|
||||
#define _USE_MATH_DEFINES
|
||||
#include <math.h>
|
||||
#include <assert.h>
|
||||
|
||||
#define EPS std::numeric_limits<vqf_real_t>::epsilon()
|
||||
#define NaN std::numeric_limits<vqf_real_t>::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();
|
||||
}
|
||||
502
lib/vqf/basicvqf.h
Normal file
502
lib/vqf/basicvqf.h
Normal file
@@ -0,0 +1,502 @@
|
||||
// SPDX-FileCopyrightText: 2021 Daniel Laidig <laidig@control.tu-berlin.de>
|
||||
//
|
||||
// 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 <stddef.h>
|
||||
|
||||
#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 <https://doi.org/10.1016/j.inffus.2022.10.014>`_.
|
||||
* [Accepted manuscript available at `arXiv:2203.17024 <https://arxiv.org/abs/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() <VQF.getQuat6D>`, :meth:`getQuat9D() <VQF.getQuat9D>` and
|
||||
* the other getter methods.
|
||||
*
|
||||
* If the full data is available in (row-major) data buffers, you can use :meth:`updateBatch() <VQF.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 <VQF>` | -- | -- |
|
||||
* +------------------------+--------------------------+---------------------------+---------------------------+
|
||||
* \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
|
||||
943
lib/vqf/vqf.cpp
Normal file
943
lib/vqf/vqf.cpp
Normal file
@@ -0,0 +1,943 @@
|
||||
// SPDX-FileCopyrightText: 2021 Daniel Laidig <laidig@control.tu-berlin.de>
|
||||
//
|
||||
// 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 <algorithm>
|
||||
#include <limits>
|
||||
#define _USE_MATH_DEFINES
|
||||
#include <math.h>
|
||||
#include <assert.h>
|
||||
|
||||
#define EPS std::numeric_limits<vqf_real_t>::epsilon()
|
||||
#define NaN std::numeric_limits<vqf_real_t>::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();
|
||||
}
|
||||
1018
lib/vqf/vqf.h
Normal file
1018
lib/vqf/vqf.h
Normal file
File diff suppressed because it is too large
Load Diff
@@ -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
|
||||
|
||||
|
||||
@@ -202,9 +202,6 @@ namespace SlimeVR
|
||||
on();
|
||||
m_CurrentStage = ON;
|
||||
break;
|
||||
on();
|
||||
m_CurrentStage = ON;
|
||||
break;
|
||||
}
|
||||
}
|
||||
else
|
||||
|
||||
@@ -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];
|
||||
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
|
||||
@@ -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);
|
||||
|
||||
@@ -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)
|
||||
|
||||
85
src/defines_bmi160.h
Normal file
85
src/defines_bmi160.h
Normal file
@@ -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 <debug.h>).
|
||||
#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
|
||||
60
src/defines_sensitivity.h
Normal file
60
src/defines_sensitivity.h
Normal file
@@ -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
|
||||
@@ -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
|
||||
|
||||
@@ -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
|
||||
|
||||
211
src/motionprocessing/GyroTemperatureCalibrator.cpp
Normal file
211
src/motionprocessing/GyroTemperatureCalibrator.cpp
Normal file
@@ -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;
|
||||
}
|
||||
219
src/motionprocessing/GyroTemperatureCalibrator.h
Normal file
219
src/motionprocessing/GyroTemperatureCalibrator.h
Normal file
@@ -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 <Arduino.h>
|
||||
#include <stdint.h>
|
||||
#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
|
||||
125
src/motionprocessing/OnlinePolyfit.h
Normal file
125
src/motionprocessing/OnlinePolyfit.h
Normal file
@@ -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 <Arduino.h>
|
||||
|
||||
#ifndef ONLINE_POLYFIT_H
|
||||
#define ONLINE_POLYFIT_H
|
||||
|
||||
template <uint32_t degree, uint32_t dimensions, uint64_t forgettingFactorNumSamples>
|
||||
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<float, float> 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
|
||||
262
src/motionprocessing/RestDetection.h
Normal file
262
src/motionprocessing/RestDetection.h
Normal file
@@ -0,0 +1,262 @@
|
||||
// SPDX-FileCopyrightText: 2021 Daniel Laidig <laidig@control.tu-berlin.de>
|
||||
// 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 <Arduino.h>
|
||||
#include <vqf.h>
|
||||
#include <basicvqf.h>
|
||||
#include "types.h"
|
||||
|
||||
#define NaN std::numeric_limits<sensor_real_t>::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
|
||||
10
src/motionprocessing/types.h
Normal file
10
src/motionprocessing/types.h
Normal file
@@ -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
|
||||
File diff suppressed because it is too large
Load Diff
@@ -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 <BMI160.h>
|
||||
#include <vqf.h>
|
||||
#include <basicvqf.h>
|
||||
#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<uint8_t, float> 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<sensor_real_t> 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;
|
||||
};
|
||||
|
||||
@@ -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
|
||||
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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
|
||||
|
||||
|
||||
@@ -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
|
||||
|
||||
|
||||
@@ -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:
|
||||
|
||||
@@ -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);
|
||||
|
||||
@@ -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() {
|
||||
|
||||
Reference in New Issue
Block a user