Trinity Devboard PCB V1.0 Firmware. FreeRTOS is setup and the MCU reads IMU data over SPI fand Magnetometer data over I2C, each with a seperate task. Sensordata is then run though MadgwickAHRS and send over USB as serial packet data to use in trinity visualizer. Bare minimum functionality works and is replicated from the first prototype.

2026-09-13 19:51:03 +02:00
commit 7ee381de58
1630 changed files with 632404 additions and 0 deletions
@@ -0,0 +1,135 @@
+#ifndef MADGWICK_H
+#define MADGWICK_H
+
+typedef struct {
+    float q[4];     // Quaternion: [qw, qx, qy, qz]
+    float beta;     // Filter gain (tunes correction strength)
+    float sampleFreq; // Sampling frequency in Hz
+} MadgwickFilter;
+
+void MadgwickFilter_Init(MadgwickFilter* filter, float sampleFreq, float beta);
+void MadgwickFilter_Update(MadgwickFilter* filter, float gx, float gy, float gz,
+                          float ax, float ay, float az);
+void MadgwickFilter_GetEulerAngles(MadgwickFilter* filter, float* roll, float* pitch, float* yaw);
+
+#ifdef MADGWICK_IMPLEMENTATION
+#include <math.h>
+
+// Initialize the filter
+void MadgwickFilter_Init(MadgwickFilter* filter, float sampleFreq, float beta) {
+    filter->q[0] = 1.0f;  // qw (scalar component)
+    filter->q[1] = 0.0f;  // qx
+    filter->q[2] = 0.0f;  // qy
+    filter->q[3] = 0.0f;  // qz
+    filter->beta = beta;  // Filter gain (e.g., 0.1)
+    filter->sampleFreq = sampleFreq; // Sampling frequency in Hz
+}
+
+// Update the quaternion using accelerometer and gyroscope data
+void MadgwickFilter_Update(MadgwickFilter* filter, float gx, float gy, float gz,
+                          float ax, float ay, float az) {
+    float q[4] = {filter->q[0], filter->q[1], filter->q[2], filter->q[3]};
+    float recipNorm;
+    float s[4];
+    float qDot[4];
+    float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2, _8q1, _8q2, q0q0, q1q1, q2q2, q3q3;
+
+    // Convert gyroscope data from degrees/s to radians/s
+    gx *= 0.0174533f; // deg/s to rad/s
+    gy *= 0.0174533f;
+    gz *= 0.0174533f;
+
+    // Rate of change of quaternion from gyroscope
+    qDot[0] = 0.5f * (-q[1] * gx - q[2] * gy - q[3] * gz);
+    qDot[1] = 0.5f * (q[0] * gx + q[2] * gz - q[3] * gy);
+    qDot[2] = 0.5f * (q[0] * gy - q[1] * gz + q[3] * gx);
+    qDot[3] = 0.5f * (q[0] * gz + q[1] * gy - q[2] * gx);
+
+    // Compute accelerometer objective function and Jacobian
+    if ((ax != 0.0f) || (ay != 0.0f) || (az != 0.0f)) {
+        // Normalize accelerometer measurement
+        recipNorm = 1.0f / sqrtf(ax * ax + ay * ay + az * az);
+        ax *= recipNorm;
+        ay *= recipNorm;
+        az *= recipNorm;
+
+        // Auxiliary variables to avoid repeated calculations
+        _2q0 = 2.0f * q[0];
+        _2q1 = 2.0f * q[1];
+        _2q2 = 2.0f * q[2];
+        _2q3 = 2.0f * q[3];
+        _4q0 = 4.0f * q[0];
+        _4q1 = 4.0f * q[1];
+        _4q2 = 4.0f * q[2];
+        _8q1 = 8.0f * q[1];
+        _8q2 = 8.0f * q[2];
+        q0q0 = q[0] * q[0];
+        q1q1 = q[1] * q[1];
+        q2q2 = q[2] * q[2];
+        q3q3 = q[3] * q[3];
+
+        // Gradient descent algorithm corrective step
+        s[0] = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay;
+        s[1] = _4q1 * q3q3 - _2q3 * ax + 4.0f * q0q0 * q[1] - _2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az;
+        s[2] = 4.0f * q0q0 * q[2] + _2q0 * ax + _4q2 * q3q3 - _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az;
+        s[3] = 4.0f * q1q1 * q[3] - _2q1 * ax + 4.0f * q2q2 * q[3] - _2q2 * ay;
+
+        // Normalize step magnitude
+        recipNorm = 1.0f / sqrtf(s[0] * s[0] + s[1] * s[1] + s[2] * s[2] + s[3] * s[3]);
+        s[0] *= recipNorm;
+        s[1] *= recipNorm;
+        s[2] *= recipNorm;
+        s[3] *= recipNorm;
+
+        // Apply feedback step
+        qDot[0] -= filter->beta * s[0];
+        qDot[1] -= filter->beta * s[1];
+        qDot[2] -= filter->beta * s[2];
+        qDot[3] -= filter->beta * s[3];
+    }
+
+    // Integrate rate of change of quaternion
+    float dt = 1.0f / filter->sampleFreq;
+    q[0] += qDot[0] * dt;
+    q[1] += qDot[1] * dt;
+    q[2] += qDot[2] * dt;
+    q[3] += qDot[3] * dt;
+
+    // Normalize quaternion
+    recipNorm = 1.0f / sqrtf(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]);
+    q[0] *= recipNorm;
+    q[1] *= recipNorm;
+    q[2] *= recipNorm;
+    q[3] *= recipNorm;
+
+    // Update filter state
+    filter->q[0] = q[0];
+    filter->q[1] = q[1];
+    filter->q[2] = q[2];
+    filter->q[3] = q[3];
+}
+
+// Convert quaternion to Euler angles (roll, pitch, yaw) in degrees
+void MadgwickFilter_GetEulerAngles(MadgwickFilter* filter, float* roll, float* pitch, float* yaw) {
+    float q0 = filter->q[0];
+    float q1 = filter->q[1];
+    float q2 = filter->q[2];
+    float q3 = filter->q[3];
+
+    // Roll (x-axis rotation)
+    *roll = atan2f(2.0f * (q0 * q1 + q2 * q3), 1.0f - 2.0f * (q1 * q1 + q2 * q2)) * 57.2958f;
+
+    // Pitch (y-axis rotation)
+    float sinp = 2.0f * (q0 * q2 - q3 * q1);
+    if (fabsf(sinp) >= 1)
+        *pitch = copysignf(M_PI / 2, sinp) * 57.2958f; // Use 90 degrees if out of range
+    else
+        *pitch = asinf(sinp) * 57.2958f;
+
+    // Yaw (z-axis rotation) - Note: Without magnetometer, yaw will drift
+    *yaw = atan2f(2.0f * (q0 * q3 + q1 * q2), 1.0f - 2.0f * (q2 * q2 + q3 * q3)) * 57.2958f;
+}
+
+
+#endif // end of #ifdef MADGWICK_IMPLEMENTATION
+#endif //end of #ifndef MADGWICK_H