IMU 1kHz update

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

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];

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

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;