Refactor ESC LED/tone handling and status lights

zjwhitehead · zjwhitehead · commit 278a329bb216 · 2026-04-10T11:30:27.000-05:00
Introduce EscStatusLightMode and new APIs to request ESC status light mode and queue motor beeps; centralize ESC LED/tone logic in esc.cpp (syncEscOutputs, sendEscStatusLight, pending tone queue) and call it from readESCTelemetry. Rename and simplify motor-sound API (setESCMotorSound signature changed) and replace immediate beep/LED task functions with queue/request versions (startESCLedStrobeTest, escMotorBeepArm/Disarm removed). Rename NEOPIXEL_* color macros to STATUS_LED_* and update usages in main.cpp. Update main to request ESC status light mode based on connection, battery/OTA/state and to queue arm/disarm beeps. Also bump SINE-ESC-CAN dependency reference in platformio.ini to a newer commit.
diff --git a/inc/sp140/esc.h b/inc/sp140/esc.h
@@ -15,6 +15,13 @@ inline bool isMotorTempValidC(float tempC) {
 #include <SineEsc.h>
 #include <CanardAdapter.h>
 
+enum class EscStatusLightMode : uint8_t {
+  OFF = 0,
+  READY,
+  FLIGHT,
+  CAUTION,
+};
+
 void initESC();
 void setESCThrottle(int throttlePWM);
 void readESCTelemetry();
@@ -60,12 +67,12 @@ bool hasBootloaderBadError(uint16_t errorCode);
 
 // ESC LED control
 void setESCLedControl(const uint16_t *ledData, uint8_t ledNum);
-void startESCLedStrobeTest();
+void requestEscStatusLightMode(EscStatusLightMode mode);
 
 // ESC motor beep
-void setESCMotorSound(const uint8_t *beepData, uint8_t beepDataLen, uint8_t beepNum);
-void escMotorBeepArm();
-void escMotorBeepDisarm();
+void setESCMotorSound(const uint8_t *beepData, uint8_t beepNum);
+void queueEscMotorBeepArm();
+void queueEscMotorBeepDisarm();
 
 // for debugging
 void dumpThrottleResponse(const sine_esc_SetThrottleSettings2Response *res);
diff --git a/inc/sp140/utilities.h b/inc/sp140/utilities.h
@@ -6,17 +6,12 @@
 // Function to get unique chip ID
 String chipId();
 
-// Definitions for main rainbow colors in WRGB format for NeoPixel.
-// The 32-bit color value is WRGB. W (White) is ignored for RGB pixels.
-// The next bytes are R (Red), G (Green), and B (Blue).
-// For example, YELLOW is 0x00FFFF00, with FF for Red and Green, and 00 for Blue.
-
-#define NEOPIXEL_RED 0x00FF0000
-#define NEOPIXEL_ORANGE 0x00FF7F00
-#define NEOPIXEL_YELLOW 0x00FFFF00
-#define NEOPIXEL_GREEN 0x0000FF00
-#define NEOPIXEL_BLUE 0x000000FF
-#define NEOPIXEL_INDIGO 0x004B0082
-#define NEOPIXEL_VIOLET 0x008000FF
-
-#endif  // INC_SP140_UTILITIES_H_
+// Definitions for the controller status LED colors in WRGB format.
+// Keep this limited to the status colors we actually use so it does not imply
+// ESC protocol colors that the SINE library does not support.
+
+#define STATUS_LED_RED 0x00FF0000
+#define STATUS_LED_YELLOW 0x00FFFF00
+#define STATUS_LED_GREEN 0x0000FF00
+
+#endif  // INC_SP140_UTILITIES_H_
diff --git a/platformio.ini b/platformio.ini
@@ -60,7 +60,7 @@ lib_deps =
 	adafruit/Adafruit CAN@0.2.3
 	adafruit/Adafruit MCP2515@0.2.1
 	https://github.com/rlogiacco/CircularBuffer@1.4.0
-	https://github.com/openppg/SINE-ESC-CAN#03045012a838c566b34bc0ae00cef457dd6d04d2
+	https://github.com/openppg/SINE-ESC-CAN#2ab56a4e5b52e4456317c8ee3e3d802b232c6148
 	https://github.com/openppg/ANT-BMS-CAN#fd54852bc6f1c9608e37af9ca7c13ea4135c095b
 	lvgl/lvgl@^9.5.0
 	h2zero/NimBLE-Arduino@^2.3.9
diff --git a/src/sp140/esc.cpp b/src/sp140/esc.cpp
@@ -11,24 +11,141 @@
 
 #define TELEMETRY_TIMEOUT_MS 50  // Threshold to determine stale ESC telemetry in ms
 
-static CanardAdapter adapter;
-static uint8_t memory_pool[1024] __attribute__((aligned(8)));
-static SineEsc esc(adapter);
-static unsigned long lastSuccessfulCommTimeMs = 0;  // Store millis() time of last successful ESC comm
-// Flag set by requestEscHardwareInfo() (may be called from BLE task),
-// consumed safely inside readESCTelemetry() on the throttle task.
-static volatile bool s_hwInfoRequested = false;
-
-// ESC runtime accumulation — unwraps the uint16 time_10ms counter (~10.9 min period)
-// Unsigned subtraction naturally handles wrap: (uint16_t)(current - last) is correct even across rollover.
-static uint16_t sEscLastTime10ms = 0;
-static uint32_t sEscAccumulatedRuntimeMs = 0;
-static bool sEscFirstUpdate = true;
-
-
-STR_ESC_TELEMETRY_140 escTelemetryData = {
-  .escState = TelemetryState::NOT_CONNECTED,
-  .running_error = 0,
+static CanardAdapter adapter;
+static uint8_t memory_pool[1024] __attribute__((aligned(8)));
+static SineEsc esc(adapter);
+static unsigned long lastSuccessfulCommTimeMs = 0;  // Store millis() time of last successful ESC comm
+// Flag set by requestEscHardwareInfo() (may be called from BLE task),
+// consumed safely inside readESCTelemetry() on the throttle task.
+static volatile bool s_hwInfoRequested = false;
+
+enum class PendingEscTone : uint8_t {
+  NONE = 0,
+  ARM,
+  DISARM,
+};
+
+static volatile EscStatusLightMode sRequestedStatusLightMode =
+    EscStatusLightMode::OFF;
+static EscStatusLightMode sLastSentStatusLightMode = EscStatusLightMode::OFF;
+static unsigned long sLastStatusLightSendMs = 0;
+static bool sHaveSentStatusLight = false;
+static volatile PendingEscTone sPendingEscTone = PendingEscTone::NONE;
+
+// ESC runtime accumulation — unwraps the uint16 time_10ms counter (~10.9 min period)
+// Unsigned subtraction naturally handles wrap: (uint16_t)(current - last) is correct even across rollover.
+static uint16_t sEscLastTime10ms = 0;
+static uint32_t sEscAccumulatedRuntimeMs = 0;
+static bool sEscFirstUpdate = true;
+
+namespace {
+
+constexpr uint8_t kEscToneVolumePct = 50;
+constexpr uint8_t kEscToneDuration10ms = 10;
+
+void buildEscMotorTone(uint8_t* out, PendingEscTone tone) {
+  if (tone == PendingEscTone::ARM) {
+    SineEsc::makeBeepEntry(&out[0], 3, kEscToneDuration10ms, kEscToneVolumePct);
+    SineEsc::makeBeepEntry(&out[3], 6, kEscToneDuration10ms, kEscToneVolumePct);
+  } else {
+    SineEsc::makeBeepEntry(&out[0], 6, kEscToneDuration10ms, kEscToneVolumePct);
+    SineEsc::makeBeepEntry(&out[3], 3, kEscToneDuration10ms, kEscToneVolumePct);
+  }
+}
+
+unsigned long escStatusLightRefreshMs(EscStatusLightMode mode) {
+  switch (mode) {
+  case EscStatusLightMode::FLIGHT:
+    return 1700;
+  case EscStatusLightMode::READY:
+  case EscStatusLightMode::CAUTION:
+    return 1000;
+  case EscStatusLightMode::OFF:
+  default:
+    return 0;
+  }
+}
+
+void sendEscStatusLight(EscStatusLightMode mode) {
+  switch (mode) {
+  case EscStatusLightMode::READY: {
+    const uint16_t pattern[] = {
+      SineEsc::makeLedControlEntry(SineEsc::LED_GREEN_BREATH, 20),
+    };
+    esc.setLedControl(pattern, 1);
+    break;
+  }
+  case EscStatusLightMode::FLIGHT: {
+    const uint16_t pattern[] = {
+      SineEsc::makeLedControlEntry(SineEsc::LED_GREEN, 1),
+      SineEsc::makeLedControlEntry(SineEsc::LED_OFF, 2),
+      SineEsc::makeLedControlEntry(SineEsc::LED_GREEN, 1),
+      SineEsc::makeLedControlEntry(SineEsc::LED_OFF, 30),
+    };
+    esc.setLedControl(pattern, 4);
+    break;
+  }
+  case EscStatusLightMode::CAUTION: {
+    const uint16_t pattern[] = {
+      SineEsc::makeLedControlEntry(SineEsc::LED_YELLOW_BREATH, 14),
+    };
+    esc.setLedControl(pattern, 1);
+    break;
+  }
+  case EscStatusLightMode::OFF:
+  default: {
+    const uint16_t pattern[] = {
+      SineEsc::makeLedControlEntry(SineEsc::LED_OFF, 20),
+    };
+    esc.setLedControl(pattern, 1);
+    break;
+  }
+  }
+}
+
+void syncEscOutputs() {
+  const bool escConnected =
+      escTwaiInitialized &&
+      escTelemetryData.escState == TelemetryState::CONNECTED;
+
+  if (!escConnected) {
+    sPendingEscTone = PendingEscTone::NONE;
+    sHaveSentStatusLight = false;
+    sLastSentStatusLightMode = EscStatusLightMode::OFF;
+    sLastStatusLightSendMs = 0;
+    return;
+  }
+
+  const PendingEscTone pendingTone = sPendingEscTone;
+  if (pendingTone != PendingEscTone::NONE) {
+    uint8_t beepData[6];
+    buildEscMotorTone(beepData, pendingTone);
+    esc.setMotorSound(beepData, 2);
+    sPendingEscTone = PendingEscTone::NONE;
+  }
+
+  const EscStatusLightMode requestedMode = sRequestedStatusLightMode;
+  const unsigned long now = millis();
+  const bool needsRefresh =
+      sHaveSentStatusLight &&
+      escStatusLightRefreshMs(requestedMode) > 0 &&
+      (now - sLastStatusLightSendMs) >= escStatusLightRefreshMs(requestedMode);
+
+  if (!sHaveSentStatusLight || requestedMode != sLastSentStatusLightMode ||
+      needsRefresh) {
+    sendEscStatusLight(requestedMode);
+    sLastSentStatusLightMode = requestedMode;
+    sLastStatusLightSendMs = now;
+    sHaveSentStatusLight = true;
+  }
+}
+
+}  // namespace
+
+
+STR_ESC_TELEMETRY_140 escTelemetryData = {
+  .escState = TelemetryState::NOT_CONNECTED,
+  .running_error = 0,
   .selfcheck_error = 0
 };
 
@@ -174,16 +291,17 @@ void readESCTelemetry() {
   }
 
   // Populate static hardware info whenever available (comes in once after boot)
-  if (model.hasGetHardwareInfoResponse) {
-    const sine_esc_GetHwInfoResponse *hw = &model.getHardwareInfoResponse;
-    escTelemetryData.hardware_id = hw->hardware_id;
-    escTelemetryData.fw_version = hw->app_version;
-    escTelemetryData.bootloader_version = hw->bootloader_version;
-    memcpy(escTelemetryData.sn_code, hw->sn_code, sizeof(escTelemetryData.sn_code));
-  }
-
-  adapter.processTxRxOnce();  // Process CAN messages
-}
+  if (model.hasGetHardwareInfoResponse) {
+    const sine_esc_GetHwInfoResponse *hw = &model.getHardwareInfoResponse;
+    escTelemetryData.hardware_id = hw->hardware_id;
+    escTelemetryData.fw_version = hw->app_version;
+    escTelemetryData.bootloader_version = hw->bootloader_version;
+    memcpy(escTelemetryData.sn_code, hw->sn_code, sizeof(escTelemetryData.sn_code));
+  }
+
+  syncEscOutputs();
+  adapter.processTxRxOnce();  // Process CAN messages
+}
 
 /**
  * Request ESC hardware info (HW ID, FW version, bootloader version, serial number).
@@ -263,87 +381,27 @@ bool setupTWAI() {
  * @param ledData Array of packed LED entries (use SineEsc::makeLedControlEntry)
  * @param ledNum Number of entries (max 10)
  */
-void setESCLedControl(const uint16_t *ledData, uint8_t ledNum) {
-  if (!escTwaiInitialized) return;
-  esc.setLedControl(ledData, ledNum);
-}
-
-/**
- * Send a motor beep sequence to the ESC
- * @param beepData Raw byte array of 3-byte entries (tone, duration_10ms, volume)
- * @param beepDataLen Length of beepData in bytes (entries * 3)
- * @param beepNum Number of beep entries
- */
-void setESCMotorSound(const uint8_t *beepData, uint8_t beepDataLen, uint8_t beepNum) {
-  if (!escTwaiInitialized) return;
-  esc.setMotorSound(beepData, beepDataLen, beepNum);
-}
-
-// Helper to pack a beep entry into 3 bytes in a buffer
-// tone: 0-9, duration: in 10ms units, volume: 0-100
-static void packBeepEntry(uint8_t *buf, uint8_t tone, uint8_t duration10ms, uint8_t volume) {
-  buf[0] = tone;
-  buf[1] = duration10ms;
-  buf[2] = volume;
-}
-
-/**
- * Play ascending arm beep on ESC motor (matches hand controller: C7 -> E7)
- * Uses two tones at 100ms each, ascending pitch
- */
-void escMotorBeepArm() {
-  uint8_t beepData[6];
-  packBeepEntry(&beepData[0], 3, 10, 50);  // lower tone, 100ms, 50% vol
-  packBeepEntry(&beepData[3], 6, 10, 50);  // higher tone, 100ms, 50% vol
-  setESCMotorSound(beepData, 6, 2);
-  USBSerial.println("ESC: arm beep sent");
-}
-
-/**
- * Play descending disarm beep on ESC motor (matches hand controller: E7 -> C7)
- * Uses two tones at 100ms each, descending pitch
- */
-void escMotorBeepDisarm() {
-  uint8_t beepData[6];
-  packBeepEntry(&beepData[0], 6, 10, 50);  // higher tone, 100ms, 50% vol
-  packBeepEntry(&beepData[3], 3, 10, 50);  // lower tone, 100ms, 50% vol
-  setESCMotorSound(beepData, 6, 2);
-  USBSerial.println("ESC: disarm beep sent");
-}
-
-// FreeRTOS task: aviation strobe pattern
-// Quick red pulse, quick green pulse, then off for ~1.5s, repeat
-static void escLedStrobeTask(void *pvParameters) {
-  // Wait for ESC connection before starting
-  vTaskDelay(pdMS_TO_TICKS(3000));
-
-  const uint16_t pattern[] = {
-    SineEsc::makeLedControlEntry(SineEsc::LED_RED, 1),    // red flash 50ms
-    SineEsc::makeLedControlEntry(SineEsc::LED_OFF, 2),    // gap 100ms
-    SineEsc::makeLedControlEntry(SineEsc::LED_GREEN, 1),  // green flash 50ms
-    SineEsc::makeLedControlEntry(SineEsc::LED_OFF, 30),   // off 1500ms
-  };
-
-  USBSerial.println("ESC LED aviation strobe started");
-
-  for (;;) {
-    setESCLedControl(pattern, 4);
-    USBSerial.println("ESC LED: sent aviation strobe");
-    // Wait for full cycle (50 + 100 + 50 + 1500 = 1700ms) before resending
-    vTaskDelay(pdMS_TO_TICKS(1700));
-  }
-}
-
-static TaskHandle_t escLedStrobeTaskHandle = NULL;
-
-/**
- * Start the ESC LED strobe test pattern
- * Creates a FreeRTOS task that repeatedly sends the strobe sequence
- */
-void startESCLedStrobeTest() {
-  if (escLedStrobeTaskHandle != NULL) return;  // already running
-  xTaskCreate(escLedStrobeTask, "ESCLedTest", 2048, NULL, 1, &escLedStrobeTaskHandle);
-}
+void setESCLedControl(const uint16_t *ledData, uint8_t ledNum) {
+  if (!escTwaiInitialized) return;
+  esc.setLedControl(ledData, ledNum);
+}
+
+void requestEscStatusLightMode(EscStatusLightMode mode) {
+  sRequestedStatusLightMode = mode;
+}
+
+void setESCMotorSound(const uint8_t *beepData, uint8_t beepNum) {
+  if (!escTwaiInitialized) return;
+  esc.setMotorSound(beepData, beepNum);
+}
+
+void queueEscMotorBeepArm() {
+  sPendingEscTone = PendingEscTone::ARM;
+}
+
+void queueEscMotorBeepDisarm() {
+  sPendingEscTone = PendingEscTone::DISARM;
+}
 
 /**
  * Debug function to dump ESC throttle response data to serial
diff --git a/src/sp140/main.cpp b/src/sp140/main.cpp
