PS_BLE_Tracker/Arduino/PS_BLE_ShotDetection/PS_BLE_ShotDetection.ino

/*
 * XIAO nRF52840 Sense - Airsoft Tracker Pro v3.2
 * Microphone PDM natif (pas analogRead !)
 * Détection multi-capteurs : Accel + Gyro + Micro PDM
 */

#include <Arduino.h>
#include <ArduinoBLE.h>
#include <LSM6DS3.h>
#include <Wire.h>
#include <PDM.h>          // ← Bibliothèque PDM intégrée nRF52840

// ====== PINS LED ======
#define LED_RED    11
#define LED_GREEN  12
#define LED_BLUE   13

// ====== FLASH ======
#define FLASH_STORAGE_START  0x7F000   // Pairing MAC
#define FLASH_CONFIG_START   0x7E000   // ShotConfig persistée
#define CONFIG_MAGIC_NUMBER  0xDEADBEEF

// ====== ENUMS (déclarés en premier) ======
enum LedState {
  LED_BOOT, LED_NO_PAIRING, LED_ADVERTISING,
  LED_CONNECTED, LED_SHOT_FLASH, LED_REJECTED, LED_ERROR
};

enum DebugMode {
  DEBUG_OFF, DEBUG_RAW, DEBUG_TRIGGERS, DEBUG_FULL
};

// ====== STRUCTURES ======
struct PairingData {
  uint32_t magic;
  char     authorizedMAC[18];
  uint8_t  reserved[10];
};

struct ShotConfig {
  float    accelThreshold;   // G
  float    gyroThreshold;    // deg/s
  uint16_t audioThreshold;   // 0-32767 (PDM 16-bit)
  uint16_t accelWindow;      // ms
  uint16_t gyroWindow;       // ms
  uint16_t audioWindow;      // ms
  uint8_t  minSensors;       // 1-3
  uint16_t shotCooldown;     // ms
  bool     useAccel;
  bool     useGyro;
  bool     useAudio;
};

// ====== VARIABLES GLOBALES ======
PairingData pairing;
ShotConfig  shotConfig;
const uint32_t MAGIC_NUMBER = 0xCAFEBABE;

// IMU
LSM6DS3 imu(I2C_MODE, 0x6A);
float ax, ay, az, gx, gy, gz;
float roll=0, pitch=0, yaw=0;
unsigned long lastUpdate=0, lastSend=0;
const float alpha = 0.98f;

// ====== PDM MICROPHONE ======
#define PDM_BUFFER_SIZE 256
short pdmBuffer[PDM_BUFFER_SIZE];
volatile int  pdmSamplesReady = 0;
volatile int16_t pdmPeak      = 0;   // Pic absolu du dernier buffer

void onPDMdata() {
  int bytesAvailable = PDM.available();
  PDM.read(pdmBuffer, bytesAvailable);
  int samples = bytesAvailable / 2;

  // Calculer le pic absolu dans ce buffer
  int16_t peak = 0;
  for (int i = 0; i < samples; i++) {
    int16_t v = abs(pdmBuffer[i]);
    if (v > peak) peak = v;
  }
  pdmPeak = peak;
  pdmSamplesReady = 1;
}

// Valeur audio lissée pour debug
float audioSmoothed = 0;

// ====== DÉTECTION TIR ======
unsigned long lastShotTime   = 0;
bool accelTrigger = false;
bool gyroTrigger  = false;
bool audioTrigger = false;
unsigned long accelTriggerTime = 0;
unsigned long gyroTriggerTime  = 0;
unsigned long audioTriggerTime = 0;

// ====== LED ======
LedState      currentLedState = LED_BOOT;
unsigned long ledTimer        = 0;
unsigned long shotFlashTimer  = 0;
unsigned long connectionTime  = 0;
bool          ledBlinkState   = false;
bool          isAdvertising   = false;

// ====== DEBUG ======
DebugMode     debugMode     = DEBUG_OFF;
unsigned long lastDebugSend = 0;
const uint16_t DEBUG_RATE   = 50;  // 20 Hz

// ====== UUIDs BLE ======
const char* SERVICE_UUID     = "6E400001-B5A3-F393-E0A9-E50E24DCCA9E";
const char* IMU_CHAR_UUID    = "6E400002-B5A3-F393-E0A9-E50E24DCCA9E";
const char* SHOT_CHAR_UUID   = "6E400004-B5A3-F393-E0A9-E50E24DCCA9E";
const char* DEBUG_CHAR_UUID  = "6E400005-B5A3-F393-E0A9-E50E24DCCA9E";
const char* CONFIG_CHAR_UUID = "6E400006-B5A3-F393-E0A9-E50E24DCCA9E";

// ====== BLE ======
BLEService        motionService(SERVICE_UUID);
BLECharacteristic imuChar    (IMU_CHAR_UUID,    BLERead|BLENotify, 12, true);
BLECharacteristic shotChar   (SHOT_CHAR_UUID,   BLERead|BLENotify,  1, true);
BLECharacteristic debugChar  (DEBUG_CHAR_UUID,  BLERead|BLENotify, 20, true);
BLECharacteristic configChar (CONFIG_CHAR_UUID, BLERead|BLEWrite,  32, true);

// ════════════════════════════════════════════════
// LED
// ════════════════════════════════════════════════
void setLED(uint8_t r, uint8_t g, uint8_t b) {
  digitalWrite(LED_RED,   r ? LOW : HIGH);
  digitalWrite(LED_GREEN, g ? LOW : HIGH);
  digitalWrite(LED_BLUE,  b ? LOW : HIGH);
}

void changeLedState(LedState s) {
  currentLedState = s;
  ledTimer = millis();
  ledBlinkState = false;
}

void updateLED() {
  unsigned long now = millis();
  if (currentLedState == LED_SHOT_FLASH) {
    if (now - shotFlashTimer < 60) { setLED(255,0,0); return; }
    else currentLedState = LED_CONNECTED;
  }
  switch (currentLedState) {
    case LED_BOOT:
      if (now-ledTimer>200){ ledTimer=now; ledBlinkState=!ledBlinkState;
        setLED(ledBlinkState?128:0, 0, ledBlinkState?128:0); } break;
    case LED_NO_PAIRING:
      if (now-ledTimer>1000){ ledTimer=now; ledBlinkState=!ledBlinkState;
        setLED(ledBlinkState?255:0, ledBlinkState?200:0, 0); } break;
    case LED_ADVERTISING:
      if (now-ledTimer>500){ ledTimer=now; ledBlinkState=!ledBlinkState;
        setLED(0,0,ledBlinkState?255:0); } break;
    case LED_CONNECTED: setLED(0,255,0); break;
    case LED_REJECTED:
      if (now-ledTimer>150){ ledTimer=now; ledBlinkState=!ledBlinkState;
        setLED(ledBlinkState?255:0,0,0); } break;
    case LED_ERROR: setLED(255,0,0); break;
    default: break;
  }
}

// ════════════════════════════════════════════════
// CONFIG
// ════════════════════════════════════════════════
void initDefaultConfig() {
  shotConfig.accelThreshold = 2.5f;
  shotConfig.gyroThreshold  = 200.0f;
  shotConfig.audioThreshold = 3000;    // Seuil PDM (0-32767)
  shotConfig.accelWindow    = 60;   // augmenté pour visibilité debug (>DEBUG_RATE 50ms)
  shotConfig.gyroWindow     = 60;   // augmenté pour visibilité debug (>DEBUG_RATE 50ms)
  shotConfig.audioWindow    = 60;   // augmenté pour visibilité debug (>DEBUG_RATE 50ms)
  shotConfig.minSensors     = 3;
  shotConfig.shotCooldown   = 80;
  shotConfig.useAccel       = true;
  shotConfig.useGyro        = true;
  shotConfig.useAudio       = true;
}

void printConfig() {
  Serial.println("\n╔══════════ CONFIG ══════════╗");
  Serial.print("Accel  : "); Serial.print(shotConfig.accelThreshold,1);
  Serial.print(" G   ["); Serial.print(shotConfig.useAccel?"ON":"OFF"); Serial.println("]");
  Serial.print("Gyro   : "); Serial.print(shotConfig.gyroThreshold,0);
  Serial.print(" °/s ["); Serial.print(shotConfig.useGyro?"ON":"OFF"); Serial.println("]");
  Serial.print("Audio  : "); Serial.print(shotConfig.audioThreshold);
  Serial.print(" PDM ["); Serial.print(shotConfig.useAudio?"ON":"OFF"); Serial.println("]");
  Serial.print("MinSens: "); Serial.println(shotConfig.minSensors);
  Serial.print("Cooldown: "); Serial.print(shotConfig.shotCooldown); Serial.println(" ms");
  Serial.println("╚════════════════════════════╝\n");
}

// ════════════════════════════════════════════════
// FLASH
// ════════════════════════════════════════════════
void loadPairingData() {
  memcpy(&pairing, (void*)FLASH_STORAGE_START, sizeof(PairingData));
  if (pairing.magic == MAGIC_NUMBER) {
    Serial.print("🔒 PC : "); Serial.println(pairing.authorizedMAC);
    changeLedState(LED_ADVERTISING);
  } else {
    Serial.println("🔓 Aucun PC enregistré");
    pairing.magic = 0;
    memset(pairing.authorizedMAC, 0, sizeof(pairing.authorizedMAC));
    changeLedState(LED_NO_PAIRING);
  }
}

void savePairingData(const char* mac) {
  pairing.magic = MAGIC_NUMBER;
  strncpy(pairing.authorizedMAC, mac, 17);
  pairing.authorizedMAC[17] = '\0';
  NRF_NVMC->CONFIG = NVMC_CONFIG_WEN_Een; while(NRF_NVMC->READY==NVMC_READY_READY_Busy){}
  NRF_NVMC->ERASEPAGE = FLASH_STORAGE_START; while(NRF_NVMC->READY==NVMC_READY_READY_Busy){}
  NRF_NVMC->CONFIG = NVMC_CONFIG_WEN_Wen; while(NRF_NVMC->READY==NVMC_READY_READY_Busy){}
  uint32_t* src=(uint32_t*)&pairing, *dst=(uint32_t*)FLASH_STORAGE_START;
  for(int i=0;i<(int)(sizeof(PairingData)/4);i++){
    dst[i]=src[i]; while(NRF_NVMC->READY==NVMC_READY_READY_Busy){}
  }
  NRF_NVMC->CONFIG = NVMC_CONFIG_WEN_Ren; while(NRF_NVMC->READY==NVMC_READY_READY_Busy){}
  Serial.print("💾 "); Serial.println(pairing.authorizedMAC);
}

void resetPairing() {
  pairing.magic = 0; memset(pairing.authorizedMAC,0,sizeof(pairing.authorizedMAC));
  NRF_NVMC->CONFIG=NVMC_CONFIG_WEN_Een; while(NRF_NVMC->READY==NVMC_READY_READY_Busy){}
  NRF_NVMC->ERASEPAGE=FLASH_STORAGE_START; while(NRF_NVMC->READY==NVMC_READY_READY_Busy){}
  NRF_NVMC->CONFIG=NVMC_CONFIG_WEN_Ren; while(NRF_NVMC->READY==NVMC_READY_READY_Busy){}
  Serial.println("🗑️ Reset"); changeLedState(LED_NO_PAIRING);
}

bool isAuthorized(const char* mac) {
  if (pairing.magic != MAGIC_NUMBER) { savePairingData(mac); return true; }
  return (strcmp(pairing.authorizedMAC, mac) == 0);
}

// ════════════════════════════════════════════════
// CONFIG FLASH (page 0x7E000)
// ════════════════════════════════════════════════
struct StoredConfig {
  uint32_t  magic;
  ShotConfig config;
  uint8_t   reserved[28];  // padding pour aligner sur 4 bytes
};

void saveConfigToFlash() {
  StoredConfig sc;
  sc.magic = CONFIG_MAGIC_NUMBER;
  memcpy(&sc.config, &shotConfig, sizeof(ShotConfig));
  memset(sc.reserved, 0xFF, sizeof(sc.reserved));

  NRF_NVMC->CONFIG = NVMC_CONFIG_WEN_Een; while(NRF_NVMC->READY==NVMC_READY_READY_Busy){}
  NRF_NVMC->ERASEPAGE = FLASH_CONFIG_START; while(NRF_NVMC->READY==NVMC_READY_READY_Busy){}
  NRF_NVMC->CONFIG = NVMC_CONFIG_WEN_Wen; while(NRF_NVMC->READY==NVMC_READY_READY_Busy){}
  uint32_t* src = (uint32_t*)&sc;
  uint32_t* dst = (uint32_t*)FLASH_CONFIG_START;
  for (int i = 0; i < (int)(sizeof(StoredConfig)/4); i++) {
    dst[i] = src[i]; while(NRF_NVMC->READY==NVMC_READY_READY_Busy){}
  }
  NRF_NVMC->CONFIG = NVMC_CONFIG_WEN_Ren; while(NRF_NVMC->READY==NVMC_READY_READY_Busy){}
  Serial.println("💾 Config sauvegardée en flash");
}

void loadConfigFromFlash() {
  StoredConfig* sc = (StoredConfig*)FLASH_CONFIG_START;
  if (sc->magic == CONFIG_MAGIC_NUMBER) {
    memcpy(&shotConfig, &sc->config, sizeof(ShotConfig));
    Serial.println("📂 Config chargée depuis flash");
  } else {
    Serial.println("📂 Pas de config flash → valeurs par défaut");
    initDefaultConfig();
  }
}

// ════════════════════════════════════════════════
// CALLBACKS BLE
// ════════════════════════════════════════════════
void onConnect(BLEDevice central) {
  Serial.print("🔗 "); Serial.println(central.address());
  if (!isAuthorized(central.address().c_str())) {
    Serial.println("❌ REFUSÉ");
    changeLedState(LED_REJECTED); delay(3000);
    central.disconnect(); changeLedState(LED_ADVERTISING); return;
  }
  Serial.println("✅ CONNECTÉ");
  changeLedState(LED_CONNECTED);
  isAdvertising=false; lastUpdate=connectionTime=millis();
}

void onDisconnect(BLEDevice central) {
  Serial.print("❌ DÉCO (");
  Serial.print((millis()-connectionTime)/1000);
  Serial.println("s)");
  changeLedState(LED_ADVERTISING); isAdvertising=true;
}

void onConfigWrite(BLEDevice central, BLECharacteristic c) {
  uint8_t data[32];
  int len = c.readValue(data, 32);
  if (len >= (int)sizeof(ShotConfig)) {
    memcpy(&shotConfig, data, sizeof(ShotConfig));
    Serial.println("⚙️ Config reçue"); printConfig();
    saveConfigToFlash();
  }
  // Octet optionnel : debug mode à l'offset fixe 28 (si != 0xFF)
  // Offset fixe 28 = après toute ShotConfig + padding, dans le zone de padding 32 bytes
  const int debugByteIdx = 28;
  if (len > debugByteIdx && data[debugByteIdx] != 0xFF) {
    uint8_t dm = data[debugByteIdx] & 0x03;  // 0-3
    debugMode = (DebugMode)dm;
    Serial.print("🐛 Debug BLE → ");
    switch(debugMode){
      case DEBUG_OFF:      Serial.println("OFF"); break;
      case DEBUG_RAW:      Serial.println("RAW"); break;
      case DEBUG_TRIGGERS: Serial.println("TRIGGERS"); break;
      case DEBUG_FULL:     Serial.println("FULL"); break;
    }
  }
}

// ════════════════════════════════════════════════
// DÉTECTION TIR
// ════════════════════════════════════════════════
bool checkWindow(unsigned long t, uint16_t w) {
  return (millis()-t) <= w;
}

bool detectShot() {
  unsigned long now = millis();
  if ((now-lastShotTime) < shotConfig.shotCooldown) return false;
  if (accelTrigger && !checkWindow(accelTriggerTime, shotConfig.accelWindow)) accelTrigger=false;
  if (gyroTrigger  && !checkWindow(gyroTriggerTime,  shotConfig.gyroWindow))  gyroTrigger=false;
  if (audioTrigger && !checkWindow(audioTriggerTime,  shotConfig.audioWindow)) audioTrigger=false;
  uint8_t n=0;
  if (accelTrigger && shotConfig.useAccel) n++;
  if (gyroTrigger  && shotConfig.useGyro)  n++;
  if (audioTrigger && shotConfig.useAudio) n++;
  if (n >= shotConfig.minSensors) {
    accelTrigger=gyroTrigger=audioTrigger=false; return true;
  }
  return false;
}

// ════════════════════════════════════════════════
// DEBUG
// ════════════════════════════════════════════════
void sendDebugData(float accelMag, float gyroMag, uint16_t audioLevel) {
  if (debugMode==DEBUG_OFF || !BLE.connected()) return;
  unsigned long now=millis();
  if ((now-lastDebugSend)<DEBUG_RATE) return;
  lastDebugSend=now;
  uint8_t buf[20];
  buf[0]=(uint8_t)debugMode;
  memcpy(&buf[1],&accelMag,4);
  memcpy(&buf[5],&gyroMag,4);
  memcpy(&buf[9],&audioLevel,2);
  buf[11]=accelTrigger?1:0;
  buf[12]=gyroTrigger?1:0;
  buf[13]=audioTrigger?1:0;
  debugChar.writeValue(buf,14);
}

void printDebugSerial(float accelMag, float gyroMag, uint16_t audioLevel) {
  if (debugMode==DEBUG_OFF) return;
  static unsigned long lp=0;
  if ((millis()-lp)<100) return; lp=millis();
  if (debugMode==DEBUG_RAW||debugMode==DEBUG_FULL) {
    Serial.print("A:"); Serial.print(accelMag,2);
    Serial.print(" G:"); Serial.print(gyroMag,1);
    Serial.print(" M:"); Serial.print(audioLevel);
    Serial.print(" | ");
  }
  if (debugMode==DEBUG_TRIGGERS||debugMode==DEBUG_FULL) {
    Serial.print("[");
    Serial.print(accelTrigger?"A":"-");
    Serial.print(gyroTrigger?"G":"-");
    Serial.print(audioTrigger?"M":"-");
    Serial.print("]");
  }
  Serial.println();
}

// ════════════════════════════════════════════════
// COMMANDES SÉRIE
// ════════════════════════════════════════════════
void handleSerialCommand() {
  if (!Serial.available()) return;
  char cmd = Serial.read();
  switch(cmd) {
    case 'R': case 'r': resetPairing(); break;
    case 'C': case 'c': printConfig(); break;
    case 'D': case 'd':
      debugMode=(DebugMode)((debugMode+1)%4);
      Serial.print("Debug : ");
      switch(debugMode){
        case DEBUG_OFF:      Serial.println("OFF"); break;
        case DEBUG_RAW:      Serial.println("RAW"); break;
        case DEBUG_TRIGGERS: Serial.println("TRIGGERS"); break;
        case DEBUG_FULL:     Serial.println("FULL"); break;
      } break;
    case 'T': case 't':
      Serial.println("💥 TEST TIR");
      shotChar.writeValue((uint8_t)1);
      shotFlashTimer=millis(); changeLedState(LED_SHOT_FLASH); break;
    case '1': shotConfig.useAccel=!shotConfig.useAccel;
      Serial.print("Accel:"); Serial.println(shotConfig.useAccel?"ON":"OFF"); break;
    case '2': shotConfig.useGyro=!shotConfig.useGyro;
      Serial.print("Gyro:"); Serial.println(shotConfig.useGyro?"ON":"OFF"); break;
    case '3': shotConfig.useAudio=!shotConfig.useAudio;
      Serial.print("Audio:"); Serial.println(shotConfig.useAudio?"ON":"OFF"); break;
    case '+': shotConfig.accelThreshold+=0.1f;
      Serial.print("Accel:"); Serial.println(shotConfig.accelThreshold,1); break;
    case '-': shotConfig.accelThreshold=max(0.3f,shotConfig.accelThreshold-0.1f);
      Serial.print("Accel:"); Serial.println(shotConfig.accelThreshold,1); break;
    case 'G': shotConfig.gyroThreshold+=10.0f;
      Serial.print("Gyro:"); Serial.println(shotConfig.gyroThreshold,0); break;
    case 'g': shotConfig.gyroThreshold=max(20.0f,shotConfig.gyroThreshold-10.0f);
      Serial.print("Gyro:"); Serial.println(shotConfig.gyroThreshold,0); break;
    case 'M': shotConfig.audioThreshold+=200;
      Serial.print("Audio PDM:"); Serial.println(shotConfig.audioThreshold); break;
    case 'n': shotConfig.audioThreshold=max((uint16_t)200,
                (uint16_t)(shotConfig.audioThreshold-200));
      Serial.print("Audio PDM:"); Serial.println(shotConfig.audioThreshold); break;
    case 'S': case 's':
      shotConfig.minSensors++; if(shotConfig.minSensors>3) shotConfig.minSensors=1;
      Serial.print("MinSensors:"); Serial.println(shotConfig.minSensors); break;
    case 'H': case 'h':
      Serial.println("\n╔═════ COMMANDES ═════╗");
      Serial.println("R  : Reset pairing");
      Serial.println("C  : Config");
      Serial.println("D  : Debug cycle");
      Serial.println("T  : Test tir");
      Serial.println("1/2/3 : Toggle A/G/M");
      Serial.println("+/- : Accel ±0.1G");
      Serial.println("G/g : Gyro ±10°/s");
      Serial.println("M/n : Audio PDM ±200");
      Serial.println("S  : Min capteurs");
      Serial.println("╚═════════════════════╝\n"); break;
    default: break;
  }
}

// ════════════════════════════════════════════════
// SETUP
// ════════════════════════════════════════════════
void setup() {
  Serial.begin(115200);
  delay(2000);

  Serial.println("\n╔══════════════════════════════════════════╗");
  Serial.println("║  XIAO Airsoft Tracker Pro v3.2          ║");
  Serial.println("║  Accel + Gyro + Micro PDM natif         ║");
  Serial.println("╚══════════════════════════════════════════╝\n");

  pinMode(LED_RED,  OUTPUT);
  pinMode(LED_GREEN,OUTPUT);
  pinMode(LED_BLUE, OUTPUT);
  changeLedState(LED_BOOT);

  loadConfigFromFlash();  // charge depuis flash ou valeurs par défaut
  loadPairingData();

  // IMU
  Serial.print("🔧 IMU... ");
  if (imu.begin()!=0) {
    Serial.println("❌"); changeLedState(LED_ERROR);
    while(1){updateLED();delay(1);}
  }
  Serial.println("✅");

  // PDM Microphone
  Serial.print("🎙️  PDM Micro... ");
  PDM.onReceive(onPDMdata);
  // Mono, 16000 Hz (standard nRF52840)
  if (!PDM.begin(1, 16000)) {
    Serial.println("❌ PDM init failed");
    // On continue sans micro plutôt que de bloquer
    shotConfig.useAudio = false;
    Serial.println("   Audio désactivé");
  } else {
    Serial.println("✅ (16kHz mono)");
    PDM.setGain(20);  // Gain réduit (0-80) — augmentez si signal trop faible
  }

  // BLE
  Serial.print("📡 BLE... ");
  if (!BLE.begin()) {
    Serial.println("❌"); changeLedState(LED_ERROR);
    while(1){updateLED();delay(1);}
  }
  Serial.println("✅");
  delay(500);

  BLE.setDeviceName("XIAO Airsoft Pro");
  BLE.setLocalName("XIAO Airsoft Pro");
  BLE.setConnectionInterval(0x0006, 0x0C80);
  BLE.setAdvertisedService(motionService);
  motionService.addCharacteristic(imuChar);
  motionService.addCharacteristic(shotChar);
  motionService.addCharacteristic(debugChar);
  motionService.addCharacteristic(configChar);
  BLE.addService(motionService);

  uint8_t z[12]={0};
  imuChar.writeValue(z,12);
  shotChar.writeValue((uint8_t)0);

  configChar.setEventHandler(BLEWritten,    onConfigWrite);
  BLE.setEventHandler(BLEConnected,         onConnect);
  BLE.setEventHandler(BLEDisconnected,      onDisconnect);

  BLE.advertise();
  isAdvertising=true;

  Serial.println("✅ Prêt !");
  printConfig();
  Serial.println("Tapez 'H' pour l'aide");
  Serial.println("⚠️  Seuil audio PDM : 0-32767 (ajuster avec M/n)\n");
}

// ════════════════════════════════════════════════
// LOOP
// ════════════════════════════════════════════════
void loop() {
  BLE.poll();
  updateLED();
  handleSerialCommand();

  if (!BLE.connected()) return;

  unsigned long now = millis();

  // ─── Lecture IMU ───
  ax = imu.readFloatAccelX();
  ay = imu.readFloatAccelY();
  az = imu.readFloatAccelZ();
  gx = imu.readFloatGyroX() * DEG_TO_RAD;
  gy = imu.readFloatGyroY() * DEG_TO_RAD;
  gz = imu.readFloatGyroZ() * DEG_TO_RAD;

  float accelMag = sqrt(ax*ax + ay*ay + az*az);
  float gyroMag  = sqrt(gx*gx + gy*gy + gz*gz) * RAD_TO_DEG;

  // ─── Lecture PDM ───
  // pdmPeak est mis à jour par l'ISR onPDMdata()
  uint16_t audioLevel = (uint16_t)pdmPeak;

  // Lissage pour affichage debug
  audioSmoothed = 0.7f * audioSmoothed + 0.3f * audioLevel;

  // ─── Triggers ───
  unsigned long n = millis();
  if (shotConfig.useAccel && accelMag>=shotConfig.accelThreshold && !accelTrigger) {
    accelTrigger=true; accelTriggerTime=n; }
  if (shotConfig.useGyro  && gyroMag>=shotConfig.gyroThreshold   && !gyroTrigger) {
    gyroTrigger=true;  gyroTriggerTime=n; }
  if (shotConfig.useAudio && audioLevel>=shotConfig.audioThreshold && !audioTrigger) {
    audioTrigger=true; audioTriggerTime=n; }

  // ─── Détection tir ───
  if (detectShot()) {
    lastShotTime=now;
    shotChar.writeValue((uint8_t)1);
    shotFlashTimer=now; changeLedState(LED_SHOT_FLASH);
    Serial.print("💥 TIR !  A:");Serial.print(accelMag,1);
    Serial.print(" G:");Serial.print(gyroMag,0);
    Serial.print(" M:");Serial.println(audioLevel);
  }

  // ─── Debug ───
  sendDebugData(accelMag, gyroMag, audioLevel);
  printDebugSerial(accelMag, gyroMag, audioLevel);

  // ─── IMU BLE 10 Hz (debug uniquement) ───
  // En production (debugMode OFF), pas d'envoi IMU → charge BLE réduite pour Unreal
  if (debugMode != DEBUG_OFF && now-lastSend>=100) {
    lastSend=now;
    float dt=(now-lastUpdate)/1000.0f; lastUpdate=now;
    roll+=gx*dt; pitch+=gy*dt; yaw+=gz*dt;
    float ar=atan2f(ay,az);
    float ap=atan2f(-ax,sqrtf(ay*ay+az*az));
    roll =alpha*roll +(1-alpha)*ar;
    pitch=alpha*pitch+(1-alpha)*ap;
    uint8_t payload[12];
    memcpy(&payload[0],&roll,4);
    memcpy(&payload[4],&pitch,4);
    memcpy(&payload[8],&yaw,4);
    imuChar.writeValue(payload,12);
  }
}