christopherwun · Sep 2, 2025 · Oct 17, 2023 · Oct 17, 2023 · Oct 20, 2023 · Oct 20, 2023
diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml
@@ -255,10 +255,13 @@ jobs:
         wget --no-verbose -O gcc-arm.zip https://developer.arm.com/-/media/Files/downloads/gnu/13.2.rel1/binrel/arm-gnu-toolchain-13.2.rel1-mingw-w64-i686-arm-none-eabi.zip
         unzip -q -d /tmp gcc-arm.zip
         tar -C /tmp/arm-gnu-toolchain* -cf - . | tar -C /usr/local -xf -
-        pip install wheel
+        # We could use a venv instead, but that requires entering the venv on each run step
+        # that runs in its own shell. There are some actions that help with that, but not for msys2
+        # that I can find. (dhalbert)
+        pip install --break-system-packages wheel
         # requirements_dev.txt doesn't install on windows. (with msys2 python)
         # instead, pick a subset for what we want to do
-        pip install cascadetoml jinja2 typer click intelhex
+        pip install --break-system-packages cascadetoml jinja2 typer click intelhex
         # check that installed packages work....?
         which python; python --version; python -c "import cascadetoml"
         which python3; python3 --version; python3 -c "import cascadetoml"

diff --git a/.gitignore b/.gitignore
@@ -6,6 +6,7 @@
 ###################
 *.o
 *.a
+*.P
 !atmel-samd/asf/**/*.a
 *.elf
 *.bin

diff --git a/conf.py b/conf.py
@@ -277,7 +277,7 @@ def autoapi_prepare_jinja_env(jinja_env):
 
 import sphinx_rtd_theme
 html_theme = 'sphinx_rtd_theme'
-html_theme_path = [sphinx_rtd_theme.get_html_theme_path(), '.']
+# html_theme_path = [sphinx_rtd_theme.get_html_theme_path(), '.']
 
 # Theme options are theme-specific and customize the look and feel of a theme
 # further.  For a list of options available for each theme, see the

diff --git a/lib/cionic/diff_filter.c b/lib/cionic/diff_filter.c
@@ -64,7 +64,7 @@ diff_filter_process(diff_filter_t *diff, float *norms, int numchans,
         diff->rms_buffer[i][diff->rms_samples] = output;
         // output channel rms
         uv_out[i] = diff->rms[i];
-    }    
+    }
 
     // 2. if rms_buffer filled compute sos for window and update rms
     if (++diff->rms_samples >= RMS_NUM_SAMPLES) {
@@ -102,4 +102,3 @@ diff_filter_init(diff_filter_t *filter)
 {
     bzero(filter, sizeof(diff_filter_t));
 }
-
diff --git a/lib/cionic/emg_iir.c b/lib/cionic/emg_iir.c
@@ -22,10 +22,10 @@ float emg_lowpass_filter_sos[NO_OF_BQS][NO_OF_COEFFS_PER_BQ] = {
 // b0, b1, b2, a0, a1, a2
 // therefore
 // a0*y[n] = b0*x[0] + b1*x[1] + b2*x[2] -a1*y[1] -a2*y[2]
-// or 
+// or
 // y[n] = b0*x[0] + b1*x[1] + b2*x[2] -a1*y[1] -a2*y[2]
 // since a0 is normalized to 1.0.
-float 
+float
 emg_iir(emg_filter_state_t *state, float val)
 {
     float y;
@@ -34,20 +34,20 @@ emg_iir(emg_filter_state_t *state, float val)
         // update the input delay line
         state->x[i][2] = state->x[i][1];
         state->x[i][1] = state->x[i][0];
-        state->x[i][0] = val; 
-       
+        state->x[i][0] = val;
+
         y = 0;
         y += state->x[i][0]*state->coeffs[i][0];
         y += state->x[i][1]*state->coeffs[i][1];
         y += state->x[i][2]*state->coeffs[i][2];
-    
+
         // skip one coeff here
         y -= state->y[i][0]*state->coeffs[i][4];
         y -= state->y[i][1]*state->coeffs[i][5];
 
         // update the output delay line
         state->y[i][1] = state->y[i][0];
-        state->y[i][0] = y; 
+        state->y[i][0] = y;
         val = y;
     }
     return y;
@@ -61,33 +61,33 @@ emg_iir_init(emg_filter_state_t *state)
         // update the input delay line
         state->x[i][2] = 0;
         state->x[i][1] = 0;
-        state->x[i][0] = 0; 
-    
+        state->x[i][0] = 0;
+
         // update the output delay line
         state->y[i][1] = 0;
-        state->y[i][0] = 0; 
+        state->y[i][0] = 0;
     }
 }
 
 // the DC blocking is showing promise
 #define NO_EMG_RMS_DC_BLOCKING
 // the diff is not working that great
 #define NO_EMG_RMS_DIFF
-double 
+double
 emg_mwa_rms(emg_mwa_state_t *state, float val)
 {
     // subtract the oldest value
-    state->sum -= state->mw[state->write_ptr];    
+    state->sum -= state->mw[state->write_ptr];
     // update the delay line
-    state->mw[state->write_ptr] = val*val;    
+    state->mw[state->write_ptr] = val*val;
     state->write_ptr = (state->write_ptr+1) % EMG_RMS_MA_SIZE;
 
     // the latest sum
     state->sum += val*val;
     double result = state->sum;
     result = sqrt(result/EMG_RMS_MA_SIZE);
 #ifdef EMG_RMS_DC_BLOCKING
-    result = result + state->dc; 
+    result = result + state->dc;
     state->dc = state->dc - result*state->alpha;
 #endif
 #ifdef EMG_RMS_DIFF
@@ -105,12 +105,12 @@ iir_filter_init(iir_filter_t *filter)
     for( int i=0; i<IIR_NUM_CHANNELS; i++) {
         emg_iir_init(&filter->emg_lowpass_iir_state[i]);
         emg_iir_init(&filter->emg_highpass_iir_state[i]);
-        filter->emg_lowpass_iir_state[i].coeffs = emg_lowpass_filter_sos; 
-        filter->emg_highpass_iir_state[i].coeffs = emg_highpass_filter_sos; 
+        filter->emg_lowpass_iir_state[i].coeffs = emg_lowpass_filter_sos;
+        filter->emg_highpass_iir_state[i].coeffs = emg_highpass_filter_sos;
         filter->emg_mwa_state[i].sum = 0;
         filter->emg_mwa_state[i].past_sum = 0;
         filter->emg_mwa_state[i].alpha = EMG_RMS_MA_DC_BLOCK_ALPHA;
-        filter->emg_mwa_state[i].write_ptr = 0; 
+        filter->emg_mwa_state[i].write_ptr = 0;
     }
 }
 

diff --git a/lib/cionic/emg_iir.h b/lib/cionic/emg_iir.h
@@ -12,7 +12,7 @@
 #define EMG_RMS_FILTER_ORDER (6)
 #define EMG_RMS_HIGHPASS_FC (50)
 #define EMG_RMS_LOWPASS_FC (199)
-#define EMG_RMS_SUBSAMPLING_FACTOR (5)  
+#define EMG_RMS_SUBSAMPLING_FACTOR (5)
 #define EMG_RMS_MA_SIZE (300/(EMG_RMS_SUBSAMPLING_FACTOR))
 #define EMG_RMS_MA_DC_BLOCK_FC (0.001)
 #define EMG_RMS_MA_DC_BLOCK_ALPHA (2*M_PI*EMG_RMS_MA_DC_BLOCK_FC/EMG_RMS_FS)
@@ -28,24 +28,24 @@ typedef struct emg_filter_state {
 typedef struct emg_mwa_state {
     float mw[EMG_RMS_MA_SIZE];
     float sum;
-    float past_sum; 
-    float dc; 
+    float past_sum;
+    float dc;
     float alpha;
     int write_ptr;
 } emg_mwa_state_t;
 
 typedef struct iir_filter_t {
     int emg_rms_sub_sample_counter;
-    emg_filter_state_t emg_lowpass_iir_state[IIR_NUM_CHANNELS]; 
-    emg_filter_state_t emg_highpass_iir_state[IIR_NUM_CHANNELS]; 
+    emg_filter_state_t emg_lowpass_iir_state[IIR_NUM_CHANNELS];
+    emg_filter_state_t emg_highpass_iir_state[IIR_NUM_CHANNELS];
     emg_mwa_state_t emg_mwa_state[IIR_NUM_CHANNELS];
 } iir_filter_t;
 
 float emg_iir(emg_filter_state_t *, float);
 void emg_iir_init(emg_filter_state_t *);
 double emg_mwa_rms(emg_mwa_state_t *, float);
 
-// 
+//
 void iir_filter_init(iir_filter_t *filter);
 int iir_filter_process(iir_filter_t *filter, float *norms, int numchans,
                         elapsed_t ts_in, const uint8_t *buffer,

diff --git a/lib/cionic/orientation.c b/lib/cionic/orientation.c
@@ -0,0 +1,169 @@
+#include "shared-bindings/util.h"
+#include "orientation.h"
+#include <math.h>
+
+// quaternion is a 4 float array [x,y,z,w]
+
+void
+orientation_multiply(float *a, float *b, float *out)
+{
+    // val rw = a.w * b.w - a.x * b.x - a.y * b.y - a.z * b.z
+    out[3] = a[3] * b[3] - a[0] * b[0] - a[1] * b[1] - a[2] * b[2];
+    // val rx = a.w * b.x + a.x * b.w + a.y * b.z - a.z * b.y
+    out[0] = a[3] * b[0] + a[0] * b[3] + a[1] * b[2] - a[2] * b[1];
+    // val ry = a.w * b.y - a.x * b.z + a.y * b.w + a.z * b.x
+    out[1] = a[3] * b[1] - a[0] * b[2] + a[1] * b[3] + a[2] * b[0];
+    // val rz = a.w * b.z + a.x * b.y - a.y * b.x + a.z * b.w
+    out[2] = a[3] * b[2] + a[0] * b[1] - a[1] * b[0] + a[2] * b[3];
+}
+
+void
+orientation_inverse(float *a, float *out)
+{
+    // val d = w*w + x*x + y*y + z*z
+    // return Quaternion(-x / d, -y / d, -z / d, w / d)
+    float d = a[3] * a[3] + a[0] * a[0] + a[1] * a[1] + a[2] * a[2];
+    out[0] = -a[0] / d;
+    out[1] = -a[1] / d;
+    out[2] = -a[2] / d;
+    out[3] = a[3] / d;
+}
+
+void
+orientation_difference(float *quat_a, float *quat_b, float *quat_out)
+{
+    float inverse[4];
+    orientation_inverse(quat_a, inverse);
+    orientation_multiply(inverse, quat_b, quat_out);
+}
+
+
+void
+orientation_forward(float rotation, float *out)
+{
+    float half = rotation/2;
+    out[0] = 0.0;
+    out[1] = 0.0;
+    out[2] = sin(half);
+    out[3] = cos(half);
+}
+
+// apply calibration to quat_in to produce quat_out
+//
+void
+orientation_normalize(float *calibration, float *quat_in, float *quat_out)
+{
+    float uprighted[5];
+    orientation_multiply(quat_in, calibration, uprighted);
+
+    float forward[5];
+    orientation_forward(calibration[4], forward);
+
+    float forwarded[5];
+    orientation_multiply(uprighted, forward, forwarded);
+
+    float norm[5];
+    orientation_forward(-calibration[4], norm);
+
+    orientation_multiply(norm, forwarded, quat_out);
+}
+
+
+void
+orientation_quaternion_to_euler(float *q, float *e)
+{
+    // roll (x-axis rotation)
+    float sinr = 2.0 * (q[3] * q[0] + q[1] * q[2]);
+    float cosr = 1.0 - 2.0 * (q[0] * q[0] + q[1] * q[1]);
+    e[0] = atan2(sinr, cosr);
+
+    // pitch (y-axis rotation)
+    float sinp = 2.0 * (q[3] * q[1] - q[2] * q[0]);
+    if (fabs(sinp) < 1) {
+        e[1] = asin(sinp);
+    }
+    else {
+        e[1] = copysign(M_PI_2, sinp);
+    }
+
+    // yaw (z-axis rotation)
+    float siny =  2.0 * (q[3] * q[2] + q[0] * q[1]);
+    float cosy = 1.0 - 2.0 * (q[1] * q[1] + q[2] * q[2]);
+    e[2] = atan2(siny, cosy);
+}
+
+/*
+// a function that could be useful in the future
+def get_quaternion_from_euler(roll, pitch, yaw):
+  """
+  Convert an Euler angle to a quaternion.
+
+  Input
+    :param roll: The roll (rotation around x-axis) angle in radians.
+    :param pitch: The pitch (rotation around y-axis) angle in radians.
+    :param yaw: The yaw (rotation around z-axis) angle in radians.
+
+  Output
+    :return qx, qy, qz, qw: The orientation in quaternion [x,y,z,w] format
+  """
+  qx = np.sin(roll/2) * np.cos(pitch/2) * np.cos(yaw/2) - np.cos(roll/2) * np.sin(pitch/2) * np.sin(yaw/2)
+  qy = np.cos(roll/2) * np.sin(pitch/2) * np.cos(yaw/2) + np.sin(roll/2) * np.cos(pitch/2) * np.sin(yaw/2)
+  qz = np.cos(roll/2) * np.cos(pitch/2) * np.sin(yaw/2) - np.sin(roll/2) * np.sin(pitch/2) * np.cos(yaw/2)
+  qw = np.cos(roll/2) * np.cos(pitch/2) * np.cos(yaw/2) + np.sin(roll/2) * np.sin(pitch/2) * np.sin(yaw/2)
+
+  return [qx, qy, qz, qw]
+
+*/
+
+void
+euler_to_orientation_quaternion(float *e, float *q)
+{
+    float roll, pitch, yaw;
+    roll = e[0];
+    pitch = e[1];
+    yaw = e[2];
+
+    q[0] = sin(roll/2) * cos(pitch/2) * cos(yaw/2) - cos(roll/2) * sin(pitch/2) * sin(yaw/2);
+    q[1] = cos(roll/2) * sin(pitch/2) * cos(yaw/2) + sin(roll/2) * cos(pitch/2) * sin(yaw/2);
+    q[2] = cos(roll/2) * cos(pitch/2) * sin(yaw/2) - sin(roll/2) * sin(pitch/2) * cos(yaw/2);
+    q[3] = cos(roll/2) * cos(pitch/2) * cos(yaw/2) + sin(roll/2) * sin(pitch/2) * sin(yaw/2);
+}
+
+
+// clear calibration vector
+int
+orientation_cal_clear(float *calibration)
+{
+    calibration[0] = 0; // i
+    calibration[1] = 0; // j
+    calibration[2] = 0; // k
+    calibration[3] = 1; // real
+    calibration[4] = 0; // rotation
+    return 0;
+}
+
+// calibrate upright
+int orientation_cal_upright(float *quat, float *calibration)
+{
+    orientation_inverse(quat, calibration);
+    return 0;
+}
+
+// calibrate forward
+int orientation_cal_forward(float *quat, float *calibration)
+{
+    float calibrated[4];
+    float euler[3];
+
+    // apply rotation greedily until find zero pitch
+    for (float i=-180.0; i <= 180.0; i += 1.0) {
+        calibration[4] = i * 0.0174533; // convert degrees to radians
+        orientation_normalize(calibration, quat, calibrated);
+        orientation_quaternion_to_euler(calibrated, euler);
+        if (RAD2DEG(euler[0]) > 0 && RAD2DEG(euler[1]) == 0) {
+            return 0;
+        }
+    }
+    calibration[4] = 0;
+    return -1;
+}
diff --git a/lib/cionic/orientation.h b/lib/cionic/orientation.h
@@ -0,0 +1,42 @@
+#ifndef __ORIENTATION_H__
+#define __ORIENTATION_H__
+
+#define DEG2RAD(d) ((float)d * 0.0174533)
+#define RAD2DEG(r) ((int)(r*57.2958))
+
+// quat_a * quat_b output to quat_out
+//
+void orientation_multiply(float *quat_a, float *quat_b, float *quat_out);
+
+// perform quaterion invers on quat_in to produce quat_out
+//
+void orientation_inverse(float *quat_in, float *quat_out);
+
+// calculate difference between quat_a and quat_b and output to quat out
+//
+void orientation_difference(float *quat_a, float *quat_b, float *quat_out);
+
+void orientation_forward(float rotation, float *out);
+
+// apply calibration to quat_in to produce quat_out
+//
+void orientation_normalize(float *calibration, float *quat_in, float *quat_out);
+
+// convert a quaternion to euler
+//
+void orientation_quaternion_to_euler(float *quaternion, float *euler);
+
+// convert euler to quaternion
+//
+void euler_to_orientation_quaternion(float *euler, float *quaternion);
+
+// clear calibration vector
+int orientation_cal_clear(float *calibration);
+
+// calibrate upright
+int orientation_cal_upright(float *quat, float *calibration);
+
+// calibrate forward
+int orientation_cal_forward(float *quat, float *calibration);
+
+#endif
-Original file line number
+Diff line change
@@ Expand Up / @@ -6,6 +6,7 @@ @@
     ###################
     *.o
     *.a
+    *.P
     !atmel-samd/asf/**/*.a
     *.elf
     *.bin
@@ Expand Down @@