From 07460ea4c025b0de8bdbc9a1a38c8f3eff331fac Mon Sep 17 00:00:00 2001 From: GarryBGoode Date: Sat, 7 Jun 2025 13:28:00 +0200 Subject: [PATCH 01/16] Fixed int16 reconstruction in IMU reading. Removed oversampling, perform 1 read per update. Enabled internal LPF1 for gyro and accel. Increased tick frequency to 1kHz. Added compensation math for tick frequency, overall sensitivity should not change with frequency change. Added large number of gyro defines to be used later. Added rotation_fast.c and .h, which is an approximate rotation math setup, this can be used later for absolute angle tracking at 1kHz. --- src/gyro.c | 10 +++- src/headers/config.h | 6 ++- src/headers/imu.h | 93 +++++++++++++++++++++++++++++++------ src/headers/rotation_fast.h | 10 ++++ src/imu.c | 20 ++++---- src/rotation_fast.c | 64 +++++++++++++++++++++++++ 6 files changed, 177 insertions(+), 26 deletions(-) create mode 100644 src/headers/rotation_fast.h create mode 100644 src/rotation_fast.c diff --git a/src/gyro.c b/src/gyro.c index b9334594..d61a0d9d 100644 --- a/src/gyro.c +++ b/src/gyro.c @@ -159,14 +159,20 @@ void Gyro__report_incremental(Gyro *self) { double y = imu_gyro.y * CFG_GYRO_SENSITIVITY_Y * sensitivity_multiplier; double z = imu_gyro.z * CFG_GYRO_SENSITIVITY_Z * sensitivity_multiplier; // Additional processing. - double t = 1.0; - double k = 0.5; + double t = CFG_IMU_DEADZONE; + double k = CFG_IMU_DEADZONE_STRENGTH; if (x > 0 && x < t) x = hssnf(t, k, x); else if (x < 0 && x > -t) x = -hssnf(t, k, -x); if (y > 0 && y < t) y = hssnf(t, k, y); else if (y < 0 && y > -t) y = -hssnf(t, k, -y); if (z > 0 && z < t) z = hssnf(t, k, z); else if (z < 0 && z > -t) z = -hssnf(t, k, -z); + + // compensate tick frequency. + x *= (double)REFERENCE_TICK_FREQUENCY/(double)CFG_TICK_FREQUENCY; + y *= (double)REFERENCE_TICK_FREQUENCY/(double)CFG_TICK_FREQUENCY; + z *= (double)REFERENCE_TICK_FREQUENCY/(double)CFG_TICK_FREQUENCY; + // Reintroduce subpixel leftovers. x += sub_x; y += sub_y; diff --git a/src/headers/config.h b/src/headers/config.h index 76ed3877..5400bd0f 100644 --- a/src/headers/config.h +++ b/src/headers/config.h @@ -30,11 +30,15 @@ #ifdef DEVICE_DONGLE #define CFG_TICK_FREQUENCY 1000 // Hz. #else - #define CFG_TICK_FREQUENCY 250 // Hz. + #define CFG_TICK_FREQUENCY 1000 // Hz. + #define REFERENCE_TICK_FREQUENCY 250 // Hz. This used to be the default, it is used for backward compatibility. #endif #define CFG_IMU_TICK_SAMPLES 128 // Multi-sampling per pooling cycle. +#define CFG_IMU_DEADZONE 1.0 // Deadzone for the IMU in pixels per second +#define CFG_IMU_DEADZONE_STRENGTH 0.5 // Strength of the deadzone, the amount of movement reduction near zero. + #define CFG_TICK_INTERVAL_IN_MS (1000 / CFG_TICK_FREQUENCY) #define CFG_TICK_INTERVAL_IN_US (1000000 / CFG_TICK_FREQUENCY) diff --git a/src/headers/imu.h b/src/headers/imu.h index dcb21563..6622787f 100644 --- a/src/headers/imu.h +++ b/src/headers/imu.h @@ -5,25 +5,88 @@ #include "vector.h" // LSM6DSR -#define IMU_WHO_AM_I 0x0f // Identifier address. -#define IMU_CTRL1_XL 0x10 // Accelerometer config address. -#define IMU_CTRL2_G 0x11 // Gyroscope config address. -#define IMU_CTRL3_C 0x12 // IMU config address. -#define IMU_CTRL8_XL 0x17 // Accelerometer filter config address. -#define IMU_OUTX_L_G 0x22 // Gyroscope read X address. -#define IMU_OUTY_L_G 0x24 // Gyroscope read Y address. -#define IMU_OUTZ_L_G 0x26 // Gyroscope read Z address. -#define IMU_OUTX_L_XL 0x28 // Accelerometer read X address. -#define IMU_OUTY_L_XL 0x30 // Accelerometer read Y address. -#define IMU_OUTZ_L_XL 0x2A // Accelerometer read Z address. - -#define IMU_READ 0b10000000 // Read byte. +#define IMU_READ 0b10000000 +#define IMU_WHO_AM_I 0x0f // Identifier address. +#define IMU_CTRL1_XL 0x10 // Accelerometer config address. +#define IMU_CTRL2_G 0x11 // Gyroscope config address. +#define IMU_CTRL3_C 0x12 // IMU config address. +#define IMU_CTRL4_C 0x13 // IMU config4 address. Has LPF1 enable. +#define IMU_CTRL6_C 0x15 // IMU LPF config address. +#define IMU_CTRL8_XL 0x17 // Accelerometer filter config address. +#define IMU_OUTX_L_G 0x22 // Gyroscope read X address. +#define IMU_OUTY_L_G 0x24 // Gyroscope read Y address. +#define IMU_OUTZ_L_G 0x26 // Gyroscope read Z address. +#define IMU_OUTX_L_XL 0x28 // Accelerometer read X address. +#define IMU_OUTY_L_XL 0x30 // Accelerometer read Y address. +#define IMU_OUTZ_L_XL 0x2A // Accelerometer read Z address. + + #define IMU_CTRL1_XL_OFF 0b00000000 // Accelerometer value power off. -#define IMU_CTRL1_XL_2G 0b10100010 // Accelerometer value for 2G range. -#define IMU_CTRL8_XL_LP 0b00000000 // Accelerometer value for low pass filter. +#define IMU_CTRL1_XL_2G 0b10100010 // Accelerometer value for 2G range, LPF2 enabled +#define IMU_CTRL1_XL_2G_NOFILT 0b10100000 // Accelerometer value for 2G range, LPF2 disabled + +/* XL = 100b that is ODR/100 bandwidth, 66Hz for 6.6kHz ODR */ +#define IMU_CTRL8_XL_LP 0b10000000 // Accelerometer value for low pass filter. #define IMU_CTRL2_G_OFF 0b00000000 // Gyroscope value power off. #define IMU_CTRL2_G_125 0b10100010 // Gyroscope value for 125 dps. +#define IMU_CTRL2_G_250 0b10100011 // Gyroscope value for 125 dps. #define IMU_CTRL2_G_500 0b10100100 // Gyroscope value for 500 dps. +#define IMU_CTRL2_G_1000 0b10100101 // Gyroscope value for 1000 dps. + +#define GYRO_LPF1_ENABLE_CTRL4_C 0b00000010 // enable LPF1 in CTRL4_C register +#define GYRO_LPF1_DISABLE_CTRL4_C 0b00000000 // disable LPF1 in CTRL4_C register +#define GYRO_LPF1_470HZ_CTRL6_C 0b00000011 // FTYPE selection for LPF1, 470Hz cutoff at 6.6kHz ODR +#define GYRO_LPF1_223HZ_CTRL6_C 0b00000001 // FTYPE selection for LPF1, 223Hz cutoff at 6.6kHz ODR +#define GYRO_LPF1_297HZ_CTRL6_C 0b00000000 // FTYPE selection for LPF1, 297Hz cutoff at 6.6kHz ODR + +#define GYRO_FS_REG_125 0b00000010 // Gyro full scale register for 125 dps. +#define GYRO_FS_REG_250 0b00000000 // Gyro full scale register for 250 dps. +#define GYRO_FS_REG_500 0b00000100 // Gyro full scale register for 500 dps. +#define GYRO_FS_REG_1000 0b00001000 // Gyro full scale register for 1000 dps. +#define GYRO_FS_REG_2000 0b00001100 // Gyro full scale register for 2000 dps. +#define GYRO_FS_REG_4000 0b00000001 // Gyro full scale register for 4000 dps. + +/* Gyro output data rate options (Hz)*/ +#define GYRO_ODR_REG_12_5 0b00010000 +#define GYRO_ODR_REG_26 0b00100000 +#define GYRO_ODR_REG_52 0b00110000 +#define GYRO_ODR_REG_104 0b01000000 +#define GYRO_ODR_REG_208 0b01010000 +#define GYRO_ODR_REG_416 0b01100000 +#define GYRO_ODR_REG_833 0b01110000 +#define GYRO_ODR_REG_1660 0b10000000 +#define GYRO_ODR_REG_3330 0b10010000 +#define GYRO_ODR_REG_6660 0b10100000 + +/* Gyro int-to-float conversion constants (rad/s)*/ +#define GYRO_SENS_RADPS_125 7.63581547747519E-05f +#define GYRO_SENS_RADPS_250 0.000152716309549504f +#define GYRO_SENS_RADPS_500 0.000305432619099008f +#define GYRO_SENS_RADPS_1000 0.000610865238198015f +#define GYRO_SENS_RADPS_2000 0.00122173047639603f +#define GYRO_SENS_RADPS_4000 0.00244346095279206f + +#define GYRO_SENS_DEGPS_125 4.375E-03f +#define GYRO_SENS_DEGPS_250 8.75E-03f +#define GYRO_SENS_DEGPS_500 1.75E-2f +#define GYRO_SENS_DEGPS_1000 3.5E-2f +#define GYRO_SENS_DEGPS_2000 7E-2f +#define GYRO_SENS_DEGPS_4000 14E-2f + +#define RAD_2_DEG 57.2957795130823f +#define DEG_2_RAD 0.0174532925199433f +#define ACCEL_G 9.80665f // Standard gravity in m/s^2. + + + +#define ACCEL_SENS_2G 0.00059841f +#define ACCEL_SENS_4G 0.00119682f +#define ACCEL_SENS_8G 0.00239364f +#define ACCEL_SENS_16G 0.00478728f + + + + #define GYRO_USER_OFFSET_FACTOR 1.5 diff --git a/src/headers/rotation_fast.h b/src/headers/rotation_fast.h new file mode 100644 index 00000000..d3894a53 --- /dev/null +++ b/src/headers/rotation_fast.h @@ -0,0 +1,10 @@ +typedef struct rotation_statevector { + float ux; + float uy; + float uz; + float phi; +} RotationStateVector; + + +void update_rotation_state(RotationStateVector *state, const float *gyro, const float *accel, float dt); +void normalize_adjust_state(RotationStateVector *v); \ No newline at end of file diff --git a/src/imu.c b/src/imu.c index 39baccc2..4769af00 100644 --- a/src/imu.c +++ b/src/imu.c @@ -42,8 +42,12 @@ void imu_channel_select() { void imu_init_single(uint8_t cs, uint8_t gyro_conf) { uint8_t id = bus_spi_read_one(cs, IMU_READ | IMU_WHO_AM_I); + // Accel options. bus_spi_write(cs, IMU_CTRL1_XL, IMU_CTRL1_XL_2G); bus_spi_write(cs, IMU_CTRL8_XL, IMU_CTRL8_XL_LP); + // Gyro options. + bus_spi_write(cs, IMU_CTRL4_C, GYRO_LPF1_ENABLE_CTRL4_C); // Enable LPF1 + bus_spi_write(cs, IMU_CTRL6_C, GYRO_LPF1_470HZ_CTRL6_C); // FTYPE selection for LPF1 bus_spi_write(cs, IMU_CTRL2_G, gyro_conf); uint8_t xl = bus_spi_read_one(cs, IMU_READ | IMU_CTRL1_XL); uint8_t g = bus_spi_read_one(cs, IMU_READ | IMU_CTRL2_G); @@ -78,9 +82,9 @@ void imu_power_off() { Vector imu_read_gyro_bits(uint8_t cs) { uint8_t buf[6]; bus_spi_read(cs, IMU_READ | IMU_OUTX_L_G, buf, 6); - int16_t y = (((int8_t)buf[1] << 8) + (int8_t)buf[0]); - int16_t z = (((int8_t)buf[3] << 8) + (int8_t)buf[2]); - int16_t x = -(((int8_t)buf[5] << 8) + (int8_t)buf[4]); + int16_t y = (int16_t)(((uint16_t)buf[1] << 8) | (uint16_t)buf[0]); + int16_t z = (int16_t)(((uint16_t)buf[3] << 8) | (uint16_t)buf[2]); + int16_t x = -(int16_t)(((uint16_t)buf[5] << 8) | (uint16_t)buf[4]); double offset_x = (cs==PIN_SPI_CS0) ? offset_gyro_0_x : offset_gyro_1_x; double offset_y = (cs==PIN_SPI_CS0) ? offset_gyro_0_y : offset_gyro_1_y; double offset_z = (cs==PIN_SPI_CS0) ? offset_gyro_0_z : offset_gyro_1_z; @@ -102,9 +106,9 @@ Vector imu_read_gyro_bits(uint8_t cs) { Vector imu_read_accel_bits(uint8_t cs) { uint8_t buf[6]; bus_spi_read(cs, IMU_READ | IMU_OUTX_L_XL, buf, 6); - int16_t x = (((int8_t)buf[1] << 8) + (int8_t)buf[0]); - int16_t y = (((int8_t)buf[3] << 8) + (int8_t)buf[2]); - int16_t z = (((int8_t)buf[5] << 8) + (int8_t)buf[4]); + int16_t x = (int16_t)(((uint16_t)buf[1] << 8) + (uint16_t)buf[0]); + int16_t y = (int16_t)(((uint16_t)buf[3] << 8) + (uint16_t)buf[2]); + int16_t z = (int16_t)(((uint16_t)buf[5] << 8) + (uint16_t)buf[4]); double offset_x = (cs==PIN_SPI_CS0) ? offset_accel_0_x : offset_accel_1_x; double offset_y = (cs==PIN_SPI_CS0) ? offset_accel_0_y : offset_accel_1_y; double offset_z = (cs==PIN_SPI_CS0) ? offset_accel_0_z : offset_accel_1_z; @@ -140,8 +144,8 @@ Vector imu_read_gyro_burst(uint8_t cs, uint8_t samples) { } Vector imu_read_gyro() { - Vector gyro0 = imu_read_gyro_burst(IMU0, CFG_IMU_TICK_SAMPLES/8*1); - Vector gyro1 = imu_read_gyro_burst(IMU1, CFG_IMU_TICK_SAMPLES/8*7); + Vector gyro0 = imu_read_gyro_bits(IMU0); + Vector gyro1 = imu_read_gyro_bits(IMU1); double weight = max(abs(gyro1.x), abs(gyro1.y)) / 32768.0; double weight_0 = ramp_mid(weight, 0.2); double weight_1 = 1 - weight_0; diff --git a/src/rotation_fast.c b/src/rotation_fast.c new file mode 100644 index 00000000..c9e8c392 --- /dev/null +++ b/src/rotation_fast.c @@ -0,0 +1,64 @@ +#include "rotation_fast.h" +#include + +// Time constant for the comlementary filter. +// Tune the denominator to adjust the filter's response time. +// A value of 0.8 means the filter will converge in about 3x0.8 = 2.4 seconds. +float comp_tau = 1/0.8f; + +void update_rotation_state(RotationStateVector *state, const float *gyro, const float *accel, float dt) +{ + /* This is a rotation scheme that tracks the orientation of the 'up' unit vector, + and a rotation angle around the vertical axis (yaw). + The tracking is done from the local coordinate system, which is aligned with the device's orientation. + At each step the 'up' vector gets adjusted using an approximate rotation from gyro rates, + using cross-product with the current 'up' vector.*/ + float gyro_x = gyro[0]; + float gyro_y = gyro[1]; + float gyro_z = gyro[2]; + // yaw is by definition the rotation around the vertical axis, + // so we can calculate it by projecting the gyro vector onto the current 'up' vector. + // The 'up' vector is represented by the state vector (ux, uy, uz). + float yaw_value = state->ux*gyro_x + state->uy*gyro_y + state->uz*gyro_z; + // The roll and pitch components of the gyro vector are the components that are not aligned with the 'up' vector. + float gyro_rollpitch_x = gyro_x - state->ux * yaw_value; + float gyro_rollpitch_y = gyro_y - state->uy * yaw_value; + float gyro_rollpitch_z = gyro_z - state->uz * yaw_value; + + float accel_norm = sqrtf(accel[0] * accel[0] + accel[1] * accel[1] + accel[2] * accel[2]); + // Cross product of the gyro roll/pitch vector with the current 'up' vector gives us the rotation vector + // that we need to apply to the 'up' vector to adjust it. + float wcross_vector[3] = {gyro_rollpitch_z*state->uy - gyro_rollpitch_y*state->uz, + gyro_rollpitch_x*state->uz - gyro_rollpitch_z*state->ux, + gyro_rollpitch_y*state->ux - gyro_rollpitch_x*state->uy}; + state->phi += yaw_value * dt; + + state->ux += wcross_vector[0] * dt; + state->uy += wcross_vector[1] * dt; + state->uz += wcross_vector[2] * dt; + + // Adjusting the 'up' vector using the accelerometer data and complementary filter. + // Only perform the adjustment if the accelerometer data is within a reasonable range. + if (accel_norm>8 && accel_norm<12) { + state->ux += (accel[0]/accel_norm-state->ux) * comp_tau * dt; + state->uy += (accel[1]/accel_norm-state->uy) * comp_tau * dt; + state->uz += (accel[2]/accel_norm-state->uz) * comp_tau * dt; + } + normalize_adjust_state(state); +} + +void normalize_adjust_state(RotationStateVector *v) +{ + /* A normalization method based on successive adjustments, to avoid square root operation*/ + float norm = v->ux * v->ux + v->uy * v->uy + v->uz * v->uz; + float norm_diff = 1.0f - norm; + float gain = 1.0f; + if (fabsf(norm_diff) > 1e-12) + { + gain = 1.0f+ norm_diff*0.5f; + if (gain < 0.01f) gain = 0.01f; + v->ux *= gain; + v->uy *= gain; + v->uz *= gain; + } +} \ No newline at end of file From 8eb8cfbf1263c3ad62a1d8105c0970ecd48682d9 Mon Sep 17 00:00:00 2001 From: GarryBGoode Date: Sun, 8 Jun 2025 14:01:49 +0200 Subject: [PATCH 02/16] Added new gyro absolute calculation, runs in 1kHz. --- CMakeLists.txt | 1 + src/gyro.c | 34 +++++++++++++++++++++++++++++++++- 2 files changed, 34 insertions(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index ef6a8337..5446bd76 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -102,6 +102,7 @@ target_sources(${PROJECT} PUBLIC src/profiles/racing.c src/profiles/rts.c src/rotary.c + src/rotation_fast.c src/self_test.c src/thanks.c src/thumbstick.c diff --git a/src/gyro.c b/src/gyro.c index d61a0d9d..bf9023db 100644 --- a/src/gyro.c +++ b/src/gyro.c @@ -14,6 +14,7 @@ #include "pin.h" #include "touch.h" #include "vector.h" +#include "rotation_fast.h" double sensitivity_multiplier; @@ -22,6 +23,7 @@ Vector world_top; Vector world_fw; Vector world_right; Vector accel_smooth; +RotationStateVector rotation_state; void gyro_update_sensitivity() { uint8_t preset = config_get_mouse_sens_preset(); @@ -149,6 +151,36 @@ void Gyro__report_absolute(Gyro *self) { // printf("\r%6.1f %6.1f %6.1f", x*100, y*100, z*100); } +void Gyro__report_absolute_fast(Gyro *self){ + Vector gyro = imu_read_gyro(); + Vector accel = imu_read_accel(); + // gyro axis convention is different from physical IMU axis convention. + float gyro_arr[3] = {gyro.y * GYRO_SENS_RADPS_500, + gyro.z * GYRO_SENS_RADPS_500, + -gyro.x * GYRO_SENS_RADPS_500}; + // Acceleration axis convention is unchanged from physical IMU axis convention. + float accel_arr[3] = {accel.x * ACCEL_SENS_2G, + accel.y * ACCEL_SENS_2G, + accel.z * ACCEL_SENS_2G}; + update_rotation_state(&rotation_state, gyro_arr, accel_arr, 1.0f / CFG_TICK_FREQUENCY); + + // Output calculation. + // physically, X points to the right of the controller + float x = (atan2f(rotation_state.ux,rotation_state.uz)) / M_PI; + float y = -(asinf(rotation_state.uy)) / M_PI; + float z = rotation_state.phi / M_PI; + + x = constrain(x * 1.1, -1, 1); // Additional saturation. + x = ramp(x, self->absolute_x_min/180, self->absolute_x_max/180); // Adjust range. + y = ramp(y, self->absolute_y_min/180, self->absolute_y_max/180); // Adjust range. + // Output mapping. + if (x >= 0) gyro_absolute_output( x, self->actions_x_pos, &(self->pressed_x_pos)); + else gyro_absolute_output(-x, self->actions_x_neg, &(self->pressed_x_neg)); + if (y >= 0) gyro_absolute_output( y, self->actions_y_pos, &(self->pressed_y_pos)); + else gyro_absolute_output(-y, self->actions_y_neg, &(self->pressed_y_neg)); +} + + void Gyro__report_incremental(Gyro *self) { static double sub_x = 0; static double sub_y = 0; @@ -253,7 +285,7 @@ Gyro Gyro_ ( gyro.is_engaged = Gyro__is_engaged; gyro.report = Gyro__report; gyro.report_incremental = Gyro__report_incremental; - gyro.report_absolute = Gyro__report_absolute; + gyro.report_absolute = Gyro__report_absolute_fast; gyro.reset = Gyro__reset; gyro.config_x = Gyro__config_x; gyro.config_y = Gyro__config_y; From 429f5d18b0f54ac803ee7314262f1a1d82deac66 Mon Sep 17 00:00:00 2001 From: GarryBGoode Date: Sun, 8 Jun 2025 18:30:58 +0200 Subject: [PATCH 03/16] Fixed IMU not ramping between 2 sources for z axis (roll) motion. Changed gyro report absolue to use actual elapsed microseconds in hopes of better accuracy. --- src/gyro.c | 6 +++++- src/imu.c | 2 +- 2 files changed, 6 insertions(+), 2 deletions(-) diff --git a/src/gyro.c b/src/gyro.c index bf9023db..53bc2fb5 100644 --- a/src/gyro.c +++ b/src/gyro.c @@ -152,6 +152,10 @@ void Gyro__report_absolute(Gyro *self) { } void Gyro__report_absolute_fast(Gyro *self){ + static uint32_t time_us_lock_prev = 0; + uint32_t time_us = time_us_32(); + uint32_t dt_us = time_us - time_us_lock_prev; // Safe for overflow with unsigned arithmetic + time_us_lock_prev = time_us; Vector gyro = imu_read_gyro(); Vector accel = imu_read_accel(); // gyro axis convention is different from physical IMU axis convention. @@ -162,7 +166,7 @@ void Gyro__report_absolute_fast(Gyro *self){ float accel_arr[3] = {accel.x * ACCEL_SENS_2G, accel.y * ACCEL_SENS_2G, accel.z * ACCEL_SENS_2G}; - update_rotation_state(&rotation_state, gyro_arr, accel_arr, 1.0f / CFG_TICK_FREQUENCY); + update_rotation_state(&rotation_state, gyro_arr, accel_arr, dt_us / 1000000.0f); // Output calculation. // physically, X points to the right of the controller diff --git a/src/imu.c b/src/imu.c index 4769af00..df362a68 100644 --- a/src/imu.c +++ b/src/imu.c @@ -146,7 +146,7 @@ Vector imu_read_gyro_burst(uint8_t cs, uint8_t samples) { Vector imu_read_gyro() { Vector gyro0 = imu_read_gyro_bits(IMU0); Vector gyro1 = imu_read_gyro_bits(IMU1); - double weight = max(abs(gyro1.x), abs(gyro1.y)) / 32768.0; + double weight = max(max(abs(gyro1.x), abs(gyro1.y)), abs(gyro1.z)) / 32768.0; double weight_0 = ramp_mid(weight, 0.2); double weight_1 = 1 - weight_0; double x = (gyro0.x * weight_0) + (gyro1.x * weight_1 / 4); From fe28cdec8918fd8fbbef5c69b7fb1a0369baf84e Mon Sep 17 00:00:00 2001 From: GarryBGoode Date: Tue, 10 Jun 2025 17:29:12 +0200 Subject: [PATCH 04/16] Switched doubles to floats. Added calibration routine for std. deviations of accelearation and gyro. Std deviation (or variance) is used in IMU processing to make optimal balance between 2 gyros. --- src/config.c | 52 ++++++ src/gyro.c | 44 +++--- src/headers/config.h | 42 +++-- src/headers/gyro.h | 18 +-- src/headers/imu.h | 13 ++ src/headers/vector.h | 6 +- src/imu.c | 369 +++++++++++++++++++++++++++++++------------ 7 files changed, 393 insertions(+), 151 deletions(-) diff --git a/src/config.c b/src/config.c index f591ecdf..0319865e 100644 --- a/src/config.c +++ b/src/config.c @@ -152,6 +152,18 @@ void config_write_init() { .offset_accel_1_x = 0, .offset_accel_1_y = 0, .offset_accel_1_z = 0, + .stddev_gyro_0_x = 1.0, + .stddev_gyro_0_y = 1.0, + .stddev_gyro_0_z = 1.0, + .stddev_gyro_1_x = 1.0, + .stddev_gyro_1_y = 1.0, + .stddev_gyro_1_z = 1.0, + .stddev_accel_0_x = 1.0, + .stddev_accel_0_y = 1.0, + .stddev_accel_0_z = 1.0, + .stddev_accel_1_x = 1.0, + .stddev_accel_1_y = 1.0, + .stddev_accel_1_z = 1.0, .log_level = 0, .log_mask = 0, .long_calibration = 0, @@ -239,6 +251,26 @@ void config_print() { config_cache.offset_accel_1_y, config_cache.offset_accel_1_z ); + info(" stddev_gyro_0 x=%.2f y=%.2f z=%.2f\n", + config_cache.stddev_gyro_0_x, + config_cache.stddev_gyro_0_y, + config_cache.stddev_gyro_0_z + ); + info(" stddev_gyro_1 x=%.2f y=%.2f z=%.2f\n", + config_cache.stddev_gyro_1_x, + config_cache.stddev_gyro_1_y, + config_cache.stddev_gyro_1_z + ); + info(" stddev_accel_0 x=%.2f y=%.2f z=%.2f\n", + config_cache.stddev_accel_0_x, + config_cache.stddev_accel_0_y, + config_cache.stddev_accel_0_z + ); + info(" stddev_accel_1 x=%.2f y=%.2f z=%.2f\n", + config_cache.stddev_accel_1_x, + config_cache.stddev_accel_1_y, + config_cache.stddev_accel_1_z + ); } void config_print_minimal() { @@ -286,6 +318,26 @@ void config_set_accel_offset(double ax, double ay, double az, double bx, double config_cache_synced = false; } +void config_set_gyro_stddev(double ax, double ay, double az, double bx, double by, double bz) { + config_cache.stddev_gyro_0_x = ax, + config_cache.stddev_gyro_0_y = ay, + config_cache.stddev_gyro_0_z = az, + config_cache.stddev_gyro_1_x = bx, + config_cache.stddev_gyro_1_y = by, + config_cache.stddev_gyro_1_z = bz, + config_cache_synced = false; +} + +void config_set_accel_stddev(double ax, double ay, double az, double bx, double by, double bz) { + config_cache.stddev_accel_0_x = ax, + config_cache.stddev_accel_0_y = ay, + config_cache.stddev_accel_0_z = az, + config_cache.stddev_accel_1_x = bx, + config_cache.stddev_accel_1_y = by, + config_cache.stddev_accel_1_z = bz, + config_cache_synced = false; +} + void config_tune_update_leds() { if (config_tune_mode == PROC_TUNE_OS) { led_static_mask(LED_UP); diff --git a/src/gyro.c b/src/gyro.c index 53bc2fb5..86c0d630 100644 --- a/src/gyro.c +++ b/src/gyro.c @@ -16,7 +16,7 @@ #include "vector.h" #include "rotation_fast.h" -double sensitivity_multiplier; +float sensitivity_multiplier; uint8_t world_init = 0; Vector world_top; @@ -87,7 +87,7 @@ void gyro_absolute_output(float value, uint8_t *actions, bool *pressed) { } } -void gyro_incremental_output(double value, uint8_t *actions) { +void gyro_incremental_output(float value, uint8_t *actions) { for(uint8_t i=0; i<4; i++) { uint8_t action = actions[i]; if (action == MOUSE_X) hid_mouse_move(value, 0); @@ -97,9 +97,9 @@ void gyro_incremental_output(double value, uint8_t *actions) { } } -double hssnf(double t, double k, double x) { - double a = x - (x * k); - double b = 1 - (x * k * (1/t)); +float hssnf(float t, float k, float x) { + float a = x - (x * k); + float b = 1 - (x * k * (1/t)); return a / b; } @@ -186,17 +186,17 @@ void Gyro__report_absolute_fast(Gyro *self){ void Gyro__report_incremental(Gyro *self) { - static double sub_x = 0; - static double sub_y = 0; - static double sub_z = 0; + static float sub_x = 0; + static float sub_y = 0; + static float sub_z = 0; // Read gyro values. Vector imu_gyro = imu_read_gyro(); - double x = imu_gyro.x * CFG_GYRO_SENSITIVITY_X * sensitivity_multiplier; - double y = imu_gyro.y * CFG_GYRO_SENSITIVITY_Y * sensitivity_multiplier; - double z = imu_gyro.z * CFG_GYRO_SENSITIVITY_Z * sensitivity_multiplier; + float x = imu_gyro.x * CFG_GYRO_SENSITIVITY_X * sensitivity_multiplier; + float y = imu_gyro.y * CFG_GYRO_SENSITIVITY_Y * sensitivity_multiplier; + float z = imu_gyro.z * CFG_GYRO_SENSITIVITY_Z * sensitivity_multiplier; // Additional processing. - double t = CFG_IMU_DEADZONE; - double k = CFG_IMU_DEADZONE_STRENGTH; + float t = CFG_IMU_DEADZONE; + float k = CFG_IMU_DEADZONE_STRENGTH; if (x > 0 && x < t) x = hssnf(t, k, x); else if (x < 0 && x > -t) x = -hssnf(t, k, -x); if (y > 0 && y < t) y = hssnf(t, k, y); @@ -205,18 +205,18 @@ void Gyro__report_incremental(Gyro *self) { else if (z < 0 && z > -t) z = -hssnf(t, k, -z); // compensate tick frequency. - x *= (double)REFERENCE_TICK_FREQUENCY/(double)CFG_TICK_FREQUENCY; - y *= (double)REFERENCE_TICK_FREQUENCY/(double)CFG_TICK_FREQUENCY; - z *= (double)REFERENCE_TICK_FREQUENCY/(double)CFG_TICK_FREQUENCY; + x *= (float)REFERENCE_TICK_FREQUENCY/(float)CFG_TICK_FREQUENCY; + y *= (float)REFERENCE_TICK_FREQUENCY/(float)CFG_TICK_FREQUENCY; + z *= (float)REFERENCE_TICK_FREQUENCY/(float)CFG_TICK_FREQUENCY; // Reintroduce subpixel leftovers. x += sub_x; y += sub_y; z += sub_z; // Round down and save leftovers. - sub_x = modf(x, &x); - sub_y = modf(y, &y); - sub_z = modf(z, &z); + sub_x = modff(x, &x); + sub_y = modff(y, &y); + sub_z = modff(z, &z); // Report. if (x >= 0) gyro_incremental_output( x, self->actions_x_pos); else gyro_incremental_output(-x, self->actions_x_neg); @@ -260,21 +260,21 @@ void Gyro__reset(Gyro *self) { self->pressed_z_neg = false; } -void Gyro__config_x(Gyro *self, double min, double max, Actions neg, Actions pos) { +void Gyro__config_x(Gyro *self, float min, float max, Actions neg, Actions pos) { self->absolute_x_min = min; self->absolute_x_max = max; memcpy(self->actions_x_neg, neg, ACTIONS_LEN); memcpy(self->actions_x_pos, pos, ACTIONS_LEN); } -void Gyro__config_y(Gyro *self, double min, double max, Actions neg, Actions pos) { +void Gyro__config_y(Gyro *self, float min, float max, Actions neg, Actions pos) { self->absolute_y_min = min; self->absolute_y_max = max; memcpy(self->actions_y_neg, neg, ACTIONS_LEN); memcpy(self->actions_y_pos, pos, ACTIONS_LEN); } -void Gyro__config_z(Gyro *self, double min, double max, Actions neg, Actions pos) { +void Gyro__config_z(Gyro *self, float min, float max, Actions neg, Actions pos) { self->absolute_z_min = min; self->absolute_z_max = max; memcpy(self->actions_z_neg, neg, ACTIONS_LEN); diff --git a/src/headers/config.h b/src/headers/config.h index 5400bd0f..e027e7d0 100644 --- a/src/headers/config.h +++ b/src/headers/config.h @@ -45,13 +45,13 @@ #define NVM_SYNC_FREQUENCY (CFG_TICK_FREQUENCY / 2) #define CFG_CALIBRATION_SAMPLES_THUMBSTICK 100000 // Samples. -#define CFG_CALIBRATION_SAMPLES_GYRO 500000 // Samples. -#define CFG_CALIBRATION_SAMPLES_ACCEL 100000 // Samples. +#define CFG_CALIBRATION_SAMPLES_GYRO 50000 // Samples. +#define CFG_CALIBRATION_SAMPLES_ACCEL 10000 // Samples. #define CFG_CALIBRATION_LONG_FACTOR 4 #define CFG_CALIBRATION_PROGRESS_BAR 40 #define CFG_GYRO_SENSITIVITY (pow(2, -9) * 1.45) -#define CFG_GYRO_SENSITIVITY_X (CFG_GYRO_SENSITIVITY * 1) +#define CFG_GYRO_SENSITIVITY_X (CFG_GYRO_SENSITIVITY * 2) #define CFG_GYRO_SENSITIVITY_Y (CFG_GYRO_SENSITIVITY * 1) #define CFG_GYRO_SENSITIVITY_Z (CFG_GYRO_SENSITIVITY * 1) @@ -83,18 +83,30 @@ typedef struct __packed _Config { float offset_ts_ly; float offset_ts_rx; float offset_ts_ry; - double offset_gyro_0_x; - double offset_gyro_0_y; - double offset_gyro_0_z; - double offset_gyro_1_x; - double offset_gyro_1_y; - double offset_gyro_1_z; - double offset_accel_0_x; - double offset_accel_0_y; - double offset_accel_0_z; - double offset_accel_1_x; - double offset_accel_1_y; - double offset_accel_1_z; + float offset_gyro_0_x; + float offset_gyro_0_y; + float offset_gyro_0_z; + float offset_gyro_1_x; + float offset_gyro_1_y; + float offset_gyro_1_z; + float offset_accel_0_x; + float offset_accel_0_y; + float offset_accel_0_z; + float offset_accel_1_x; + float offset_accel_1_y; + float offset_accel_1_z; + float stddev_gyro_0_x; + float stddev_gyro_0_y; + float stddev_gyro_0_z; + float stddev_gyro_1_x; + float stddev_gyro_1_y; + float stddev_gyro_1_z; + float stddev_accel_0_x; + float stddev_accel_0_y; + float stddev_accel_0_z; + float stddev_accel_1_x; + float stddev_accel_1_y; + float stddev_accel_1_z; int8_t offset_gyro_user_x; int8_t offset_gyro_user_y; int8_t offset_gyro_user_z; diff --git a/src/headers/gyro.h b/src/headers/gyro.h index 43fc0fef..0c1e0c0f 100644 --- a/src/headers/gyro.h +++ b/src/headers/gyro.h @@ -18,18 +18,18 @@ struct Gyro_struct { void (*report_incremental) (Gyro *self); void (*report_absolute) (Gyro *self); void (*reset) (Gyro *self); - void (*config_x) (Gyro *self, double min, double max, Actions neg, Actions pos); - void (*config_y) (Gyro *self, double min, double max, Actions neg, Actions pos); - void (*config_z) (Gyro *self, double min, double max, Actions neg, Actions pos); + void (*config_x) (Gyro *self, float min, float max, Actions neg, Actions pos); + void (*config_y) (Gyro *self, float min, float max, Actions neg, Actions pos); + void (*config_z) (Gyro *self, float min, float max, Actions neg, Actions pos); GyroMode mode; uint8_t engage; Button engage_button; - double absolute_x_min; - double absolute_y_min; - double absolute_z_min; - double absolute_x_max; - double absolute_y_max; - double absolute_z_max; + float absolute_x_min; + float absolute_y_min; + float absolute_z_min; + float absolute_x_max; + float absolute_y_max; + float absolute_z_max; bool pressed_x_pos; bool pressed_y_pos; bool pressed_z_pos; diff --git a/src/headers/imu.h b/src/headers/imu.h index 6622787f..cd4e9a8a 100644 --- a/src/headers/imu.h +++ b/src/headers/imu.h @@ -25,6 +25,9 @@ #define IMU_CTRL1_XL_2G 0b10100010 // Accelerometer value for 2G range, LPF2 enabled #define IMU_CTRL1_XL_2G_NOFILT 0b10100000 // Accelerometer value for 2G range, LPF2 disabled +#define IMU_CTRL3_C_BOOT 0b00000001 // Reboot the IMU. +#define IMU_CTRL3_C_BDU 0b01000000 // Block data update, prevents reading inconsistent data. + /* XL = 100b that is ODR/100 bandwidth, 66Hz for 6.6kHz ODR */ #define IMU_CTRL8_XL_LP 0b10000000 // Accelerometer value for low pass filter. #define IMU_CTRL2_G_OFF 0b00000000 // Gyroscope value power off. @@ -97,3 +100,13 @@ Vector imu_read_accel(); void imu_load_calibration(); void imu_calibrate(); +typedef struct calib_data_struct { + float offset; + float variance; + float stddev; +} CalibData; + +typedef struct imu_calib_struct { + CalibData gyro[3]; + CalibData accel[3]; +} ImuCalib; \ No newline at end of file diff --git a/src/headers/vector.h b/src/headers/vector.h index 1bb36c55..59d1e78f 100644 --- a/src/headers/vector.h +++ b/src/headers/vector.h @@ -4,9 +4,9 @@ #pragma once typedef struct vector_struct { - double x; - double y; - double z; + float x; + float y; + float z; } Vector; typedef struct vector4_struct { diff --git a/src/imu.c b/src/imu.c index df362a68..873b4344 100644 --- a/src/imu.c +++ b/src/imu.c @@ -21,18 +21,44 @@ uint8_t IMU0 = 0; uint8_t IMU1 = 0; -double offset_gyro_0_x; -double offset_gyro_0_y; -double offset_gyro_0_z; -double offset_gyro_1_x; -double offset_gyro_1_y; -double offset_gyro_1_z; -double offset_accel_0_x; -double offset_accel_0_y; -double offset_accel_0_z; -double offset_accel_1_x; -double offset_accel_1_y; -double offset_accel_1_z; +float offset_gyro_0_x; +float offset_gyro_0_y; +float offset_gyro_0_z; +float offset_gyro_1_x; +float offset_gyro_1_y; +float offset_gyro_1_z; +float offset_accel_0_x; +float offset_accel_0_y; +float offset_accel_0_z; +float offset_accel_1_x; +float offset_accel_1_y; +float offset_accel_1_z; + +ImuCalib imu_calib0 = { + .gyro = { + {0, 0, 0}, // Gyro offset + {1, 1, 1}, // Gyro variance + {1, 1, 1} // Gyro stddev + }, + .accel = { + {0, 0, 0}, // Accel offset + {1, 1, 1}, // Accel variance + {1, 1, 1} // Accel stddev + } +}; + +ImuCalib imu_calib1 = { + .gyro = { + {0, 0, 0}, // Gyro offset + {1, 1, 1}, // Gyro variance + {1, 1, 1} // Gyro stddev + }, + .accel = { + {0, 0, 0}, // Accel offset + {1, 1, 1}, // Accel variance + {1, 1, 1} // Accel stddev + } +}; void imu_channel_select() { Config *config = config_read(); @@ -41,11 +67,20 @@ void imu_channel_select() { } void imu_init_single(uint8_t cs, uint8_t gyro_conf) { + // make sure Accel is ON + bus_spi_write(cs, IMU_CTRL1_XL, IMU_CTRL1_XL_2G); + // sleep_ms(10); + // Reboot the IMU. Accel must be ON before rebooting. + bus_spi_write(cs, IMU_CTRL3_C, IMU_CTRL3_C_BOOT); + // Datasheet lists typical turn on time as 35ms, wait 2x that much. + sleep_ms(120); + // Read WHO_AM_I register to check if the IMU is present. uint8_t id = bus_spi_read_one(cs, IMU_READ | IMU_WHO_AM_I); // Accel options. bus_spi_write(cs, IMU_CTRL1_XL, IMU_CTRL1_XL_2G); bus_spi_write(cs, IMU_CTRL8_XL, IMU_CTRL8_XL_LP); // Gyro options. + // bus_spi_write(cs, IMU_CTRL3_C, IMU_CTRL3_C_BDU); // Block data update bus_spi_write(cs, IMU_CTRL4_C, GYRO_LPF1_ENABLE_CTRL4_C); // Enable LPF1 bus_spi_write(cs, IMU_CTRL6_C, GYRO_LPF1_470HZ_CTRL6_C); // FTYPE selection for LPF1 bus_spi_write(cs, IMU_CTRL2_G, gyro_conf); @@ -85,20 +120,20 @@ Vector imu_read_gyro_bits(uint8_t cs) { int16_t y = (int16_t)(((uint16_t)buf[1] << 8) | (uint16_t)buf[0]); int16_t z = (int16_t)(((uint16_t)buf[3] << 8) | (uint16_t)buf[2]); int16_t x = -(int16_t)(((uint16_t)buf[5] << 8) | (uint16_t)buf[4]); - double offset_x = (cs==PIN_SPI_CS0) ? offset_gyro_0_x : offset_gyro_1_x; - double offset_y = (cs==PIN_SPI_CS0) ? offset_gyro_0_y : offset_gyro_1_y; - double offset_z = (cs==PIN_SPI_CS0) ? offset_gyro_0_z : offset_gyro_1_z; + float offset_x = (cs==PIN_SPI_CS0) ? imu_calib0.gyro[0].offset : imu_calib1.gyro[0].offset; + float offset_y = (cs==PIN_SPI_CS0) ? imu_calib0.gyro[1].offset : imu_calib1.gyro[1].offset; + float offset_z = (cs==PIN_SPI_CS0) ? imu_calib0.gyro[2].offset : imu_calib1.gyro[2].offset; #ifdef DEVICE_ALPAKKA_V0 return (Vector){ - (double)x - offset_x, - (double)y - offset_y, - (double)z - offset_z, + (float)x - offset_x, + (float)y - offset_y, + (float)z - offset_z, }; #else /* DEVICE_ALPAKKA_V1 */ return (Vector){ - (double)x - offset_x, - -(double)y - offset_y, - -(double)z - offset_z, + (float)x - offset_x, + -(float)y - offset_y, + -(float)z - offset_z, }; #endif } @@ -109,28 +144,28 @@ Vector imu_read_accel_bits(uint8_t cs) { int16_t x = (int16_t)(((uint16_t)buf[1] << 8) + (uint16_t)buf[0]); int16_t y = (int16_t)(((uint16_t)buf[3] << 8) + (uint16_t)buf[2]); int16_t z = (int16_t)(((uint16_t)buf[5] << 8) + (uint16_t)buf[4]); - double offset_x = (cs==PIN_SPI_CS0) ? offset_accel_0_x : offset_accel_1_x; - double offset_y = (cs==PIN_SPI_CS0) ? offset_accel_0_y : offset_accel_1_y; - double offset_z = (cs==PIN_SPI_CS0) ? offset_accel_0_z : offset_accel_1_z; + float offset_x = (cs==PIN_SPI_CS0) ? offset_accel_0_x : offset_accel_1_x; + float offset_y = (cs==PIN_SPI_CS0) ? offset_accel_0_y : offset_accel_1_y; + float offset_z = (cs==PIN_SPI_CS0) ? offset_accel_0_z : offset_accel_1_z; #ifdef DEVICE_ALPAKKA_V0 return (Vector){ - (double)x - offset_x, - (double)y - offset_y, - (double)z - offset_z, + (float)x - offset_x, + (float)y - offset_y, + (float)z - offset_z, }; #else /* DEVICE_ALPAKKA_V1 */ return (Vector){ - -(double)x - offset_x, - -(double)y - offset_y, - (double)z - offset_z, + -(float)x - offset_x, + -(float)y - offset_y, + (float)z - offset_z, }; #endif } Vector imu_read_gyro_burst(uint8_t cs, uint8_t samples) { - double x = 0; - double y = 0; - double z = 0; + float x = 0; + float y = 0; + float z = 0; for(uint8_t i=0; i0.20*32768 ? weight_0.x : 1; + weight_0.y = absf(gyro0.y)>0.20*32768 ? weight_0.y : 1; + weight_0.z = absf(gyro0.z)>0.20*32768 ? weight_0.z : 1; + float x = (gyro0.x * weight_0.x) + (gyro1.x * (1.0-weight_0.x) / 4); + float y = (gyro0.y * weight_0.y) + (gyro1.y * (1.0-weight_0.y) / 4); + float z = (gyro0.z * weight_0.z) + (gyro1.z * (1.0-weight_0.z) / 4); return (Vector){x, y, z}; } @@ -165,97 +217,210 @@ Vector imu_read_accel() { }; } -void imu_calibrate_single(uint8_t cs, bool mode, double* x, double* y, double* z) { - char *mode_str = mode ? "accel" : "gyro"; +void imu_calibrate_single(uint8_t cs, ImuCalib *calib) { + char *mode_str = "both"; info("IMU: cs=%i calibrating %s...\n", cs, mode_str); - double sum_x = 0; - double sum_y = 0; - double sum_z = 0; + float mean_x_accel = 0; + float mean_x_gyro = 0; + float mean_y_accel = 0; + float mean_y_gyro = 0; + float mean_z_accel = 0; + float mean_z_gyro = 0; + float var_x_accel = 0; + float var_x_gyro = 0; + float var_y_accel = 0; + float var_y_gyro = 0; + float var_z_accel = 0; + float var_z_gyro = 0; // Determine number of samples. uint32_t nsamples; - if (mode==1) { - nsamples = CFG_CALIBRATION_SAMPLES_ACCEL; - } else { - nsamples = CFG_CALIBRATION_SAMPLES_GYRO; - Config *config = config_read(); - if (config->long_calibration) { - nsamples *= CFG_CALIBRATION_LONG_FACTOR; - } + nsamples = CFG_CALIBRATION_SAMPLES_GYRO; + Config *config = config_read(); + if (config->long_calibration) { + nsamples *= CFG_CALIBRATION_LONG_FACTOR; + } // Sampling. uint32_t i = 0; info("| 0%%%*s100%% |\n", CFG_CALIBRATION_PROGRESS_BAR - 10, ""); while(i < nsamples) { - Vector sample = mode ? imu_read_accel_bits(cs) : imu_read_gyro_bits(cs); - sum_x += sample.x; - sum_y += sample.y; - sum_z += sample.z; + Vector sample_accel = imu_read_accel_bits(cs); + Vector sample_gyro = imu_read_gyro_bits(cs); + + // using cumulative average algorithm + // the reason is that this way variance can use the actual value of the average + + mean_x_accel = (sample_accel.x + mean_x_accel*i)/(i+1); + mean_y_accel = (sample_accel.y + mean_y_accel*i)/(i+1); + mean_z_accel = (sample_accel.z + mean_z_accel*i)/(i+1); + var_x_accel = ((sample_accel.x - mean_x_accel) * (sample_accel.x - mean_x_accel) + var_x_accel * i) / (i + 1); + var_y_accel = ((sample_accel.y - mean_y_accel) * (sample_accel.y - mean_y_accel) + var_y_accel * i) / (i + 1); + var_z_accel = ((sample_accel.z - mean_z_accel) * (sample_accel.z - mean_z_accel) + var_z_accel * i) / (i + 1); + + mean_x_gyro = (sample_gyro.x + mean_x_gyro*i)/(i+1); + mean_y_gyro = (sample_gyro.y + mean_y_gyro*i)/(i+1); + mean_z_gyro = (sample_gyro.z + mean_z_gyro*i)/(i+1); + var_x_gyro = ((sample_gyro.x - mean_x_gyro) * (sample_gyro.x - mean_x_gyro) + var_x_gyro * i) / (i + 1); + var_y_gyro = ((sample_gyro.y - mean_y_gyro) * (sample_gyro.y - mean_y_gyro) + var_y_gyro * i) / (i + 1); + var_z_gyro = ((sample_gyro.z - mean_z_gyro) * (sample_gyro.z - mean_z_gyro) + var_z_gyro * i) / (i + 1); + i++; + sleep_us(1E6 / 6660); // 6660Hz sampling rate of IMU. if (!(i % (nsamples / CFG_CALIBRATION_PROGRESS_BAR))) info("="); } - // Average. - *x = sum_x / nsamples; - *y = sum_y / nsamples; - *z = sum_z / nsamples; + calib->accel[0].offset = mean_x_accel; + calib->accel[1].offset = mean_y_accel; + calib->accel[2].offset = mean_z_accel; + calib-> gyro[0].offset = mean_x_gyro; + calib-> gyro[1].offset = mean_y_gyro; + calib-> gyro[2].offset = mean_z_gyro; + + calib->accel[0].variance = var_x_accel; + calib->accel[1].variance = var_y_accel; + calib->accel[2].variance = var_z_accel; + calib-> gyro[0].variance = var_x_gyro; + calib-> gyro[1].variance = var_y_gyro; + calib-> gyro[2].variance = var_z_gyro; + + calib->accel[0].stddev = sqrtf(var_x_accel); + calib->accel[1].stddev = sqrtf(var_y_accel); + calib->accel[2].stddev = sqrtf(var_z_accel); + calib-> gyro[0].stddev = sqrtf(var_x_gyro); + calib-> gyro[1].stddev = sqrtf(var_y_gyro); + calib-> gyro[2].stddev = sqrtf(var_z_gyro); + + + // Assuming the resting state of the controller is having a vector of 1G // pointing down. (Newton's fault for inventing the gravity /jk). - if (mode==1) *z -= BIT_14; - info("\nIMU: cs=%i %s calibrated x=%.02f y=%.02f z=%.02f\n", cs, mode_str, *x, *y, *z); + + info("\nIMU: cs=%i %s calibrated \n x_acc=%.02f y_acc=%.02f z_acc=%.02f\n", cs, mode_str, + calib->accel[0].offset, calib->accel[1].offset, calib->accel[2].offset); + info(" x_gyro=%.02f y_gyro=%.02f z_gyro=%.02f\n", + calib->gyro[0].offset, calib->gyro[1].offset, calib->gyro[2].offset); + info(" x_acc_var=%.02f y_acc_var=%.02f z_acc_var=%.02f\n", + calib->accel[0].variance, calib->accel[1].variance, calib->accel[2].variance); + info(" x_gyro_var=%.02f y_gyro_var=%.02f z_gyro_var=%.02f\n", + calib->gyro[0].variance, calib->gyro[1].variance, calib->gyro[2].variance); } void imu_load_calibration() { Config *config = config_read(); - offset_gyro_0_x = config->offset_gyro_0_x - (config->offset_gyro_user_x * GYRO_USER_OFFSET_FACTOR); - offset_gyro_0_y = config->offset_gyro_0_y - (config->offset_gyro_user_y * GYRO_USER_OFFSET_FACTOR); - offset_gyro_0_z = config->offset_gyro_0_z - (config->offset_gyro_user_z * GYRO_USER_OFFSET_FACTOR); - offset_gyro_1_x = config->offset_gyro_1_x - (config->offset_gyro_user_x * GYRO_USER_OFFSET_FACTOR); - offset_gyro_1_y = config->offset_gyro_1_y - (config->offset_gyro_user_y * GYRO_USER_OFFSET_FACTOR); - offset_gyro_1_z = config->offset_gyro_1_z - (config->offset_gyro_user_z * GYRO_USER_OFFSET_FACTOR); - offset_accel_0_x = config->offset_accel_0_x; - offset_accel_0_y = config->offset_accel_0_y; - offset_accel_0_z = config->offset_accel_0_z; - offset_accel_1_x = config->offset_accel_1_x; - offset_accel_1_y = config->offset_accel_1_y; - offset_accel_1_z = config->offset_accel_1_z; + imu_calib0.gyro[0].offset = config->offset_gyro_0_x - (config->offset_gyro_user_x * GYRO_USER_OFFSET_FACTOR); + imu_calib0.gyro[1].offset = config->offset_gyro_0_y - (config->offset_gyro_user_y * GYRO_USER_OFFSET_FACTOR); + imu_calib0.gyro[2].offset = config->offset_gyro_0_z - (config->offset_gyro_user_z * GYRO_USER_OFFSET_FACTOR); + imu_calib1.gyro[0].offset = config->offset_gyro_1_x - (config->offset_gyro_user_x * GYRO_USER_OFFSET_FACTOR); + imu_calib1.gyro[1].offset = config->offset_gyro_1_y - (config->offset_gyro_user_y * GYRO_USER_OFFSET_FACTOR); + imu_calib1.gyro[2].offset = config->offset_gyro_1_z - (config->offset_gyro_user_z * GYRO_USER_OFFSET_FACTOR); + imu_calib0.accel[0].offset = config->offset_accel_0_x; + imu_calib0.accel[1].offset = config->offset_accel_0_y; + imu_calib0.accel[2].offset = config->offset_accel_0_z; + imu_calib1.accel[0].offset = config->offset_accel_1_x; + imu_calib1.accel[1].offset = config->offset_accel_1_y; + imu_calib1.accel[2].offset = config->offset_accel_1_z; + imu_calib0.gyro[0].variance = config->stddev_gyro_0_x * config->stddev_gyro_0_x; + imu_calib0.gyro[1].variance = config->stddev_gyro_0_y * config->stddev_gyro_0_y; + imu_calib0.gyro[2].variance = config->stddev_gyro_0_z * config->stddev_gyro_0_z; + imu_calib1.gyro[0].variance = config->stddev_gyro_1_x * config->stddev_gyro_1_x; + imu_calib1.gyro[1].variance = config->stddev_gyro_1_y * config->stddev_gyro_1_y; + imu_calib1.gyro[2].variance = config->stddev_gyro_1_z * config->stddev_gyro_1_z; + imu_calib0.accel[0].variance = config->stddev_accel_0_x * config->stddev_accel_0_x; + imu_calib0.accel[1].variance = config->stddev_accel_0_y * config->stddev_accel_0_y; + imu_calib0.accel[2].variance = config->stddev_accel_0_z * config->stddev_accel_0_z; + imu_calib1.accel[0].variance = config->stddev_accel_1_x * config->stddev_accel_1_x; + imu_calib1.accel[1].variance = config->stddev_accel_1_y * config->stddev_accel_1_y; + imu_calib1.accel[2].variance = config->stddev_accel_1_z * config->stddev_accel_1_z; + imu_calib0.gyro[0].stddev = config->stddev_gyro_0_x; + imu_calib0.gyro[1].stddev = config->stddev_gyro_0_y; + imu_calib0.gyro[2].stddev = config->stddev_gyro_0_z; + imu_calib1.gyro[0].stddev = config->stddev_gyro_1_x; + imu_calib1.gyro[1].stddev = config->stddev_gyro_1_y; + imu_calib1.gyro[2].stddev = config->stddev_gyro_1_z; + imu_calib0.accel[0].stddev = config->stddev_accel_0_x; + imu_calib0.accel[1].stddev = config->stddev_accel_0_y; + imu_calib0.accel[2].stddev = config->stddev_accel_0_z; + imu_calib1.accel[0].stddev = config->stddev_accel_1_x; + imu_calib1.accel[1].stddev = config->stddev_accel_1_y; + imu_calib1.accel[2].stddev = config->stddev_accel_1_z; } void imu_reset_calibration() { - offset_gyro_0_x = 0; - offset_gyro_0_y = 0; - offset_gyro_0_z = 0; - offset_gyro_1_x = 0; - offset_gyro_1_y = 0; - offset_gyro_1_z = 0; - offset_accel_0_x = 0; - offset_accel_0_y = 0; - offset_accel_0_z = 0; - offset_accel_1_x = 0; - offset_accel_1_y = 0; - offset_accel_1_z = 0; + imu_calib0.gyro[0].offset = 0; + imu_calib0.gyro[1].offset = 0; + imu_calib0.gyro[2].offset = 0; + imu_calib1.gyro[0].offset = 0; + imu_calib1.gyro[1].offset = 0; + imu_calib1.gyro[2].offset = 0; + imu_calib0.accel[0].offset = 0; + imu_calib0.accel[1].offset = 0; + imu_calib0.accel[2].offset = 0; + imu_calib1.accel[0].offset = 0; + imu_calib1.accel[1].offset = 0; + imu_calib1.accel[2].offset = 0; + + imu_calib0.gyro[0].variance = 1; + imu_calib0.gyro[1].variance = 1; + imu_calib0.gyro[2].variance = 1; + imu_calib0.gyro[0].stddev = 1; + imu_calib0.gyro[1].stddev = 1; + imu_calib0.gyro[2].stddev = 1; + imu_calib1.gyro[0].variance = 1; + imu_calib1.gyro[1].variance = 1; + imu_calib1.gyro[2].variance = 1; + imu_calib1.gyro[0].stddev = 1; + imu_calib1.gyro[1].stddev = 1; + imu_calib1.gyro[2].stddev = 1; + imu_calib0.accel[0].variance = 1; + imu_calib0.accel[1].variance = 1; + imu_calib0.accel[2].variance = 1; + imu_calib1.accel[0].variance = 1; + imu_calib1.accel[1].variance = 1; + imu_calib1.accel[2].variance = 1; + imu_calib0.accel[0].stddev = 1; + imu_calib0.accel[1].stddev = 1; + imu_calib0.accel[2].stddev = 1; + imu_calib1.accel[0].stddev = 1; + imu_calib1.accel[1].stddev = 1; + imu_calib1.accel[2].stddev = 1; } void imu_calibrate() { config_set_gyro_user_offset(0, 0, 0); imu_reset_calibration(); - imu_calibrate_single(PIN_SPI_CS0, 0, &offset_gyro_0_x, &offset_gyro_0_y, &offset_gyro_0_z); - imu_calibrate_single(PIN_SPI_CS1, 0, &offset_gyro_1_x, &offset_gyro_1_y, &offset_gyro_1_z); - imu_calibrate_single(PIN_SPI_CS0, 1, &offset_accel_0_x, &offset_accel_0_y, &offset_accel_0_z); - imu_calibrate_single(PIN_SPI_CS1, 1, &offset_accel_1_x, &offset_accel_1_y, &offset_accel_1_z); + imu_calibrate_single(PIN_SPI_CS0, &imu_calib0); + imu_calibrate_single(PIN_SPI_CS1, &imu_calib1); + config_set_gyro_offset( - offset_gyro_0_x, - offset_gyro_0_y, - offset_gyro_0_z, - offset_gyro_1_x, - offset_gyro_1_y, - offset_gyro_1_z + imu_calib0.gyro[0].offset, + imu_calib0.gyro[1].offset, + imu_calib0.gyro[2].offset, + imu_calib1.gyro[0].offset, + imu_calib1.gyro[1].offset, + imu_calib1.gyro[2].offset ); config_set_accel_offset( - offset_accel_0_x, - offset_accel_0_y, - offset_accel_0_z, - offset_accel_1_x, - offset_accel_1_y, - offset_accel_1_z + imu_calib0.accel[0].offset, + imu_calib0.accel[1].offset, + imu_calib0.accel[2].offset, + imu_calib1.accel[0].offset, + imu_calib1.accel[1].offset, + imu_calib1.accel[2].offset + ); + config_set_gyro_stddev( + imu_calib0.gyro[0].stddev, + imu_calib0.gyro[1].stddev, + imu_calib0.gyro[2].stddev, + imu_calib1.gyro[0].stddev, + imu_calib1.gyro[1].stddev, + imu_calib1.gyro[2].stddev + ); + config_set_accel_stddev( + imu_calib0.accel[0].stddev, + imu_calib0.accel[1].stddev, + imu_calib0.accel[2].stddev, + imu_calib1.accel[0].stddev, + imu_calib1.accel[1].stddev, + imu_calib1.accel[2].stddev ); imu_load_calibration(); } From 7133f5b9f3aa41a3cda6f0837ef62721ea5bc5e6 Mon Sep 17 00:00:00 2001 From: GarryBGoode Date: Tue, 10 Jun 2025 17:36:22 +0200 Subject: [PATCH 05/16] Updated config version --- src/headers/config.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/headers/config.h b/src/headers/config.h index e027e7d0..6b877ffc 100644 --- a/src/headers/config.h +++ b/src/headers/config.h @@ -12,7 +12,7 @@ #define MINOR 1000 #define PATCH 1 -#define NVM_CONFIG_VERSION ((MAJOR * 1) + (MINOR * 1) + (PATCH * 0)) +#define NVM_CONFIG_VERSION ((MAJOR * 1) + (MINOR * 2) + (PATCH * 0)) #define NVM_CONTROL_BYTE 0b01010101 #define NVM_CONFIG_ADDR 0x001D0000 #define NVM_CONFIG_SIZE 256 From 335801f0dc7ce4a0449ad7b8508b25c74fd0893d Mon Sep 17 00:00:00 2001 From: GarryBGoode Date: Tue, 10 Jun 2025 17:46:46 +0200 Subject: [PATCH 06/16] More double to float --- src/config.c | 12 ++++++------ src/ctrl.c | 2 +- src/headers/config.h | 12 +++++++----- src/imu.c | 6 +++--- 4 files changed, 17 insertions(+), 15 deletions(-) diff --git a/src/config.c b/src/config.c index 0319865e..4f6761eb 100644 --- a/src/config.c +++ b/src/config.c @@ -298,7 +298,7 @@ void config_set_thumbstick_offset(float lx, float ly, float rx, float ry) { config_cache_synced = false; } -void config_set_gyro_offset(double ax, double ay, double az, double bx, double by, double bz) { +void config_set_gyro_offset(float ax, float ay, float az, float bx, float by, float bz) { config_cache.offset_gyro_0_x = ax, config_cache.offset_gyro_0_y = ay, config_cache.offset_gyro_0_z = az, @@ -308,7 +308,7 @@ void config_set_gyro_offset(double ax, double ay, double az, double bx, double b config_cache_synced = false; } -void config_set_accel_offset(double ax, double ay, double az, double bx, double by, double bz) { +void config_set_accel_offset(float ax, float ay, float az, float bx, float by, float bz) { config_cache.offset_accel_0_x = ax, config_cache.offset_accel_0_y = ay, config_cache.offset_accel_0_z = az, @@ -318,7 +318,7 @@ void config_set_accel_offset(double ax, double ay, double az, double bx, double config_cache_synced = false; } -void config_set_gyro_stddev(double ax, double ay, double az, double bx, double by, double bz) { +void config_set_gyro_stddev(float ax, float ay, float az, float bx, float by, float bz) { config_cache.stddev_gyro_0_x = ax, config_cache.stddev_gyro_0_y = ay, config_cache.stddev_gyro_0_z = az, @@ -328,7 +328,7 @@ void config_set_gyro_stddev(double ax, double ay, double az, double bx, double b config_cache_synced = false; } -void config_set_accel_stddev(double ax, double ay, double az, double bx, double by, double bz) { +void config_set_accel_stddev(float ax, float ay, float az, float bx, float by, float bz) { config_cache.stddev_accel_0_x = ax, config_cache.stddev_accel_0_y = ay, config_cache.stddev_accel_0_z = az, @@ -505,7 +505,7 @@ uint8_t config_get_touch_sens_value(uint8_t index) { return config_cache.sens_touch_values[index]; } -double config_get_mouse_sens_value(uint8_t index) { +float config_get_mouse_sens_value(uint8_t index) { return config_cache.sens_mouse_values[index]; } @@ -521,7 +521,7 @@ void config_set_touch_sens_values(uint8_t* values) { config_cache_synced = false; } -void config_set_mouse_sens_values(double* values) { +void config_set_mouse_sens_values(float* values) { config_cache.sens_mouse_values[0] = values[0]; config_cache.sens_mouse_values[1] = values[1]; config_cache.sens_mouse_values[2] = values[2]; diff --git a/src/ctrl.c b/src/ctrl.c index 9352068b..fbf2fd67 100644 --- a/src/ctrl.c +++ b/src/ctrl.c @@ -64,7 +64,7 @@ void ctrl_config_set(Ctrl_cfg_type key, uint8_t preset, uint8_t values[5]) { } else if (key == SENS_MOUSE) { // Scaled by 10 since the USB communication works with integers. - double values_fmt[] = { + float values_fmt[] = { values[0] / 10.0, values[1] / 10.0, values[2] / 10.0 diff --git a/src/headers/config.h b/src/headers/config.h index 6b877ffc..c1322181 100644 --- a/src/headers/config.h +++ b/src/headers/config.h @@ -76,7 +76,7 @@ typedef struct __packed _Config { int8_t sens_touch; int8_t deadzone; int8_t vibration; - double sens_mouse_values[3]; + float sens_mouse_values[3]; int8_t sens_touch_values[5]; float deadzone_values[3]; float offset_ts_lx; @@ -126,8 +126,10 @@ Config* config_read(); void config_delete(); void config_set_thumbstick_offset(float lx, float ly, float rx, float ry); -void config_set_gyro_offset(double ax, double ay, double az, double bx, double by, double bz); -void config_set_accel_offset(double ax, double ay, double az, double bx, double by, double bz); +void config_set_gyro_offset(float ax, float ay, float az, float bx, float by, float bz); +void config_set_accel_offset(float ax, float ay, float az, float bx, float by, float bz); +void config_set_gyro_stddev(float ax, float ay, float az, float bx, float by, float bz); +void config_set_accel_stddev(float ax, float ay, float az, float bx, float by, float bz); uint8_t config_get_protocol(); void config_tune_set_mode(uint8_t mode); void config_tune(bool direction); @@ -151,11 +153,11 @@ void config_set_mouse_sens_preset(uint8_t preset, bool notify_webusb); void config_set_deadzone_preset(uint8_t preset, bool notify_webusb); uint8_t config_get_touch_sens_value(uint8_t index); -double config_get_mouse_sens_value(uint8_t index); +float config_get_mouse_sens_value(uint8_t index); float config_get_deadzone_value(uint8_t index); void config_set_touch_sens_values(uint8_t* values); -void config_set_mouse_sens_values(double* values); +void config_set_mouse_sens_values(float* values); void config_set_deadzone_values(float* values); void config_set_log_level(LogLevel log_level); diff --git a/src/imu.c b/src/imu.c index 873b4344..9e723613 100644 --- a/src/imu.c +++ b/src/imu.c @@ -198,9 +198,9 @@ Vector imu_read_gyro() { // IMU 0 is on 500dps, IMU 1 is on 125dps. // when IMU 0 is on 20% of its range or higher, IMU 1 is on 80% of its range // then IMU 0 takes over 100% of the weight. - weight_0.x = absf(gyro0.x)>0.20*32768 ? weight_0.x : 1; - weight_0.y = absf(gyro0.y)>0.20*32768 ? weight_0.y : 1; - weight_0.z = absf(gyro0.z)>0.20*32768 ? weight_0.z : 1; + weight_0.x = fabsf(gyro0.x)>0.20*32768 ? weight_0.x : 1; + weight_0.y = fabsf(gyro0.y)>0.20*32768 ? weight_0.y : 1; + weight_0.z = fabsf(gyro0.z)>0.20*32768 ? weight_0.z : 1; float x = (gyro0.x * weight_0.x) + (gyro1.x * (1.0-weight_0.x) / 4); float y = (gyro0.y * weight_0.y) + (gyro1.y * (1.0-weight_0.y) / 4); float z = (gyro0.z * weight_0.z) + (gyro1.z * (1.0-weight_0.z) / 4); From e513da10eb487da8311cdf9591851658cd75829b Mon Sep 17 00:00:00 2001 From: GarryBGoode Date: Fri, 27 Jun 2025 16:02:25 +0200 Subject: [PATCH 07/16] Refined IMU switch-over logic, it's not per-axis, but vector length based now. If gyro rate is higher than 100dps as a vector, measurement switches to the 500dps range IMU. Small refinement of fast rotation. --- src/headers/imu.h | 4 +++- src/imu.c | 58 ++++++++++++++++++++++++++++++--------------- src/rotation_fast.c | 16 ++++++------- 3 files changed, 49 insertions(+), 29 deletions(-) diff --git a/src/headers/imu.h b/src/headers/imu.h index cd4e9a8a..cee6610f 100644 --- a/src/headers/imu.h +++ b/src/headers/imu.h @@ -87,7 +87,9 @@ #define ACCEL_SENS_8G 0.00239364f #define ACCEL_SENS_16G 0.00478728f - +// Highest value in integer, needed to reach the respective dps range value. +// E.g. GYRO_SENS_DEGPS_500 * 28571 ~= 500 +#define GYRO_MAXVAL_INT 28571 diff --git a/src/imu.c b/src/imu.c index 20cbd961..138b4b83 100644 --- a/src/imu.c +++ b/src/imu.c @@ -34,6 +34,9 @@ float offset_accel_1_x; float offset_accel_1_y; float offset_accel_1_z; +float imu_gyro0_weight[3] = {0.5f, 0.5f, 0.5f}; +float imu_gyro1_weight[3] = {0.5f, 0.5f, 0.5f}; + ImuCalib imu_calib0 = { .gyro = { {0, 0, 0}, // Gyro offset @@ -121,7 +124,7 @@ Vector imu_read_gyro_bits(uint8_t cs) { int16_t y = (int16_t)(((uint16_t)buf[1] << 8) | (uint16_t)buf[0]); int16_t z = (int16_t)(((uint16_t)buf[3] << 8) | (uint16_t)buf[2]); int16_t x = -(int16_t)(((uint16_t)buf[5] << 8) | (uint16_t)buf[4]); - + float offset_x = (cs==PIN_SPI_CS0) ? imu_calib0.gyro[0].offset : imu_calib1.gyro[0].offset; float offset_y = (cs==PIN_SPI_CS0) ? imu_calib0.gyro[1].offset : imu_calib1.gyro[1].offset; float offset_z = (cs==PIN_SPI_CS0) ? imu_calib0.gyro[2].offset : imu_calib1.gyro[2].offset; @@ -187,29 +190,26 @@ Vector imu_read_gyro_burst(uint8_t cs, uint8_t samples) { Vector imu_read_gyro() { Vector gyro0 = imu_read_gyro_bits(IMU0); Vector gyro1 = imu_read_gyro_bits(IMU1); - // sum of variances - Vector variance_div = { - imu_calib0.gyro[0].variance + imu_calib1.gyro[0].variance/16, - imu_calib0.gyro[1].variance + imu_calib1.gyro[1].variance/16, - imu_calib0.gyro[2].variance + imu_calib1.gyro[2].variance/16 - }; - // weight of IMU0 proportional to variance of IMU1 - // the more noise on IMU1 the more we use IMU0... - // see also: Kalman Filter. Vector weight_0 = { - imu_calib1.gyro[0].variance/ variance_div.x/16, - imu_calib1.gyro[1].variance/ variance_div.y/16, - imu_calib1.gyro[2].variance/ variance_div.z/16, + imu_gyro0_weight[0], + imu_gyro0_weight[1], + imu_gyro0_weight[2] }; + // IMU 0 is on 500dps, IMU 1 is on 125dps. // when IMU 0 is on 20% of its range or higher, IMU 1 is on 80% of its range // then IMU 0 takes over 100% of the weight. - weight_0.x = fabsf(gyro0.x)>0.20*32768 ? weight_0.x : 1; - weight_0.y = fabsf(gyro0.y)>0.20*32768 ? weight_0.y : 1; - weight_0.z = fabsf(gyro0.z)>0.20*32768 ? weight_0.z : 1; - float x = (gyro0.x * weight_0.x) + (gyro1.x * (1.0-weight_0.x) / 4); - float y = (gyro0.y * weight_0.y) + (gyro1.y * (1.0-weight_0.y) / 4); - float z = (gyro0.z * weight_0.z) + (gyro1.z * (1.0-weight_0.z) / 4); + // When vector-absolute value saturates, it is safer to switch all axes. + + if (gyro0.x*gyro0.x + gyro0.y*gyro0.y + gyro0.z*gyro0.z > 0.20f*0.20f*GYRO_MAXVAL_INT*GYRO_MAXVAL_INT) { + weight_0.x = 1; + weight_0.y = 1; + weight_0.z = 1; + } + + float x = (gyro0.x * weight_0.x) + (gyro1.x * (1.0f-weight_0.x) / 4); + float y = (gyro0.y * weight_0.y) + (gyro1.y * (1.0f-weight_0.y) / 4); + float z = (gyro0.z * weight_0.z) + (gyro1.z * (1.0f-weight_0.z) / 4); return (Vector){x, y, z}; } @@ -348,6 +348,26 @@ void imu_load_calibration() { imu_calib1.accel[0].stddev = config->stddev_accel_1_x; imu_calib1.accel[1].stddev = config->stddev_accel_1_y; imu_calib1.accel[2].stddev = config->stddev_accel_1_z; + + + // sum of variances + Vector variance_div = { + imu_calib0.gyro[0].variance + imu_calib1.gyro[0].variance/16, + imu_calib0.gyro[1].variance + imu_calib1.gyro[1].variance/16, + imu_calib0.gyro[2].variance + imu_calib1.gyro[2].variance/16 + }; + // weight of IMU0 proportional to variance of IMU1 + // the more noise on IMU1 the more we use IMU0... + // see also: Kalman Filter. + + imu_gyro0_weight[0] = imu_calib1.gyro[0].variance/ variance_div.x/16; + imu_gyro0_weight[1] = imu_calib1.gyro[1].variance/ variance_div.y/16; + imu_gyro0_weight[2] = imu_calib1.gyro[2].variance/ variance_div.z/16; + + imu_gyro1_weight[0] = 1 - imu_gyro0_weight[0]; + imu_gyro1_weight[1] = 1 - imu_gyro0_weight[1]; + imu_gyro1_weight[2] = 1 - imu_gyro0_weight[2]; + } void imu_reset_calibration() { diff --git a/src/rotation_fast.c b/src/rotation_fast.c index c9e8c392..c5559a65 100644 --- a/src/rotation_fast.c +++ b/src/rotation_fast.c @@ -6,6 +6,7 @@ // A value of 0.8 means the filter will converge in about 3x0.8 = 2.4 seconds. float comp_tau = 1/0.8f; + void update_rotation_state(RotationStateVector *state, const float *gyro, const float *accel, float dt) { /* This is a rotation scheme that tracks the orientation of the 'up' unit vector, @@ -20,23 +21,20 @@ void update_rotation_state(RotationStateVector *state, const float *gyro, const // so we can calculate it by projecting the gyro vector onto the current 'up' vector. // The 'up' vector is represented by the state vector (ux, uy, uz). float yaw_value = state->ux*gyro_x + state->uy*gyro_y + state->uz*gyro_z; - // The roll and pitch components of the gyro vector are the components that are not aligned with the 'up' vector. - float gyro_rollpitch_x = gyro_x - state->ux * yaw_value; - float gyro_rollpitch_y = gyro_y - state->uy * yaw_value; - float gyro_rollpitch_z = gyro_z - state->uz * yaw_value; + float accel_norm = sqrtf(accel[0] * accel[0] + accel[1] * accel[1] + accel[2] * accel[2]); - // Cross product of the gyro roll/pitch vector with the current 'up' vector gives us the rotation vector + // Cross product of the gyro vector with the current 'up' vector gives us the rotation vector // that we need to apply to the 'up' vector to adjust it. - float wcross_vector[3] = {gyro_rollpitch_z*state->uy - gyro_rollpitch_y*state->uz, - gyro_rollpitch_x*state->uz - gyro_rollpitch_z*state->ux, - gyro_rollpitch_y*state->ux - gyro_rollpitch_x*state->uy}; + float wcross_vector[3] = {gyro_z*state->uy - gyro_y*state->uz, + gyro_x*state->uz - gyro_z*state->ux, + gyro_y*state->ux - gyro_x*state->uy}; state->phi += yaw_value * dt; state->ux += wcross_vector[0] * dt; state->uy += wcross_vector[1] * dt; state->uz += wcross_vector[2] * dt; - + // Adjusting the 'up' vector using the accelerometer data and complementary filter. // Only perform the adjustment if the accelerometer data is within a reasonable range. if (accel_norm>8 && accel_norm<12) { From 82bd96231452627de0f05ae147a1b40b7c084bf3 Mon Sep 17 00:00:00 2001 From: GarryBGoode Date: Sun, 6 Jul 2025 14:35:46 +0200 Subject: [PATCH 08/16] Changed gyro X sensitivity from 2 back to 1 --- src/headers/config.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/headers/config.h b/src/headers/config.h index e162758e..fe9c9bba 100644 --- a/src/headers/config.h +++ b/src/headers/config.h @@ -49,7 +49,7 @@ #define CFG_CALIBRATION_PROGRESS_BAR 40 #define CFG_GYRO_SENSITIVITY (pow(2, -9) * 1.45) -#define CFG_GYRO_SENSITIVITY_X (CFG_GYRO_SENSITIVITY * 2) +#define CFG_GYRO_SENSITIVITY_X (CFG_GYRO_SENSITIVITY * 1) #define CFG_GYRO_SENSITIVITY_Y (CFG_GYRO_SENSITIVITY * 1) #define CFG_GYRO_SENSITIVITY_Z (CFG_GYRO_SENSITIVITY * 1) From ce8e557726c1105411ef7cafaa0dd14721674d6e Mon Sep 17 00:00:00 2001 From: GarryBGoode Date: Sun, 13 Jul 2025 18:45:10 +0200 Subject: [PATCH 09/16] Added printing of imu weights. Added missing Z axis reporting for abs angle mode. --- src/gyro.c | 3 +++ src/imu.c | 11 ++++++++--- 2 files changed, 11 insertions(+), 3 deletions(-) diff --git a/src/gyro.c b/src/gyro.c index a251235a..e16171e1 100644 --- a/src/gyro.c +++ b/src/gyro.c @@ -181,6 +181,9 @@ void Gyro__report_absolute_fast(Gyro *self){ else gyro_absolute_output(-x, self->actions_x_neg, &(self->pressed_x_neg)); if (y >= 0) gyro_absolute_output( y, self->actions_y_pos, &(self->pressed_y_pos)); else gyro_absolute_output(-y, self->actions_y_neg, &(self->pressed_y_neg)); + if (z >= 0) gyro_absolute_output( z, self->actions_z_pos, &(self->pressed_z_pos)); + else gyro_absolute_output(-z, self->actions_z_neg, &(self->pressed_z_neg)); + } diff --git a/src/imu.c b/src/imu.c index f78f45ba..d0cc8432 100644 --- a/src/imu.c +++ b/src/imu.c @@ -201,9 +201,9 @@ Vector imu_read_gyro() { weight_0.z = 1; } - float x = (gyro0.x * weight_0.x) + (gyro1.x * (1.0f-weight_0.x) / 4); - float y = (gyro0.y * weight_0.y) + (gyro1.y * (1.0f-weight_0.y) / 4); - float z = (gyro0.z * weight_0.z) + (gyro1.z * (1.0f-weight_0.z) / 4); + float x = (gyro0.x * weight_0.x) + (gyro1.x * (1.0f-weight_0.x) / 4.0f); + float y = (gyro0.y * weight_0.y) + (gyro1.y * (1.0f-weight_0.y) / 4.0f); + float z = (gyro0.z * weight_0.z) + (gyro1.z * (1.0f-weight_0.z) / 4.0f); return (Vector){x, y, z}; } @@ -343,6 +343,7 @@ void imu_load_calibration() { imu_calib1.accel[1].stddev = config->stddev_accel_1_y; imu_calib1.accel[2].stddev = config->stddev_accel_1_z; + // IMU0 is on 500dps, IMU1 is on 125dps. // sum of variances Vector variance_div = { @@ -361,6 +362,10 @@ void imu_load_calibration() { imu_gyro1_weight[0] = 1 - imu_gyro0_weight[0]; imu_gyro1_weight[1] = 1 - imu_gyro0_weight[1]; imu_gyro1_weight[2] = 1 - imu_gyro0_weight[2]; + info("IMU0 weight: x=%.02f y=%.02f z=%.02f\n", + imu_gyro0_weight[0], imu_gyro0_weight[1], imu_gyro0_weight[2]); + info("IMU1 weight: x=%.02f y=%.02f z=%.02f\n", + imu_gyro1_weight[0], imu_gyro1_weight[1], imu_gyro1_weight[2]); } From b3e55fdaa932688abefcf7afb246ec260f7ac8ea Mon Sep 17 00:00:00 2001 From: GarryBGoode Date: Tue, 22 Jul 2025 22:55:22 +0200 Subject: [PATCH 10/16] Powf(x,2) instead of x*x is a crime --- src/thumbstick.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/thumbstick.c b/src/thumbstick.c index 603c1555..bf4d4d26 100644 --- a/src/thumbstick.c +++ b/src/thumbstick.c @@ -460,7 +460,7 @@ void Thumbstick__report(Thumbstick *self) { deadzone /= self->saturation; // Calculate trigonometry. float angle = atan2(x, -y) * (180 / M_PI); - float radius = sqrt(powf(x, 2) + powf(y, 2)); + float radius = sqrt(x*x+ y*y); radius = constrain(radius, 0, 1); if (radius < deadzone) { radius = 0; From 986f01d3c6547f37f2c107001088a9a051c7f2a6 Mon Sep 17 00:00:00 2001 From: GarryBGoode Date: Tue, 22 Jul 2025 22:56:53 +0200 Subject: [PATCH 11/16] Added code to cycle through HID reports (keyboard, mouse, gamepad) even when everything is - in theory - synced. --- src/hid.c | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/src/hid.c b/src/hid.c index 58d08ac1..6900cf17 100644 --- a/src/hid.c +++ b/src/hid.c @@ -541,6 +541,7 @@ bool hid_should_replay(ReportType type) { } ReportType hid_get_priority() { + static uint8_t cycle_i = 0; // Not all events are sent everytime, they are delivered based on their // priority ratio and how long they have been queueing. // For example thumbstick movement may be queued for some cycles if there @@ -564,6 +565,18 @@ ReportType hid_get_priority() { if (config_get_protocol() == PROTOCOL_GENERIC) return REPORT_GAMEPAD; else return REPORT_XINPUT; } + // if all was synced, just cycle through the reports anyway. + if (cycle_i == 0) { + cycle_i = 1; + return REPORT_KEYBOARD; + } else if (cycle_i == 1) { + cycle_i = 2; + return REPORT_MOUSE; + } else if (cycle_i == 2) { + cycle_i = 0; + if (config_get_protocol() == PROTOCOL_GENERIC) return REPORT_GAMEPAD; + else return REPORT_XINPUT; + } return 0; } From 07f9d3bb71d388694f4ce6942017fc39da868503 Mon Sep 17 00:00:00 2001 From: GarryBGoode Date: Sat, 2 Aug 2025 14:28:43 +0200 Subject: [PATCH 12/16] Lowered tick frequency to 500Hz due to some limitation in wireless mode that's preventing 1kHz. Lowered IMU filter frequency to accomodate 500Hz. Added compensator for thumbstick smoothing so that percieved speed of smoothing is the same while changing tick frequency. --- src/gyro.c | 16 +++++++++------- src/headers/config.h | 2 +- src/imu.c | 2 +- src/thumbstick.c | 2 +- 4 files changed, 12 insertions(+), 10 deletions(-) diff --git a/src/gyro.c b/src/gyro.c index e16171e1..f2867154 100644 --- a/src/gyro.c +++ b/src/gyro.c @@ -196,8 +196,15 @@ void Gyro__report_incremental(Gyro *self) { float x = imu_gyro.x * CFG_GYRO_SENSITIVITY_X * sensitivity_multiplier; float y = imu_gyro.y * CFG_GYRO_SENSITIVITY_Y * sensitivity_multiplier; float z = imu_gyro.z * CFG_GYRO_SENSITIVITY_Z * sensitivity_multiplier; - // Additional processing. - float t = CFG_IMU_DEADZONE; + + + // compensate tick frequency. + x *= (float)REFERENCE_TICK_FREQUENCY/(float)CFG_TICK_FREQUENCY; + y *= (float)REFERENCE_TICK_FREQUENCY/(float)CFG_TICK_FREQUENCY; + z *= (float)REFERENCE_TICK_FREQUENCY/(float)CFG_TICK_FREQUENCY; + + //Additional processing. + float t = CFG_IMU_DEADZONE*0; float k = CFG_IMU_DEADZONE_STRENGTH; if (x > 0 && x < t) x = hssnf(t, k, x); else if (x < 0 && x > -t) x = -hssnf(t, k, -x); @@ -206,11 +213,6 @@ void Gyro__report_incremental(Gyro *self) { if (z > 0 && z < t) z = hssnf(t, k, z); else if (z < 0 && z > -t) z = -hssnf(t, k, -z); - // compensate tick frequency. - x *= (float)REFERENCE_TICK_FREQUENCY/(float)CFG_TICK_FREQUENCY; - y *= (float)REFERENCE_TICK_FREQUENCY/(float)CFG_TICK_FREQUENCY; - z *= (float)REFERENCE_TICK_FREQUENCY/(float)CFG_TICK_FREQUENCY; - // Reintroduce subpixel leftovers. x += sub_x; y += sub_y; diff --git a/src/headers/config.h b/src/headers/config.h index fe9c9bba..a5026354 100644 --- a/src/headers/config.h +++ b/src/headers/config.h @@ -28,7 +28,7 @@ #ifdef DEVICE_DONGLE #define CFG_TICK_FREQUENCY 1000 // Hz. #else - #define CFG_TICK_FREQUENCY 1000 // Hz. + #define CFG_TICK_FREQUENCY 500 // Hz. #define REFERENCE_TICK_FREQUENCY 250 // Hz. This used to be the default, it is used for backward compatibility. #endif diff --git a/src/imu.c b/src/imu.c index d0cc8432..cbf68260 100644 --- a/src/imu.c +++ b/src/imu.c @@ -79,7 +79,7 @@ void imu_init_single(uint8_t cs, uint8_t gyro_conf) { // Gyro options. // bus_spi_write(cs, IMU_CTRL3_C, IMU_CTRL3_C_BDU); // Block data update bus_spi_write(cs, IMU_CTRL4_C, GYRO_LPF1_ENABLE_CTRL4_C); // Enable LPF1 - bus_spi_write(cs, IMU_CTRL6_C, GYRO_LPF1_470HZ_CTRL6_C); // FTYPE selection for LPF1 + bus_spi_write(cs, IMU_CTRL6_C, GYRO_LPF1_223HZ_CTRL6_C); // FTYPE selection for LPF1 bus_spi_write(cs, IMU_CTRL2_G, gyro_conf); uint8_t xl = bus_spi_read_one(cs, IMU_READ | IMU_CTRL1_XL); uint8_t g = bus_spi_read_one(cs, IMU_READ | IMU_CTRL2_G); diff --git a/src/thumbstick.c b/src/thumbstick.c index bf4d4d26..81d20b96 100644 --- a/src/thumbstick.c +++ b/src/thumbstick.c @@ -63,7 +63,7 @@ void thumbstick_update_offsets() { // Refresh runtime smoothing factor with value from config. void thumbstick_update_smooth_samples() { Config *config = config_read(); - thumbstick_smooth_samples = config->thumbstick_smooth_samples; + thumbstick_smooth_samples = config->thumbstick_smooth_samples * CFG_TICK_FREQUENCY/REFERENCE_TICK_FREQUENCY; } void thumbstick_calibrate_each(uint8_t pin_x, uint8_t pin_y, float *result_x, float *result_y) { From 38f1ade29705c73ab9639a99d4d1f57f3373d291 Mon Sep 17 00:00:00 2001 From: GarryBGoode Date: Sat, 2 Aug 2025 16:05:36 +0200 Subject: [PATCH 13/16] Added function to monitor idle state in wireless mode, and automatically go to sleep after 2 minutes of inactivity. --- src/headers/hid.h | 6 ++++++ src/hid.c | 36 ++++++++++++++++++++++++++++++++++++ 2 files changed, 42 insertions(+) diff --git a/src/headers/hid.h b/src/headers/hid.h index cf2cd7d3..f1c01a2c 100644 --- a/src/headers/hid.h +++ b/src/headers/hid.h @@ -17,6 +17,10 @@ #define GAMEPAD_AXIS_INDEX_END PROC_INDEX - 1 #define PROC_INDEX_END 255 +// timeout for going to sleep in wireless mode +// set to 0 to disable +#define HID_IDLE_TIMEOUT 2*60*CFG_TICK_FREQUENCY // 2 minutes in ticks + #define KEY_NONE 0 #define KEY_A 4 @@ -348,3 +352,5 @@ typedef struct __packed _GamepadReport { } GamepadReport; void hid_report_dongle(uint8_t report_id, uint8_t* payload); + +bool hid_idle_timeout(); \ No newline at end of file diff --git a/src/hid.c b/src/hid.c index 6900cf17..eb6f746a 100644 --- a/src/hid.c +++ b/src/hid.c @@ -87,6 +87,8 @@ static bool report_was_sent[4] = {false,}; // Prevent replay if no report was e static uint8_t cycles_without_reporting[4] = {0,}; // Cycles since the last report. static uint8_t replayed_ntimes[4] = {0,}; // How many times the last report was replayed. +uint32_t idle_counter = 0; // Counter to track idle time. + void hid_set_allow_communication(bool value) { hid_allow_communication = value; } @@ -604,6 +606,7 @@ bool hid_report_wired() { } bool hid_report_wireless() { + if (hid_idle_timeout()) power_dormant(); // If idle long enough, go to sleep. if (!hid_allow_communication) return true; ReportType device_to_report = hid_get_priority(); if (device_to_report == REPORT_KEYBOARD) hid_report_keyboard(false); @@ -683,3 +686,36 @@ void hid_init() { info("INIT: HID\n"); alarm_pool = alarm_pool_create(2, 255); } + + +bool hid_idle_timeout(){ + static uint8_t state_matrix_prev[256] = {0,}; + static int16_t mouse_x_prev = 0; + static int16_t mouse_y_prev = 0; + static double gamepad_axis_prev[6] = {0,}; + bool changed = false; + // Check if the state matrix has changed. + if (memcmp(state_matrix, state_matrix_prev, sizeof(state_matrix)) != 0) changed = true; + // Check if the mouse position has changed. + if (mouse_x != mouse_x_prev || mouse_y != mouse_y_prev) changed = true; + // Check if the gamepad axis has changed. + for (uint8_t i = 0; i < 6; i++) { + if (gamepad_axis[i] != gamepad_axis_prev[i]) { + changed = true; + break; + } + } + // Update previous state variables. + memcpy(state_matrix_prev, state_matrix, sizeof(state_matrix)); + mouse_x_prev = mouse_x; + mouse_y_prev = mouse_y; + memcpy(gamepad_axis_prev, gamepad_axis, sizeof(gamepad_axis)); + + if (changed) { + idle_counter = 0; // Reset idle counter if there was any change. + } else { + idle_counter++; // Increment idle counter if no changes. + } + if(idle_counter > HID_IDLE_TIMEOUT && HID_IDLE_TIMEOUT>0) return true; + else return false; // Return true if idle timeout is reached. +} \ No newline at end of file From 0f799195af8efd0e61dfd5abf11b20f1b74ad4b7 Mon Sep 17 00:00:00 2001 From: GarryBGoode Date: Sat, 9 Aug 2025 18:38:39 +0200 Subject: [PATCH 14/16] Removed TICK_FREQUENCY for dongle mode. Dongle will run as fast as it can. It will update USB as soon as it receives anything. --- src/headers/config.h | 11 +++-------- src/hid.c | 2 +- src/loop.c | 18 ++++++++++-------- 3 files changed, 14 insertions(+), 17 deletions(-) diff --git a/src/headers/config.h b/src/headers/config.h index a5026354..59396be6 100644 --- a/src/headers/config.h +++ b/src/headers/config.h @@ -23,15 +23,10 @@ #define NVM_HOME_PROFILE_VERSION ((MAJOR * 1) + (MINOR * 1) + (PATCH * 0)) #define NVM_PROFILE_VERSION ((MAJOR * 1) + (MINOR * 0) + (PATCH * 0)) -#define CFG_LED_BRIGHTNESS 0.2 - -#ifdef DEVICE_DONGLE - #define CFG_TICK_FREQUENCY 1000 // Hz. -#else - #define CFG_TICK_FREQUENCY 500 // Hz. - #define REFERENCE_TICK_FREQUENCY 250 // Hz. This used to be the default, it is used for backward compatibility. -#endif +#define CFG_LED_BRIGHTNESS 0.05 +#define CFG_TICK_FREQUENCY 500 // Hz. +#define REFERENCE_TICK_FREQUENCY 250 // Hz. This used to be the default, it is used for backward compatibility. #define CFG_IMU_TICK_SAMPLES 128 // Multi-sampling per pooling cycle. #define CFG_IMU_DEADZONE 1.0 // Deadzone for the IMU in pixels per second diff --git a/src/hid.c b/src/hid.c index eb6f746a..f2bc7bf3 100644 --- a/src/hid.c +++ b/src/hid.c @@ -630,7 +630,7 @@ bool hid_report_wireless() { } void hid_report_dongle(uint8_t report_id, uint8_t* payload) { - tud_task(); + if (tud_ready()) { if (report_id == REPORT_KEYBOARD) { if (tud_hid_ready()) { diff --git a/src/loop.c b/src/loop.c index 0abba5af..2e26284f 100644 --- a/src/loop.c +++ b/src/loop.c @@ -213,8 +213,8 @@ void loop_dongle_task() { uint32_t now = time_us_32(); if (device_mode == WIRELESS) { config_sync(); - wireless_dongle_task(); tud_task(); + wireless_dongle_task(); if (tud_ready()) { webusb_read(); webusb_flush(); @@ -247,15 +247,12 @@ void loop_run() { logging_set_onloop(true); while (true) { i++; - // Start timer. - uint32_t start = time_us_32(); + // Task. #if defined DEVICE_ALPAKKA_V0 || defined DEVICE_ALPAKKA_V1 - loop_controller_task(); - #endif - #ifdef DEVICE_DONGLE - loop_dongle_task(); - #endif + // Start timer. + uint32_t start = time_us_32(); + loop_controller_task(); // Calculate used time. uint32_t used = time_us_32() - start; int32_t unused = CFG_TICK_INTERVAL_IN_US - (int32_t)used; @@ -276,5 +273,10 @@ void loop_run() { info("+"); sleep_us(0); // Allow IRQ to take over even if the controller is overwhelmed. }; + #endif + #ifdef DEVICE_DONGLE + loop_dongle_task(); + #endif + } } From 19169321716dfcce1ff002300ab17398a11a7949 Mon Sep 17 00:00:00 2001 From: GarryBGoode Date: Sat, 9 Aug 2025 19:24:01 +0200 Subject: [PATCH 15/16] Some more double->float conversion in thumbstick --- src/thumbstick.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/thumbstick.c b/src/thumbstick.c index 81d20b96..bd9f7986 100644 --- a/src/thumbstick.c +++ b/src/thumbstick.c @@ -459,8 +459,8 @@ void Thumbstick__report(Thumbstick *self) { float deadzone = self->deadzone_override ? self->deadzone : config_deadzone; deadzone /= self->saturation; // Calculate trigonometry. - float angle = atan2(x, -y) * (180 / M_PI); - float radius = sqrt(x*x+ y*y); + float angle = atan2f(x, -y) * (180 / M_PI); + float radius = sqrtf(x*x+ y*y); radius = constrain(radius, 0, 1); if (radius < deadzone) { radius = 0; From 6a1203cbb3c84bcead39ef5e922492a5842b75aa Mon Sep 17 00:00:00 2001 From: GarryBGoode Date: Mon, 27 Oct 2025 22:11:27 +0100 Subject: [PATCH 16/16] Added screening logic to detect outliers in IMU gyro signal. Might help with click-noise. --- src/imu.c | 72 ++++++++++++++++++++++++++++++++++++++++++++++++------- 1 file changed, 63 insertions(+), 9 deletions(-) diff --git a/src/imu.c b/src/imu.c index cbf68260..8a37489a 100644 --- a/src/imu.c +++ b/src/imu.c @@ -182,6 +182,7 @@ Vector imu_read_gyro_burst(uint8_t cs, uint8_t samples) { } Vector imu_read_gyro() { + static Vector gyro_prev = {0,0,0}; Vector gyro0 = imu_read_gyro_bits(IMU0); Vector gyro1 = imu_read_gyro_bits(IMU1); Vector weight_0 = { @@ -190,21 +191,74 @@ Vector imu_read_gyro() { imu_gyro0_weight[2] }; + Vector mean3 = {0,0,0}; + bool out_of_range = false; + // IMU 0 is on 500dps, IMU 1 is on 125dps. // when IMU 0 is on 20% of its range or higher, IMU 1 is on 80% of its range // then IMU 0 takes over 100% of the weight. // When vector-absolute value saturates, it is safer to switch all axes. - - if (gyro0.x*gyro0.x + gyro0.y*gyro0.y + gyro0.z*gyro0.z > 0.20f*0.20f*GYRO_MAXVAL_INT*GYRO_MAXVAL_INT) { - weight_0.x = 1; - weight_0.y = 1; - weight_0.z = 1; + + float gyro0_abs = (gyro0.x*gyro0.x + gyro0.y*gyro0.y + gyro0.z*gyro0.z); + float gyro1_abs = (gyro1.x*gyro1.x + gyro1.y*gyro1.y + gyro1.z*gyro1.z); + + if (gyro0_abs > 0.20f*0.20f*GYRO_MAXVAL_INT*GYRO_MAXVAL_INT) { + if (gyro1_abs > 0.5f*0.5f*GYRO_MAXVAL_INT*GYRO_MAXVAL_INT ) + { + // truly out of range + weight_0.x = 1; + weight_0.y = 1; + weight_0.z = 1; + out_of_range = true; + } + else + { + // possible noise spike on gyro0 + weight_0.x = 0; + weight_0.y = 0; + weight_0.z = 0; + } + } + gyro1.x /=4.0f; // Scale 125dps to 500dps + gyro1.y /=4.0f; + gyro1.z /=4.0f; + + Vector weight_1 = { + 1-weight_0.x, + 1-weight_0.y, + 1-weight_0.z + }; - float x = (gyro0.x * weight_0.x) + (gyro1.x * (1.0f-weight_0.x) / 4.0f); - float y = (gyro0.y * weight_0.y) + (gyro1.y * (1.0f-weight_0.y) / 4.0f); - float z = (gyro0.z * weight_0.z) + (gyro1.z * (1.0f-weight_0.z) / 4.0f); - return (Vector){x, y, z}; + if(!out_of_range) + { + mean3.x = (gyro0.x *weight_0.x+ gyro1.x * weight_1.x + gyro_prev.x) / (weight_0.x + weight_1.x +1); + mean3.y = (gyro0.y *weight_0.y+ gyro1.y * weight_1.y + gyro_prev.y) / (weight_0.y + weight_1.y +1); + mean3.z = (gyro0.z *weight_0.z+ gyro1.z * weight_1.z + gyro_prev.z) / (weight_0.z + weight_1.z +1); + if ((gyro0.x-mean3.x)*(gyro0.x-mean3.x) + (gyro0.y-mean3.y)*(gyro0.y-mean3.y) + (gyro0.z-mean3.z)*(gyro0.z-mean3.z) > + 0.10f*0.10f*GYRO_MAXVAL_INT*GYRO_MAXVAL_INT) + { + // too much deviation from mean, possible noise spike + weight_0.x = 0; + weight_0.y = 0; + weight_0.z = 0; + } + if ((gyro1.x-mean3.x)*(gyro1.x-mean3.x) + (gyro1.y-mean3.y)*(gyro1.y-mean3.y) + (gyro1.z-mean3.z)*(gyro1.z-mean3.z) > + 0.10f*0.10f*GYRO_MAXVAL_INT*GYRO_MAXVAL_INT) + { + // too much deviation from mean, possible noise spike + weight_1.x = 0; + weight_1.y = 0; + weight_1.z = 0; + } + } + // If out of range, it makes less sense to check outliers from current and prev values. + // So no else clause here. + + gyro_prev.x = (gyro0.x * weight_0.x) + (gyro1.x * (weight_1.x)); + gyro_prev.y = (gyro0.y * weight_0.y) + (gyro1.y * (weight_1.y)); + gyro_prev.z = (gyro0.z * weight_0.z) + (gyro1.z * (weight_1.z)); + return gyro_prev; } Vector imu_read_accel() {