Separate analog lib + frogboard base + Simple LPF-based snapback filter

.gitignore

@@ -1 +1,2 @@
-build/
+build/
+.cache/

.vscode/launch.json

@@ -0,0 +1,17 @@
+{
+    // Use IntelliSense to learn about possible attributes.
+    // Hover to view descriptions of existing attributes.
+    // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387
+    "version": "0.2.0",
+    "configurations": [
+        {
+            "name": "Cortex Debug",
+            "cwd": "${workspaceFolder}",
+            "executable": "./build/frogboard.elf",
+            "request": "launch",
+            "type": "cortex-debug",
+            "runToEntryPoint": "main",
+            "servertype": "stlink"
+        }
+    ]
+}

.vscode/settings.json

@@ -11,6 +11,9 @@
         "lis3mdl.h": "c",
         "hw_intf.h": "c",
         "phobri_v1_x.h": "c",
-        "zenith.h": "c"
-    }
+        "zenith.h": "c",
+        "stick.h": "c",
+        "stick_task.h": "c"
+    },
+    "cortex-debug.variableUseNaturalFormat": false
 }

fw/analog_lib/CMakeLists.txt

@@ -0,0 +1,11 @@
+add_library(analog_lib INTERFACE)
+
+target_sources(analog_lib INTERFACE
+  ${CMAKE_CURRENT_LIST_DIR}/src/linearize.c 
+  ${CMAKE_CURRENT_LIST_DIR}/src/notch_remap.c 
+  ${CMAKE_CURRENT_LIST_DIR}/src/stick.c 
+)
+
+target_include_directories(analog_lib INTERFACE
+  ${CMAKE_CURRENT_LIST_DIR}/include
+)

fw/include/zenith/input/linearize.h → fw/analog_lib/include/linearize.h

@@ -1,7 +1,7 @@
 #ifndef ZENITH_LINEARIZE_H
 #define ZENITH_LINEARIZE_H
 
-#include "zenith/input/stick.h"
+#include "stick.h"
 
 static const double linearize_reference[] = {
     -100 / 127.0, -75 / 127.0, 0 / 127.0, 75 / 127.0, 100 / 127.0};

fw/include/zenith/input/notch_remap.h → fw/analog_lib/include/notch_remap.h

@@ -1,7 +1,7 @@
 #ifndef ZENITH_NOTCH_REMAP_H
 #define ZENITH_NOTCH_REMAP_H
 
-#include "zenith/input/stick.h"
+#include "stick.h"
 
 void notch_remap(const ax_t x_in, const ax_t y_in, ax_t *x_out, ax_t *y_out,
                  const bool gate_limiter_enable,

fw/analog_lib/include/stick.h

@@ -0,0 +1,52 @@
+#ifndef STICK_H
+#define STICK_H
+
+#ifdef DEBUG
+#define debug_print(fmt, args...) printf(fmt, ##args)
+#else
+#define debug_print(fmt, args...)
+#endif
+
+#include "stick_types.h"
+
+#include <stdbool.h>
+#include <stdint.h>
+
+#define NUM_NOTCHES 8
+#define FIT_ORDER 3
+
+typedef struct {
+    bool calibrated;
+    float fit_coeffs_x[FIT_ORDER + 1];
+    float fit_coeffs_y[FIT_ORDER + 1];
+
+    float affine_coeffs[NUM_NOTCHES][4];
+    float boundary_angles[NUM_NOTCHES];
+
+    ax_t notch_points_x_in[NUM_NOTCHES];
+    ax_t notch_points_y_in[NUM_NOTCHES];
+} calib_results_t;
+
+typedef struct {
+    ax_t notch_points_x[NUM_NOTCHES];
+    ax_t notch_points_y[NUM_NOTCHES];
+    float angle_deadzones[NUM_NOTCHES];
+    float mag_threshold;
+} stick_config_t;
+
+#define CALIBRATION_NUM_STEPS NUM_NOTCHES * 2
+
+extern volatile int _cal_step;
+
+typedef enum { CALIB_NONE, CALIB_ADVANCE, CALIB_UNDO } cal_msg_t;
+
+void analoglib_init(calib_results_t *settings_calib_results, stick_config_t *settings_stick_config);
+
+void analoglib_cal_advance(analog_data_t *in);
+
+void analoglib_cal_undo(void);
+
+void analoglib_process(analog_data_t *in, analog_data_t *out,
+                   const bool gate_limiter_enable);
+
+#endif // STICK_H

fw/analog_lib/include/stick_types.h

@@ -0,0 +1,29 @@
+#ifndef STICK_TYPES_H
+#define STICK_TYPES_H
+
+#include <math.h>
+
+// Represents the range of an axis from -1.0 to 1.0.
+// This is an arbitrary range, but it's meant to be easiest to
+// convert to different representations used by different devices.
+typedef float ax_t;
+
+#define AX_TO_UINT8(x)                                                         \
+    ((uint8_t)((int8_t)((x) * (1 << (8 - 1))) + (1 << (8 - 1))))
+#define AX_TO_INT8(x) ((int8_t)((x) * (1 << (8 - 1))))
+#define AX_TO_INT16(x) ((int16_t)((x) * (1 << (16 - 1))))
+#define AX_TO_INT32(x) ((int32_t)((x) * (1 << (32 - 1))))
+#define INT_N_TO_AX(x, N) (((ax_t)(x)) / (1 << (N - 1)))
+#define UINT_N_TO_AX(x, N)                                                     \
+    ((ax_t)((int)((x) - (1 << (N - 1)))) / (1 << (N - 1)))
+
+typedef struct {
+    ax_t ax1;
+    ax_t ax2;
+    ax_t ax3;
+    ax_t ax4;
+    ax_t ax5;
+    ax_t ax6;
+} analog_data_t;
+
+#endif // STICK_TYPES_H

fw/lib/input/linearize.c → fw/analog_lib/src/linearize.c

@@ -1,4 +1,4 @@
-#include "zenith/includes.h"
+#include "linearize.h"
 
 ax_t linearize(const ax_t point, const float coefficients[]) {
     return (coefficients[0] * (point * point * point) +

fw/lib/input/notch_remap.c → fw/analog_lib/src/notch_remap.c

@@ -1,4 +1,4 @@
-#include "zenith/includes.h"
+#include "notch_remap.h"
 
 // Invert a 3x3 matrix
 void inverse(const float in[3][3], float out[3][3]) {
@@ -226,7 +226,7 @@ void notch_calibrate(const ax_t in_points_x[], const ax_t in_points_y[],
             calib_results->boundary_angles[0]) {
             calib_results->boundary_angles[cur] += M_PI * 2;
         }
-        debug_print("Boundary angle for region %d: %f\n", cur,
-                    calib_results->boundary_angles[cur]);
+        debug_print("Boundary angle for region %d: %d\n\r", cur,
+                    (uint32_t)(calib_results->boundary_angles[cur] * (180.)/M_PI));
     }
 }

fw/lib/input/calib.c → fw/analog_lib/src/stick.c

@@ -1,7 +1,11 @@
-#include "zenith/includes.h"
+#include "stick.h"
+#include "notch_remap.h"
+#include "linearize.h"
+
+calib_results_t* al_g_calib_results;
+stick_config_t* al_g_stick_config;
 
 volatile int _cal_step = 0;
-volatile _Atomic cal_msg_t _cal_msg;
 ax_t raw_cal_points_x[CALIBRATION_NUM_STEPS];
 ax_t raw_cal_points_y[CALIBRATION_NUM_STEPS];
 
@@ -73,47 +77,18 @@ void fold_center_points(const ax_t raw_cal_points_x[],
     cleaned_points_y[0] = cleaned_points_y[0] / ((float)NUM_NOTCHES - 2);
 }
 
-void calibration_advance(analog_data_t *in) {
-    // failsafe - this function should not be called if incrementing the step
-    // would lead to an invalid state
-    if (_cal_step < 1 || _cal_step > CALIBRATION_NUM_STEPS)
-        return;
-
-    raw_cal_points_x[_cal_step - 1] = in->ax1;
-    raw_cal_points_y[_cal_step - 1] = in->ax2;
-    debug_print("Raw X value collected: %f\nRaw Y value collected: %f\n",
-                in->ax1, in->ax2);
-    _cal_step++;
-
-    if (_cal_step > CALIBRATION_NUM_STEPS) {
-        calibration_finish();
-    } else {
-        debug_print("Calibration Step [%d/%d]\n", _cal_step,
-                    CALIBRATION_NUM_STEPS);
-    }
-}
-
-void calibration_undo(void) {
-    // Go back one calibration step, only if we are actually calibrating and
-    // not at the beginning.
-    if (_cal_step > 1) {
-        _cal_step--;
-    }
-    debug_print("Calibration Step [%d/%d]\n", _cal_step, CALIBRATION_NUM_STEPS);
-}
-
 void calibration_finish(void) {
     // We're done calibrating. Do the math to save our calibration parameters
     ax_t cleaned_points_x[NUM_NOTCHES + 1];
     ax_t cleaned_points_y[NUM_NOTCHES + 1];
     for (int i = 0; i < CALIBRATION_NUM_STEPS; i++) {
-        debug_print("Raw Cal point:  %d; (x,y) = (%f, %f)\n", i,
+        debug_print("Raw Cal point:  %d; (x,y) = (%f, %f)\n\r", i,
                     raw_cal_points_x[i], raw_cal_points_y[i]);
     }
     fold_center_points(raw_cal_points_x, raw_cal_points_y, cleaned_points_x,
                        cleaned_points_y);
     for (int i = 0; i <= NUM_NOTCHES; i++) {
-        debug_print("Clean Cal point:  %d; (x,y) = (%f, %f)\n", i,
+        debug_print("Clean Cal point:  %d; (x,y) = (%f, %f)\n\r", i,
                     cleaned_points_x[i], cleaned_points_y[i]);
     }
 
@@ -123,13 +98,13 @@ void calibration_finish(void) {
     ax_t linearized_points_y[NUM_NOTCHES];
 
     linearize_cal(cleaned_points_x, cleaned_points_y, linearized_points_x,
-                  linearized_points_y, &(_settings[_profile].calib_results));
+                  linearized_points_y, al_g_calib_results);
 
     // Copy the linearized points we have just found to phobri's internal data
     // sturcture.
     for (int i = 0; i < NUM_NOTCHES; i++) {
-        _settings[_profile].calib_results.notch_points_x_in[i] = linearized_points_x[i];
-        _settings[_profile].calib_results.notch_points_y_in[i] = linearized_points_y[i];
+        al_g_calib_results->notch_points_x_in[i] = linearized_points_x[i];
+        al_g_calib_results->notch_points_y_in[i] = linearized_points_y[i];
         debug_print("Linearized point:  %d; (x,y) = (%f, %f)\n", i,
                     linearized_points_x[i], linearized_points_y[i]);
     }
@@ -143,31 +118,31 @@ void calibration_finish(void) {
         // angle; doing this will mess it up if the sensor is negative to go up
         // in Y; need to figure out the appropriate place in the code to
         // compensate for this
-        _settings[_profile].calib_results.notch_points_x_in[i] =
+        al_g_calib_results->notch_points_x_in[i] =
             (cleaned_points_x[i + 1] - cleaned_points_x[0]) * x_flip;
-        _settings[_profile].calib_results.notch_points_y_in[i] =
+        al_g_calib_results->notch_points_y_in[i] =
             (cleaned_points_y[i + 1] - cleaned_points_y[0]) * y_flip;
         debug_print("Notch Point in point:  %d; (x,y) = (%f, %f)\n", i,
-                    _settings[_profile].calib_results.notch_points_x_in[i],
-                    _settings[_profile].calib_results.notch_points_y_in[i]);
+                    al_g_calib_results->notch_points_x_in[i],
+                    al_g_calib_results->notch_points_y_in[i]);
     }
     // copy over center offset
-    _settings[_profile].calib_results.fit_coeffs_x[0] = cleaned_points_x[0];
-    _settings[_profile].calib_results.fit_coeffs_y[0] = cleaned_points_y[0];
+    al_g_calib_results->fit_coeffs_x[0] = cleaned_points_x[0];
+    al_g_calib_results->fit_coeffs_y[0] = cleaned_points_y[0];
     // set direction for each axis
-    _settings[_profile].calib_results.fit_coeffs_x[1] = x_flip;
-    _settings[_profile].calib_results.fit_coeffs_y[1] = y_flip;
+    al_g_calib_results->fit_coeffs_x[1] = x_flip;
+    al_g_calib_results->fit_coeffs_y[1] = y_flip;
 
 #endif // ZTH_LINEARIZATON_EN
 
 
-    notch_calibrate(_settings[_profile].calib_results.notch_points_x_in,
-                    _settings[_profile].calib_results.notch_points_y_in,
-                    _settings[_profile].stick_config.notch_points_x,
-                    _settings[_profile].stick_config.notch_points_y,
-                    &(_settings[_profile].calib_results));
+    notch_calibrate(al_g_calib_results->notch_points_x_in,
+                    al_g_calib_results->notch_points_y_in,
+                    al_g_stick_config->notch_points_x,
+                    al_g_stick_config->notch_points_y,
+                    al_g_calib_results);
     debug_print("Calibrated!\n");
-    _settings[_profile].calib_results.calibrated = true;
+    al_g_calib_results->calibrated = true;
     /*debug_print("X coeffs: %f %f %f %f, Y coeffs: %f %f %f %f\n",
            _settings.calib_results.fit_coeffs_x[0],
            _settings.calib_results.fit_coeffs_x[1],
@@ -178,4 +153,60 @@ void calibration_finish(void) {
            _settings.calib_results.fit_coeffs_y[2],
            _settings.calib_results.fit_coeffs_y[3]);*/
     _cal_step = 0;
-}
+}
+
+void analoglib_cal_advance(analog_data_t *in) {
+    // failsafe - this function should not be called if incrementing the step
+    // would lead to an invalid state
+    if (_cal_step < 1 || _cal_step > CALIBRATION_NUM_STEPS)
+        return;
+
+    raw_cal_points_x[_cal_step - 1] = in->ax1;
+    raw_cal_points_y[_cal_step - 1] = in->ax2;
+    debug_print("Raw X value collected: %f\n\rRaw Y value collected: %f\n\r",
+                in->ax1, in->ax2);
+    _cal_step++;
+
+    if (_cal_step > CALIBRATION_NUM_STEPS) {
+        calibration_finish();
+    } else {
+        debug_print("Calibration Step [%d/%d]\n\r", _cal_step,
+                    CALIBRATION_NUM_STEPS);
+    }
+}
+
+void analoglib_cal_undo(void) {
+    // Go back one calibration step, only if we are actually calibrating and
+    // not at the beginning.
+    if (_cal_step > 1) {
+        _cal_step--;
+    }
+    debug_print("Calibration Step [%d/%d]\n", _cal_step, CALIBRATION_NUM_STEPS);
+}
+
+
+void analoglib_init(calib_results_t *settings_calib_results, stick_config_t *settings_stick_config) {
+    al_g_calib_results = settings_calib_results;
+    al_g_stick_config = settings_stick_config;
+}
+
+void analoglib_process(analog_data_t *in, analog_data_t *out,
+                   const bool gate_limiter_enable) {
+
+#if ZTH_LINEARIZATION_EN
+    ax_t notch_remap_in_x = linearize(in->ax1, calib_results->fit_coeffs_x);
+    ax_t notch_remap_in_y = linearize(in->ax2, calib_results->fit_coeffs_y);
+#else
+    ax_t notch_remap_in_x = al_g_calib_results->fit_coeffs_x[1] * (in->ax1 - al_g_calib_results->fit_coeffs_x[0]);
+    ax_t notch_remap_in_y = al_g_calib_results->fit_coeffs_y[1] * (in->ax2 - al_g_calib_results->fit_coeffs_y[0]);
+#endif
+
+    ax_t remapped_x, remapped_y;
+    notch_remap(notch_remap_in_x, notch_remap_in_y, &remapped_x, &remapped_y,
+                gate_limiter_enable, al_g_calib_results, al_g_stick_config);
+
+    out->ax1 = fmin(1.0, fmax(-1.0, remapped_x));
+    out->ax2 = fmin(1.0, fmax(-1.0, remapped_y));
+}
+
+

fw/include/zenith/includes.h

@@ -39,11 +39,9 @@
 #include "zenith/comms/n64_crc.h"
 #include "zenith/comms/comms.h" // For talking over the controller's main protocol (N64, GCC, etc.)
 
+#include "stick.h"
 #include "zenith/input/btn_remap.h"
-#include "zenith/input/stick.h"
-#include "zenith/input/notch_remap.h"
-#include "zenith/input/linearize.h"
-#include "zenith/input/calib.h"
+#include "zenith/input/stick_task.h"
 
 // clang-format on