Enhance ESC control with LED patterns and motor beep functionality

extra_script.py

@@ -2,12 +2,6 @@
 import subprocess
 
 Import("env")
-folder = env.GetProjectOption("custom_src_folder")
-
-# Generic
-env.Replace(
-    PROJECT_SRC_DIR="$PROJECT_DIR/src/" + folder
-)
 
 def get_git_revision_short_hash():
     try:

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();
@@ -58,6 +65,13 @@ bool hasMotorIDet2Error(uint16_t errorCode);
 bool hasSwHwIncompatError(uint16_t errorCode);
 bool hasBootloaderBadError(uint16_t errorCode);
 
+// ESC LED control
+void requestEscStatusLightMode(EscStatusLightMode mode);
+
+// ESC motor beep
+void queueEscMotorBeepArm();
+void queueEscMotorBeepDisarm();
+
 // for debugging
 void dumpThrottleResponse(const sine_esc_SetThrottleSettings2Response *res);
 void dumpESCMessages();  // dumps all messages to USBSerial

inc/sp140/utilities.h

@@ -1,22 +1,17 @@
-#ifndef INC_SP140_UTILITIES_H_
-#define INC_SP140_UTILITIES_H_
-
-#include <Arduino.h>
-
-// Function to get unique chip ID
-String chipId();
+#ifndef INC_SP140_UTILITIES_H_
+#define INC_SP140_UTILITIES_H_
 
-// 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.
+#include <Arduino.h>
 
-#define LED_RED 0x00FF0000
-#define LED_ORANGE 0x00FF7F00
-#define LED_YELLOW 0x00FFFF00
-#define LED_GREEN 0x0000FF00
-#define LED_BLUE 0x000000FF
-#define LED_INDIGO 0x004B0082
-#define LED_VIOLET 0x008000FF
+// Function to get unique chip ID
+String chipId();
 
-#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_

platformio.ini

@@ -11,6 +11,7 @@
 [platformio]
 lib_dir = libraries
 include_dir = inc
+src_dir = src/sp140
 default_envs =
 	OpenPPG-CESP32S3-CAN-SP140
 
@@ -42,6 +43,7 @@ build_flags =
 	-D CORE_DEBUG_LEVEL=2
 	-D CONFIG_ARDUINO_LOOP_STACK_SIZE=8192
 	-Wno-error=format
+	-Wno-error=int-in-bool-context
 	;-D BLE_PAIR_ON_BOOT
 build_type = debug
 debug_speed = 12000
@@ -58,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#8caa93996b5d000fe10ca5265bd1c472dfdf885b
+	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
@@ -105,9 +107,9 @@ build_flags =
   -std=c++17
 build_src_filter =
   -<*>
-  +<sp140/lvgl/lvgl_main_screen.cpp>
-  +<sp140/lvgl/lvgl_updates.cpp>
-  +<sp140/lvgl/lvgl_alerts.cpp>
+  +<lvgl/lvgl_main_screen.cpp>
+  +<lvgl/lvgl_updates.cpp>
+  +<lvgl/lvgl_alerts.cpp>
 test_filter = test_screenshots
 lib_deps =
   lvgl/lvgl@^9.5.0

src/sp140/esc.cpp

@@ -19,12 +19,132 @@ static unsigned long lastSuccessfulCommTimeMs = 0;  // Store millis() time of la
 // 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 kEscToneLow = 3;
+constexpr uint8_t kEscToneHigh = 6;
+constexpr uint8_t kEscToneVolumePct = 80;
+constexpr uint8_t kEscToneDuration10ms = 10;
+
+// Caller must pass ARM or DISARM (never NONE).
+void buildEscMotorTone(uint8_t* out, PendingEscTone tone) {
+  if (tone == PendingEscTone::ARM) {
+    SineEsc::makeBeepEntry(&out[0], kEscToneLow, kEscToneDuration10ms, kEscToneVolumePct);
+    SineEsc::makeBeepEntry(&out[3], kEscToneHigh, kEscToneDuration10ms, kEscToneVolumePct);
+  } else {
+    SineEsc::makeBeepEntry(&out[0], kEscToneHigh, kEscToneDuration10ms, kEscToneVolumePct);
+    SineEsc::makeBeepEntry(&out[3], kEscToneLow, 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, 20),
+    };
+    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,
@@ -182,6 +302,7 @@ void readESCTelemetry() {
     memcpy(escTelemetryData.sn_code, hw->sn_code, sizeof(escTelemetryData.sn_code));
   }
 
+  syncEscOutputs();
   adapter.processTxRxOnce();  // Process CAN messages
 }
 
@@ -258,6 +379,18 @@ bool setupTWAI() {
   return true;
 }
 
+void requestEscStatusLightMode(EscStatusLightMode mode) {
+  sRequestedStatusLightMode = mode;
+}
+
+void queueEscMotorBeepArm() {
+  sPendingEscTone = PendingEscTone::ARM;
+}
+
+void queueEscMotorBeepDisarm() {
+  sPendingEscTone = PendingEscTone::DISARM;
+}
+
 /**
  * Debug function to dump ESC throttle response data to serial
  * @param res Pointer to the throttle response structure from ESC

src/sp140/main.cpp

@@ -107,7 +107,7 @@ UnifiedBatteryData unifiedBatteryData = {0.0f, 0.0f, 0.0f, 0.0f};  // volts, amp
 // Throttle PWM smoothing buffer is managed in throttle.cpp
 
 Adafruit_NeoPixel pixels(1, 21, NEO_GRB + NEO_KHZ800);
-uint32_t led_color = LED_RED;  // current LED color
+uint32_t led_color = STATUS_LED_RED;  // current LED color
 
 // Global variable for device state
 volatile DeviceState currentState = DISARMED;
@@ -735,7 +735,7 @@ void setup() {
     initVibeMotor();
   }
 
-  setLEDColor(LED_YELLOW);  // Indicate boot in progress
+  setLEDColor(STATUS_LED_YELLOW);  // Indicate boot in progress
 
   // SPI bus (shared between display and BMS CAN)
   setupSPI(board_config);
@@ -766,7 +766,7 @@ void setup() {
     perfModeSwitch();
   }
 
-  setLEDColor(LED_GREEN);
+  setLEDColor(STATUS_LED_GREEN);
 
   // Show splash screen (blocking)
   if (xSemaphoreTake(lvglMutex, portMAX_DELAY) == pdTRUE) {
@@ -968,6 +968,7 @@ void resumeLEDTask() {
 void runDisarmAlert() {
   u_int16_t disarm_melody[] = {2637, 2093};
   playMelody(disarm_melody, 2);
+  queueEscMotorBeepDisarm();
   pulseVibeMotor();
 }
 
@@ -1183,6 +1184,27 @@ void syncESCTelemetry() {
     escTelemetryData.escState = TelemetryState::NOT_CONNECTED;
   }
 
+  EscStatusLightMode escStatusLightMode = EscStatusLightMode::OFF;
+  const bool escConnected =
+      escTelemetryData.escState == TelemetryState::CONNECTED;
+  const bool bmsConnected =
+      bmsTelemetryData.bmsState == TelemetryState::CONNECTED;
+  const bool batteryCaution =
+      bmsConnected &&
+      (bmsTelemetryData.low_soc_warning || !bmsTelemetryData.battery_ready ||
+       !bmsTelemetryData.is_discharge_mos);
+
+  if (escConnected) {
+    if (isOtaInProgress() || batteryCaution) {
+      escStatusLightMode = EscStatusLightMode::CAUTION;
+    } else if (currentState == DISARMED) {
+      escStatusLightMode = EscStatusLightMode::READY;
+    } else {
+      escStatusLightMode = EscStatusLightMode::FLIGHT;
+    }
+  }
+
+  requestEscStatusLightMode(escStatusLightMode);
   telemetryHubWriteEsc(escTelemetryData);
 }
 
@@ -1204,6 +1226,7 @@ bool armSystem() {
   vTaskSuspend(blinkLEDTaskHandle);
   setLEDs(HIGH);  // solid LED while armed
   playMelody(arm_melody, 2);
+  queueEscMotorBeepArm();
   // runVibePattern(arm_vibes, 7);
   pulseVibeMotor();  // Ensure this is the active call
   return true;

test/test_screenshots/emulator_stubs.cpp

@@ -11,6 +11,7 @@
 #include "sp140/vibration_pwm.h"
 #include "sp140/esp32s3-config.h"
 #include "sp140/ble.h"
+#include "sp140/ble/ble_core.h"
 
 // --- Hardware config ---
 HardwareConfig s3_config = {};
@@ -126,6 +127,13 @@ void addAltimeterMonitors() {}
 void addInternalMonitors() {}
 void enableMonitoring() {}
 
+// --- BLE core stubs ---
+void setupBLE() {}
+void requestFastConnParams() {}
+void requestNormalConnParams() {}
+void enterBLEPairingMode() {}
+bool isBLEPairingModeActive() { return false; }
+
 // --- BMS stubs ---
 BMS_CAN* bms_can = nullptr;
 bool initBMSCAN(SPIClass*) { return false; }