final_project_537_demo_host.txt

#include <LSM6DS3.h>
#include <Wire.h>
#include <lstm_48_2_q.h>

LSM6DS3 myIMU(I2C_MODE, 0x6A);  // I2C address for LSM6DS3

#define CONVERT_G_TO_MS2 9.80665f

// === Adjustable parameters ===
const int SAMPLE_RATE_HZ = 60;     // Samples per second
const int NUM_SAMPLES = 60;        // Total samples to record (1 second)
const float SAMPLE_INTERVAL_MS = 1000.0 / SAMPLE_RATE_HZ;

float meanX = 0.0f, meanY = 0.0f, meanZ = 0.0f;
const float MEAN_ALPHA = 0.02f;    // smoothing factor

const float b0 = 0.13110644f;
const float b1 = 0.26221288f;
const float b2 = 0.13110644f;

const float a1 = -0.74778918f;
const float a2 =  0.27221494f;

// delay states per axis (DF2T)
float s1x = 0, s2x = 0;
float s1y = 0, s2y = 0;
float s1z = 0, s2z = 0;
// ==============================

inline float butterworth_filter(float x, float &s1, float &s2) {
  float y = b0 * x + s1;
  s1 = b1 * x - a1 * y + s2;
  s2 = b2 * x - a2 * y;
  return y;
}

void setup() {
  Serial.begin(115200);
  while (!Serial)
    ;

  if (myIMU.begin() != 0) {
    Serial.println("Device error");
    while (1);
  } else {
    Serial.println("Device OK!");
  }

  Serial.println("Ready");
}

void loop() {
  // Wait for serial command 's' to start recording
  if (Serial.available()) {
    char command = Serial.read();
    if (command == 's' || command == 'S') {
      recordMotion();
    }
  }
}

// ===========================================
// Function to record accelerometer data
// ===========================================
void recordMotion() {
  Serial.println("Recording");

  unsigned long lastSampleTime = millis();
  float ax, ay, az;

  for (int i = 0; i < NUM_SAMPLES; i++) {
    ax = myIMU.readFloatAccelX() * CONVERT_G_TO_MS2;
    ay = myIMU.readFloatAccelY() * CONVERT_G_TO_MS2;
    az = myIMU.readFloatAccelZ() * CONVERT_G_TO_MS2;

    // ---------------------------------------------------------
    // 1) Exponential running mean (online DC removal)
    // ---------------------------------------------------------
    meanX += MEAN_ALPHA * (ax - meanX);
    meanY += MEAN_ALPHA * (ay - meanY);
    meanZ += MEAN_ALPHA * (az - meanZ);

    float ax_demean = ax - meanX;
    float ay_demean = ay - meanY;
    float az_demean = az - meanZ;

    // ---------------------------------------------------------
    // 2) Apply 2nd-order Butterworth filter
    // ---------------------------------------------------------
    float fx = butterworth_filter(ax_demean, s1x, s2x);
    float fy = butterworth_filter(ay_demean, s1y, s2y);
    float fz = butterworth_filter(az_demean, s1z, s2z);

    Serial.print(fx, 6);
    Serial.print('\t');
    Serial.print(fy, 6);
    Serial.print('\t');
    Serial.println(fz, 6);

    // Wait for next sample
    while (millis() - lastSampleTime < SAMPLE_INTERVAL_MS) {
      // passive wait
    }
    lastSampleTime += SAMPLE_INTERVAL_MS;
  }

  Serial.println("Done");
  Serial.println("Ready");
}