From 14e9c96f55fd04c0e8609ed1cee92e6cf3b2d067 Mon Sep 17 00:00:00 2001
From: Nik Rozman <admin@piskot.si>
Date: Tue, 24 Mar 2026 23:29:34 +0100
Subject: [PATCH] Improve sleep and add button waking

---
 source/sleep.cpp | 36 ++++++++++++++++++++++++++++++++----
 source/sleep.h   |  4 ++--
 2 files changed, 34 insertions(+), 6 deletions(-)

diff --git a/source/sleep.cpp b/source/sleep.cpp
index 53b60c0..1ae4630 100644
--- a/source/sleep.cpp
+++ b/source/sleep.cpp
@@ -66,6 +66,11 @@ static void imuInt1ISR() {
   imuWakeFlag = true;
 }
 
+static volatile bool btnWakeFlag = false;
+static void btnWakeISR() {
+  btnWakeFlag = true;
+}
+
 // Arm wakeup interrupt 
 static void armWakeupInterrupt() {
   lsmWrite(SLP_WAKE_UP_DUR, (uint8_t)((SLEEP_WAKEUP_DUR & 0x03) << 4));
@@ -127,6 +132,15 @@ static void enterImuLP() {
 
   armWakeupInterrupt();
 
+  // Arm button wake interrupt
+  #if BTN_LEFT_PIN != BTN_PIN_NONE
+    btnWakeFlag = false;
+    attachInterrupt(digitalPinToInterrupt(BTN_LEFT_PIN), btnWakeISR, FALLING);
+  #endif
+
+  // Turn off all LEDs for sleep
+  digitalWrite(LED_RED, HIGH); digitalWrite(LED_GREEN, HIGH); digitalWrite(LED_BLUE, HIGH);
+
   lpEnteredMs = millis();
   sleepStage = SLEEP_IMU_LP;
   Serial.print("[SLEEP] IMU LP entered - idle for ");
@@ -153,9 +167,9 @@ static void enterDeepSleep() {
   Serial.println("[SLEEP] Deep sleep - WFE on INT1");
   Serial.flush();
 
-  digitalWrite(LED_RED, LOW); delay(80); digitalWrite(LED_RED, HIGH);
+  digitalWrite(LED_RED, HIGH); digitalWrite(LED_GREEN, HIGH); digitalWrite(LED_BLUE, HIGH);
 
-  while (!imuWakeFlag) {
+  while (!imuWakeFlag && !btnWakeFlag) {
     (void)lsmRead(SLP_WAKE_UP_SRC);
     sd_app_evt_wait();
   }
@@ -172,6 +186,11 @@ void sleepManagerWakeIMU() {
 
   disarmWakeupInterrupt();
 
+  // Detach button wake interrupt — normal polling takes over
+  #if BTN_LEFT_PIN != BTN_PIN_NONE
+    detachInterrupt(digitalPinToInterrupt(BTN_LEFT_PIN));
+  #endif
+
   // Only recalibrate if gyro was off long enough for thermal drift to accumulate,
   // or if waking from full deep sleep. Short LP naps reuse the existing bias.
   unsigned long lpDuration = millis() - lpEnteredMs;
@@ -225,14 +244,23 @@ void sleepManagerInit() {
 // Returns true → caller must skip IMU reads this iteration.
 bool sleepManagerUpdate(unsigned long nowMs, bool idle, bool bleConnected) {
 
-  // ISR wakeup 
+  // ISR wakeup (IMU motion or button press)
+  bool woke = false;
   if (imuWakeFlag) {
     imuWakeFlag = false;
     Serial.print("[SLEEP] INT1 fired - stage="); Serial.println((int)sleepStage);
+    woke = true;
+  }
+  if (btnWakeFlag) {
+    btnWakeFlag = false;
+    Serial.print("[SLEEP] Button fired - stage="); Serial.println((int)sleepStage);
+    woke = true;
+  }
+  if (woke) {
     if (sleepStage == SLEEP_DEEP || sleepStage == SLEEP_IMU_LP) {
       sleepManagerWakeIMU();
     } else {
-      (void)lsmRead(SLP_WAKE_UP_SRC);  // normal-mode edge, clear latch only
+      (void)lsmRead(SLP_WAKE_UP_SRC);
     }
   }
 
diff --git a/source/sleep.h b/source/sleep.h
index a261edb..b7815bb 100644
--- a/source/sleep.h
+++ b/source/sleep.h
@@ -11,12 +11,12 @@
 
 // LSM6DS3 wakeup threshold: 1 LSB = 7.8 mg at ±2 g FS (±2g range).
 #ifndef SLEEP_WAKEUP_THS
-  #define SLEEP_WAKEUP_THS   16    // 0–63
+  #define SLEEP_WAKEUP_THS   6    // 0–63
 #endif
 
 // Number of consecutive 26 Hz samples that must exceed the threshold.
 #ifndef SLEEP_WAKEUP_DUR
-  #define SLEEP_WAKEUP_DUR   2    // 0–3
+  #define SLEEP_WAKEUP_DUR   1    // 0–3
 #endif
 
 // GPIO pin connected to LSM6DS3 INT1.