Fall Detection Algorithm

#include "Arduino_BMI270_BMM150.h"

// Define states
enum FallState {
  IDLE,
  SUSPECTING_FALL,
  FALL_DETECTED
};

FallState currentState = IDLE;
unsigned long suspectStartTime = 0; //timestamp (millis) of when suspect state has been entered
float accumulatedAngle = 0.0;
const float impact_Threshold = 2.5; // 2.5g
const unsigned long suspected_Fall = 1000; // 1 second in milliseconds
const float angle_Threshold = 50.0; // 50 degrees

void setup() {
  Serial.begin(115200);
  while (!Serial);
  Serial.println("Started - Detecting a Fall!");

  if (!IMU.begin()) {
    Serial.println("Failed to initialize IMU!");
    while (1);
  }
}

void loop() {
  float ax, ay, az;
  float gx, gy, gz;

  // reading accelerometer data
  if (IMU.accelerationAvailable()) {
    IMU.readAcceleration(ax, ay, az);
    
    // Calculate magnitude of acceleration vector (or check individual axes), require absolute value to capture acceleration in any diretion
    float accMagnitude = sqrt(ax*ax + ay*ay + az*az); 
    bool impactDetected =(fabs(az) > impact_Threshold) || //floating-point absolute value, returns magnitude without sign. checking in the z-direction since a fall can also be detected
                          (accMagnitude > impact_Threshold + 1.0); // define this boolean, dynamic acceleration is 2.5 + the additional 1.0 that is intrinsic to being at rest

    switch (currentState) { //state machine with idle, suspect and actual fall
      
      case IDLE:
        if (impactDetected) {
          Serial.println("Impact detected -> SUSPECTING_FALL");
          currentState = SUSPECT;
          suspectStartTime = millis();
          accumulatedAngle = 0.0; // Reset angle accumulator
        }
        break;

      case SUSPECT:
        // check if 1 second has passed
        if (millis() - suspectStartTime > suspected_Fall) { //if the time elapsed is greater than 1 second and a fall hasn't yet happened, return back to idle
          Serial.println("FSM: Timeout -> IDLE");
          currentState = IDLE;
        } 
        else {
          // Read Gyro and integrate angle
          if (IMU.gyroscopeAvailable()) {
            IMU.readGyroscope(gx, gy, gz); // but if within this 1 second, angle changes, then detects a fall
            
            // Calculate angular speed magnitude (deg/s)
            float angularSpeed = sqrt(gx*gx + gy*gy + gz*gz);
            
            // Integrate: angle = rads/s * s (100 hz = 100 cycles / s ~ period of 0.01s, so the time between two consecutive samples is 0.01s. 
            // approximate dt=
            accumulatedAngle += angularSpeed * 0.01; 

            // Check if threshold breached
            if (accumulatedAngle > angle_Threshold) {
              Serial.println("FSM: Angle threshold exceeded -> FALL DETECTED");
              currentState = FALL_DETECTED;
              
              // Output data for logging/verification
              Serial.print("Impact Time: "); Serial.println(suspectStartTime);
              Serial.print("Accumulated Angle: "); Serial.println(accumulatedAngle);
            }
          }
        }
        break;
        

      case FALL_DETECTED:
        Serial.println("!!! FALL ALERT !!!");
        delay(5000); 
        currentState = IDLE; //return to idle to continue testing
        break;
    }
  }
}