diff --git a/CMakeLists.txt b/CMakeLists.txt index a0ece44e..dfd47433 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -105,6 +105,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/config.c b/src/config.c index 33529127..60f53778 100644 --- a/src/config.c +++ b/src/config.c @@ -151,6 +151,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, @@ -238,6 +250,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() { @@ -265,7 +297,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, @@ -275,7 +307,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, @@ -285,6 +317,26 @@ void config_set_accel_offset(double ax, double ay, double az, double bx, double config_cache_synced = false; } +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, + 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(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, + 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); @@ -457,7 +509,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]; } @@ -473,7 +525,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 5f80acf1..9bbb1392 100644 --- a/src/ctrl.c +++ b/src/ctrl.c @@ -61,7 +61,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/gyro.c b/src/gyro.c index 93f8f198..f2867154 100644 --- a/src/gyro.c +++ b/src/gyro.c @@ -13,14 +13,16 @@ #include "pin.h" #include "touch.h" #include "vector.h" +#include "rotation_fast.h" -double sensitivity_multiplier; +float sensitivity_multiplier; uint8_t world_init = 0; 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(); @@ -84,7 +86,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); @@ -94,9 +96,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; } @@ -148,32 +150,77 @@ 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){ + 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. + 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, dt_us / 1000000.0f); + + // 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)); + 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)); + +} + + 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; - // Additional processing. - double t = 1.0; - double k = 0.5; + 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; + + + // 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); 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); + // 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); @@ -217,21 +264,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); @@ -246,7 +293,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; diff --git a/src/headers/config.h b/src/headers/config.h index 00209ed0..59396be6 100644 --- a/src/headers/config.h +++ b/src/headers/config.h @@ -19,28 +19,27 @@ #define NVM_PROFILE_SIZE 4096 #define NVM_PROFILE_SLOTS 14 -#define NVM_CONFIG_VERSION ((MAJOR * 1) + (MINOR * 1) + (PATCH * 0)) +#define NVM_CONFIG_VERSION ((MAJOR * 1) + (MINOR * 2) + (PATCH * 0)) #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 250 // Hz. -#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 +#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) #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 @@ -83,25 +82,37 @@ 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; 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; @@ -121,8 +132,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); @@ -146,11 +159,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/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/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/headers/imu.h b/src/headers/imu.h index dcb21563..cee6610f 100644 --- a/src/headers/imu.h +++ b/src/headers/imu.h @@ -5,25 +5,93 @@ #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 + +#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. #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 + +// 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 + + #define GYRO_USER_OFFSET_FACTOR 1.5 @@ -34,3 +102,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/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/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/hid.c b/src/hid.c index 58d08ac1..f2bc7bf3 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; } @@ -541,6 +543,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 +567,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; } @@ -591,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); @@ -614,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()) { @@ -670,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 diff --git a/src/imu.c b/src/imu.c index 4642f224..8a37489a 100644 --- a/src/imu.c +++ b/src/imu.c @@ -15,18 +15,47 @@ 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; + +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 + {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(); @@ -35,9 +64,22 @@ 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_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); @@ -72,23 +114,26 @@ 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 = (((int16_t)buf[1] << 8) | (int16_t)buf[0]); - int16_t z = (((int16_t)buf[3] << 8) | (int16_t)buf[2]); - int16_t x = -(((int16_t)buf[5] << 8) | (int16_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; + + 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; + #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 } @@ -96,31 +141,34 @@ 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 = (((int16_t)buf[1] << 8) | (int16_t)buf[0]); - int16_t y = (((int16_t)buf[3] << 8) | (int16_t)buf[2]); - int16_t z = (((int16_t)buf[5] << 8) | (int16_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; + + 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]); + + 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; i 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 + }; + + 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() { @@ -155,97 +271,235 @@ 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; + + // IMU0 is on 500dps, IMU1 is on 125dps. + + // 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]; + 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]); + } 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(); } 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 + } } diff --git a/src/rotation_fast.c b/src/rotation_fast.c new file mode 100644 index 00000000..c5559a65 --- /dev/null +++ b/src/rotation_fast.c @@ -0,0 +1,62 @@ +#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; + + + float accel_norm = sqrtf(accel[0] * accel[0] + accel[1] * accel[1] + accel[2] * accel[2]); + // 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_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) { + 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 diff --git a/src/thumbstick.c b/src/thumbstick.c index ba576716..a3d98179 100644 --- a/src/thumbstick.c +++ b/src/thumbstick.c @@ -65,7 +65,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) { @@ -518,8 +518,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(powf(x, 2) + powf(y, 2)); + 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;