From 4c4311c590c1915746c2ba1962efd7186c9ae55b Mon Sep 17 00:00:00 2001 From: Paul <36135447+PaulDWhite@users.noreply.github.com> Date: Fri, 10 Apr 2026 11:36:23 -0400 Subject: [PATCH 1/6] Add ESC LED control and motor beep for arm/disarm - Update SINE-ESC-CAN library to v0.0.4 (led-control branch) which adds SetLedControl (Data ID 218) and includes SetMotorSound (Data ID 214) - Add setMotorSound() to SineEsc class to wire up motor beep protocol - Add ESC LED aviation strobe pattern (red/green double pulse) - Add motor beep on arm (ascending tone) and disarm (descending tone) matching the hand controller buzzer cadence - Rename LED_RED/GREEN/YELLOW macros to NEOPIXEL_* to avoid collision with SineEsc::LedPattern enum Co-Authored-By: Claude Opus 4.6 (1M context) --- extra_script.py | 6 --- inc/sp140/esc.h | 9 +++++ inc/sp140/utilities.h | 28 +++++++------- platformio.ini | 10 +++-- src/sp140/esc.cpp | 87 +++++++++++++++++++++++++++++++++++++++++++ src/sp140/main.cpp | 11 ++++-- 6 files changed, 124 insertions(+), 27 deletions(-) diff --git a/extra_script.py b/extra_script.py index d4ef0ae4..8da3c942 100644 --- a/extra_script.py +++ b/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: diff --git a/inc/sp140/esc.h b/inc/sp140/esc.h index 939019f7..ff444c2f 100644 --- a/inc/sp140/esc.h +++ b/inc/sp140/esc.h @@ -58,6 +58,15 @@ bool hasMotorIDet2Error(uint16_t errorCode); bool hasSwHwIncompatError(uint16_t errorCode); bool hasBootloaderBadError(uint16_t errorCode); +// ESC LED control +void setESCLedControl(const uint16_t *ledData, uint8_t ledNum); +void startESCLedStrobeTest(); + +// ESC motor beep +void setESCMotorSound(const uint8_t *beepData, uint8_t beepDataLen, uint8_t beepNum); +void escMotorBeepArm(); +void escMotorBeepDisarm(); + // for debugging void dumpThrottleResponse(const sine_esc_SetThrottleSettings2Response *res); void dumpESCMessages(); // dumps all messages to USBSerial diff --git a/inc/sp140/utilities.h b/inc/sp140/utilities.h index 3ab16967..e3077d8c 100644 --- a/inc/sp140/utilities.h +++ b/inc/sp140/utilities.h @@ -1,22 +1,22 @@ -#ifndef INC_SP140_UTILITIES_H_ -#define INC_SP140_UTILITIES_H_ - -#include - -// Function to get unique chip ID -String chipId(); +#ifndef INC_SP140_UTILITIES_H_ +#define INC_SP140_UTILITIES_H_ + +#include + +// 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 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 +#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_ diff --git a/platformio.ini b/platformio.ini index 00f46887..588a49bb 100644 --- a/platformio.ini +++ b/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#03045012a838c566b34bc0ae00cef457dd6d04d2 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 = -<*> - + - + - + + + + + + + test_filter = test_screenshots lib_deps = lvgl/lvgl@^9.5.0 diff --git a/src/sp140/esc.cpp b/src/sp140/esc.cpp index 8bc476ec..1b9487e0 100644 --- a/src/sp140/esc.cpp +++ b/src/sp140/esc.cpp @@ -258,6 +258,93 @@ bool setupTWAI() { return true; } +/** + * Send an LED control sequence to the ESC + * @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); +} + /** * Debug function to dump ESC throttle response data to serial * @param res Pointer to the throttle response structure from ESC diff --git a/src/sp140/main.cpp b/src/sp140/main.cpp index 41d762f4..8b04e8aa 100644 --- a/src/sp140/main.cpp +++ b/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 = NEOPIXEL_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(NEOPIXEL_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(NEOPIXEL_GREEN); // Show splash screen (blocking) if (xSemaphoreTake(lvglMutex, portMAX_DELAY) == pdTRUE) { @@ -815,6 +815,9 @@ void setup() { // ========================================================================= setupTasks(); + // Start ESC LED strobe test + startESCLedStrobeTest(); + // ========================================================================= // PHASE 7: Start External Interfaces // ========================================================================= @@ -968,6 +971,7 @@ void resumeLEDTask() { void runDisarmAlert() { u_int16_t disarm_melody[] = {2637, 2093}; playMelody(disarm_melody, 2); + escMotorBeepDisarm(); pulseVibeMotor(); } @@ -1204,6 +1208,7 @@ bool armSystem() { vTaskSuspend(blinkLEDTaskHandle); setLEDs(HIGH); // solid LED while armed playMelody(arm_melody, 2); + escMotorBeepArm(); // runVibePattern(arm_vibes, 7); pulseVibeMotor(); // Ensure this is the active call return true; From 278a329bb216c8ca5c02709a272097c8a4e4b717 Mon Sep 17 00:00:00 2001 From: Zach Whitehead Date: Fri, 10 Apr 2026 11:30:27 -0500 Subject: [PATCH 2/6] Refactor ESC LED/tone handling and status lights 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. --- inc/sp140/esc.h | 15 ++- inc/sp140/utilities.h | 23 ++-- platformio.ini | 2 +- src/sp140/esc.cpp | 276 +++++++++++++++++++++++++----------------- src/sp140/main.cpp | 34 ++++-- 5 files changed, 214 insertions(+), 136 deletions(-) diff --git a/inc/sp140/esc.h b/inc/sp140/esc.h index ff444c2f..854ad147 100644 --- a/inc/sp140/esc.h +++ b/inc/sp140/esc.h @@ -15,6 +15,13 @@ inline bool isMotorTempValidC(float tempC) { #include #include +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 index e3077d8c..da463e58 100644 --- 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 index 588a49bb..279f54ed 100644 --- 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 index 1b9487e0..e2b39397 100644 --- 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 index 8b04e8aa..5af94daa 100644 --- a/src/sp140/main.cpp +++ b/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 = NEOPIXEL_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(NEOPIXEL_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(NEOPIXEL_GREEN); + setLEDColor(STATUS_LED_GREEN); // Show splash screen (blocking) if (xSemaphoreTake(lvglMutex, portMAX_DELAY) == pdTRUE) { @@ -815,9 +815,6 @@ void setup() { // ========================================================================= setupTasks(); - // Start ESC LED strobe test - startESCLedStrobeTest(); - // ========================================================================= // PHASE 7: Start External Interfaces // ========================================================================= @@ -971,7 +968,7 @@ void resumeLEDTask() { void runDisarmAlert() { u_int16_t disarm_melody[] = {2637, 2093}; playMelody(disarm_melody, 2); - escMotorBeepDisarm(); + queueEscMotorBeepDisarm(); pulseVibeMotor(); } @@ -1187,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); } @@ -1208,7 +1226,7 @@ bool armSystem() { vTaskSuspend(blinkLEDTaskHandle); setLEDs(HIGH); // solid LED while armed playMelody(arm_melody, 2); - escMotorBeepArm(); + queueEscMotorBeepArm(); // runVibePattern(arm_vibes, 7); pulseVibeMotor(); // Ensure this is the active call return true; From f865e29f1bea429349495bda172c240afbf4ec3b Mon Sep 17 00:00:00 2001 From: Zach Whitehead Date: Fri, 10 Apr 2026 13:05:33 -0500 Subject: [PATCH 3/6] Remove ESC LED and motor sound APIs Delete deprecated setESCLedControl and setESCMotorSound functions: remove their declarations from inc/sp140/esc.h and implementations from src/sp140/esc.cpp. These wrappers simply forwarded to esc.setLedControl/esc.setMotorSound and checked escTwaiInitialized; the public interface is being simplified in favor of requestEscStatusLightMode and the queueEscMotorBeep* helpers. --- inc/sp140/esc.h | 2 -- src/sp140/esc.cpp | 15 --------------- 2 files changed, 17 deletions(-) diff --git a/inc/sp140/esc.h b/inc/sp140/esc.h index 854ad147..23115794 100644 --- a/inc/sp140/esc.h +++ b/inc/sp140/esc.h @@ -66,11 +66,9 @@ bool hasSwHwIncompatError(uint16_t errorCode); bool hasBootloaderBadError(uint16_t errorCode); // ESC LED control -void setESCLedControl(const uint16_t *ledData, uint8_t ledNum); void requestEscStatusLightMode(EscStatusLightMode mode); // ESC motor beep -void setESCMotorSound(const uint8_t *beepData, uint8_t beepNum); void queueEscMotorBeepArm(); void queueEscMotorBeepDisarm(); diff --git a/src/sp140/esc.cpp b/src/sp140/esc.cpp index e2b39397..7c4244c4 100644 --- a/src/sp140/esc.cpp +++ b/src/sp140/esc.cpp @@ -376,25 +376,10 @@ bool setupTWAI() { return true; } -/** - * Send an LED control sequence to the ESC - * @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); -} - 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; } From 57293d4dfd2033c544575b0159b933b35199cb89 Mon Sep 17 00:00:00 2001 From: Zach Whitehead Date: Fri, 10 Apr 2026 13:39:03 -0500 Subject: [PATCH 4/6] Update esc.cpp --- src/sp140/esc.cpp | 1141 +++++++++++++++++++++++---------------------- 1 file changed, 572 insertions(+), 569 deletions(-) diff --git a/src/sp140/esc.cpp b/src/sp140/esc.cpp index 7c4244c4..251e9355 100644 --- a/src/sp140/esc.cpp +++ b/src/sp140/esc.cpp @@ -1,16 +1,16 @@ -#include "sp140/esc.h" -#include "sp140/globals.h" -#include - -#pragma GCC diagnostic ignored "-Wmissing-field-initializers" -#include "driver/twai.h" - -#define ESC_RX_PIN 3 // CAN RX pin to transceiver -#define ESC_TX_PIN 2 // CAN TX pin to transceiver -#define LOCAL_NODE_ID 0x01 // The ID on the network of this device - -#define TELEMETRY_TIMEOUT_MS 50 // Threshold to determine stale ESC telemetry in ms - +#include "sp140/esc.h" +#include "sp140/globals.h" +#include + +#pragma GCC diagnostic ignored "-Wmissing-field-initializers" +#include "driver/twai.h" + +#define ESC_RX_PIN 3 // CAN RX pin to transceiver +#define ESC_TX_PIN 2 // CAN TX pin to transceiver +#define LOCAL_NODE_ID 0x01 // The ID on the network of this device + +#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); @@ -40,16 +40,19 @@ static bool sEscFirstUpdate = true; namespace { -constexpr uint8_t kEscToneVolumePct = 50; +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], 3, kEscToneDuration10ms, kEscToneVolumePct); - SineEsc::makeBeepEntry(&out[3], 6, kEscToneDuration10ms, kEscToneVolumePct); + SineEsc::makeBeepEntry(&out[0], kEscToneLow, kEscToneDuration10ms, kEscToneVolumePct); + SineEsc::makeBeepEntry(&out[3], kEscToneHigh, kEscToneDuration10ms, kEscToneVolumePct); } else { - SineEsc::makeBeepEntry(&out[0], 6, kEscToneDuration10ms, kEscToneVolumePct); - SineEsc::makeBeepEntry(&out[3], 3, kEscToneDuration10ms, kEscToneVolumePct); + SineEsc::makeBeepEntry(&out[0], kEscToneHigh, kEscToneDuration10ms, kEscToneVolumePct); + SineEsc::makeBeepEntry(&out[3], kEscToneLow, kEscToneDuration10ms, kEscToneVolumePct); } } @@ -87,7 +90,7 @@ void sendEscStatusLight(EscStatusLightMode mode) { } case EscStatusLightMode::CAUTION: { const uint16_t pattern[] = { - SineEsc::makeLedControlEntry(SineEsc::LED_YELLOW_BREATH, 14), + SineEsc::makeLedControlEntry(SineEsc::LED_YELLOW_BREATH, 20), }; esc.setLedControl(pattern, 1); break; @@ -146,151 +149,151 @@ void syncEscOutputs() { STR_ESC_TELEMETRY_140 escTelemetryData = { .escState = TelemetryState::NOT_CONNECTED, .running_error = 0, - .selfcheck_error = 0 -}; - -/** - * Initialize the ESC communication system - * Sets up the CAN bus (TWAI) interface and configures the ESC with default settings - * Must be called before any other ESC functions - */ -void initESC() { - escTwaiInitialized = setupTWAI(); - if (!escTwaiInitialized) { - USBSerial.println("ESC TWAI Initialization failed. ESC will not function."); - return; // Can't proceed without TWAI driver - } - - adapter.begin(memory_pool, sizeof(memory_pool)); - adapter.setLocalNodeId(LOCAL_NODE_ID); - esc.begin(0x20); // Default ID for the ESC - - // Set idle throttle only if ESC is found - const uint16_t IdleThrottle_us = 10000; // 1000us (0.1us resolution) - esc.setThrottleSettings2(IdleThrottle_us); - adapter.processTxRxOnce(); - vTaskDelay(pdMS_TO_TICKS(20)); // Wait for ESC to process the command -} - -/** - * Set the ESC throttle value - * @param throttlePWM Throttle value in microseconds (1000-2000) - * 1000 = minimum throttle, 2000 = maximum throttle - * - * Important: The ESC requires messages at least every 300ms or it will reset - */ -void setESCThrottle(int throttlePWM) { - // Input validation - if (throttlePWM < 1000 || throttlePWM > 2000) { - return; // Ignore invalid throttle values - } - - // Direct calculation: multiply by 10 to convert μs to 0.1μs resolution - uint16_t scaledThrottle = throttlePWM * 10; - esc.setThrottleSettings2(scaledThrottle); -} - -/** - * Read telemetry data from the ESC - * Updates the global escTelemetryData structure with current values - * Should be called regularly (every 20-50ms) to maintain connection - * Also monitors connection state and sets NOT_CONNECTED if timeout occurs - */ -void readESCTelemetry() { - // Only proceed if TWAI is initialized - if (!escTwaiInitialized) { return; } // NOLINT(whitespace/newline) - - const SineEscModel &model = esc.getModel(); - - if (model.hasSetThrottleSettings2Response) { - const sine_esc_SetThrottleSettings2Response *res = &model.setThrottleSettings2Response; - - uint16_t rawTime10ms = res->time_10ms; - - // Check if the timestamp from the ESC has actually changed (stale-data guard) - if (rawTime10ms != sEscLastTime10ms) { - // Unwrap the uint16 counter using unsigned subtraction — naturally correct across rollover. - // e.g. last=65000, current=100: (uint16_t)(100-65000) = 636 ticks = 6360ms elapsed. - if (!sEscFirstUpdate) { - uint16_t deltaTicks = rawTime10ms - sEscLastTime10ms; - sEscAccumulatedRuntimeMs += (uint32_t)deltaTicks * 10UL; - } - sEscFirstUpdate = false; - sEscLastTime10ms = rawTime10ms; - - // Timestamp is new, process the telemetry data - escTelemetryData.lastUpdateMs = sEscAccumulatedRuntimeMs; - escTelemetryData.esc_runtime_ms = sEscAccumulatedRuntimeMs; - - // Update telemetry data - escTelemetryData.volts = res->voltage / 10.0f; - escTelemetryData.amps = res->bus_current / 10.0f; - escTelemetryData.mos_temp = res->mos_temp / 10.0f; - escTelemetryData.cap_temp = res->cap_temp / 10.0f; - escTelemetryData.mcu_temp = res->mcu_temp / 10.0f; - // Filter motor temp - only update if sensor is connected (valid range: -20°C to 140°C) - // Disconnected sensor reads ~149°C (thermistor pulled high) - float rawMotorTemp = res->motor_temp / 10.0f; - if (isMotorTempValidC(rawMotorTemp)) { - escTelemetryData.motor_temp = rawMotorTemp; - } else { - // Store invalid motor temp as NaN. Downstream consumers can skip on isnan(). - escTelemetryData.motor_temp = NAN; - } - escTelemetryData.phase_current = res->current / 10.0f; - escTelemetryData.eRPM = res->speed; - escTelemetryData.inPWM = res->recv_pwm / 10.0f; - escTelemetryData.comm_pwm = res->comm_pwm; - escTelemetryData.v_modulation = res->v_modulation; - watts = escTelemetryData.amps * escTelemetryData.volts; - - // Store error bitmasks - escTelemetryData.running_error = res->running_error; - escTelemetryData.selfcheck_error = res->selfcheck_error; - - // Record the time of this successful communication using the local clock - lastSuccessfulCommTimeMs = millis(); - } // else: Timestamp hasn't changed, treat as stale data, don't update local timer or telemetry - - } else { - // If we are connected but don't have a response, perhaps mark as stale or log? - // For now, do nothing - } - - // Update connection state based on time since last successful communication - unsigned long currentTimeMs = millis(); - if (lastSuccessfulCommTimeMs == 0 || (currentTimeMs - lastSuccessfulCommTimeMs) > TELEMETRY_TIMEOUT_MS) { - if (escTelemetryData.escState != TelemetryState::NOT_CONNECTED) { - // Log state change only if it actually changed - USBSerial.printf("ESC State: %d -> NOT_CONNECTED (Timeout)\n", static_cast(escTelemetryData.escState)); - escTelemetryData.escState = TelemetryState::NOT_CONNECTED; - // Reset runtime accumulator so it restarts from zero on reconnect. - // Keep sEscLastTime10ms at its last real value so the stale-data guard - // continues to reject the unchanged res->time_10ms from the SINE library. - sEscAccumulatedRuntimeMs = 0; - sEscFirstUpdate = true; - } - } else { - if (escTelemetryData.escState != TelemetryState::CONNECTED) { - // Log state change only if it actually changed - USBSerial.printf("ESC State: %d -> CONNECTED\n", static_cast(escTelemetryData.escState)); - escTelemetryData.escState = TelemetryState::CONNECTED; - // Auto-request hardware info on first connection if not already populated - if (escTelemetryData.hardware_id == 0) { - USBSerial.println("ESC: Auto-requesting hardware info"); - esc.getHardwareInfo(); - } - } - } - - // Process any pending hardware info request (set from BLE command or retry) - if (s_hwInfoRequested) { - s_hwInfoRequested = false; - USBSerial.println("ESC: Sending GetHwInfo (app-requested)"); - esc.getHardwareInfo(); - } - - // Populate static hardware info whenever available (comes in once after boot) + .selfcheck_error = 0 +}; + +/** + * Initialize the ESC communication system + * Sets up the CAN bus (TWAI) interface and configures the ESC with default settings + * Must be called before any other ESC functions + */ +void initESC() { + escTwaiInitialized = setupTWAI(); + if (!escTwaiInitialized) { + USBSerial.println("ESC TWAI Initialization failed. ESC will not function."); + return; // Can't proceed without TWAI driver + } + + adapter.begin(memory_pool, sizeof(memory_pool)); + adapter.setLocalNodeId(LOCAL_NODE_ID); + esc.begin(0x20); // Default ID for the ESC + + // Set idle throttle only if ESC is found + const uint16_t IdleThrottle_us = 10000; // 1000us (0.1us resolution) + esc.setThrottleSettings2(IdleThrottle_us); + adapter.processTxRxOnce(); + vTaskDelay(pdMS_TO_TICKS(20)); // Wait for ESC to process the command +} + +/** + * Set the ESC throttle value + * @param throttlePWM Throttle value in microseconds (1000-2000) + * 1000 = minimum throttle, 2000 = maximum throttle + * + * Important: The ESC requires messages at least every 300ms or it will reset + */ +void setESCThrottle(int throttlePWM) { + // Input validation + if (throttlePWM < 1000 || throttlePWM > 2000) { + return; // Ignore invalid throttle values + } + + // Direct calculation: multiply by 10 to convert μs to 0.1μs resolution + uint16_t scaledThrottle = throttlePWM * 10; + esc.setThrottleSettings2(scaledThrottle); +} + +/** + * Read telemetry data from the ESC + * Updates the global escTelemetryData structure with current values + * Should be called regularly (every 20-50ms) to maintain connection + * Also monitors connection state and sets NOT_CONNECTED if timeout occurs + */ +void readESCTelemetry() { + // Only proceed if TWAI is initialized + if (!escTwaiInitialized) { return; } // NOLINT(whitespace/newline) + + const SineEscModel &model = esc.getModel(); + + if (model.hasSetThrottleSettings2Response) { + const sine_esc_SetThrottleSettings2Response *res = &model.setThrottleSettings2Response; + + uint16_t rawTime10ms = res->time_10ms; + + // Check if the timestamp from the ESC has actually changed (stale-data guard) + if (rawTime10ms != sEscLastTime10ms) { + // Unwrap the uint16 counter using unsigned subtraction — naturally correct across rollover. + // e.g. last=65000, current=100: (uint16_t)(100-65000) = 636 ticks = 6360ms elapsed. + if (!sEscFirstUpdate) { + uint16_t deltaTicks = rawTime10ms - sEscLastTime10ms; + sEscAccumulatedRuntimeMs += (uint32_t)deltaTicks * 10UL; + } + sEscFirstUpdate = false; + sEscLastTime10ms = rawTime10ms; + + // Timestamp is new, process the telemetry data + escTelemetryData.lastUpdateMs = sEscAccumulatedRuntimeMs; + escTelemetryData.esc_runtime_ms = sEscAccumulatedRuntimeMs; + + // Update telemetry data + escTelemetryData.volts = res->voltage / 10.0f; + escTelemetryData.amps = res->bus_current / 10.0f; + escTelemetryData.mos_temp = res->mos_temp / 10.0f; + escTelemetryData.cap_temp = res->cap_temp / 10.0f; + escTelemetryData.mcu_temp = res->mcu_temp / 10.0f; + // Filter motor temp - only update if sensor is connected (valid range: -20°C to 140°C) + // Disconnected sensor reads ~149°C (thermistor pulled high) + float rawMotorTemp = res->motor_temp / 10.0f; + if (isMotorTempValidC(rawMotorTemp)) { + escTelemetryData.motor_temp = rawMotorTemp; + } else { + // Store invalid motor temp as NaN. Downstream consumers can skip on isnan(). + escTelemetryData.motor_temp = NAN; + } + escTelemetryData.phase_current = res->current / 10.0f; + escTelemetryData.eRPM = res->speed; + escTelemetryData.inPWM = res->recv_pwm / 10.0f; + escTelemetryData.comm_pwm = res->comm_pwm; + escTelemetryData.v_modulation = res->v_modulation; + watts = escTelemetryData.amps * escTelemetryData.volts; + + // Store error bitmasks + escTelemetryData.running_error = res->running_error; + escTelemetryData.selfcheck_error = res->selfcheck_error; + + // Record the time of this successful communication using the local clock + lastSuccessfulCommTimeMs = millis(); + } // else: Timestamp hasn't changed, treat as stale data, don't update local timer or telemetry + + } else { + // If we are connected but don't have a response, perhaps mark as stale or log? + // For now, do nothing + } + + // Update connection state based on time since last successful communication + unsigned long currentTimeMs = millis(); + if (lastSuccessfulCommTimeMs == 0 || (currentTimeMs - lastSuccessfulCommTimeMs) > TELEMETRY_TIMEOUT_MS) { + if (escTelemetryData.escState != TelemetryState::NOT_CONNECTED) { + // Log state change only if it actually changed + USBSerial.printf("ESC State: %d -> NOT_CONNECTED (Timeout)\n", static_cast(escTelemetryData.escState)); + escTelemetryData.escState = TelemetryState::NOT_CONNECTED; + // Reset runtime accumulator so it restarts from zero on reconnect. + // Keep sEscLastTime10ms at its last real value so the stale-data guard + // continues to reject the unchanged res->time_10ms from the SINE library. + sEscAccumulatedRuntimeMs = 0; + sEscFirstUpdate = true; + } + } else { + if (escTelemetryData.escState != TelemetryState::CONNECTED) { + // Log state change only if it actually changed + USBSerial.printf("ESC State: %d -> CONNECTED\n", static_cast(escTelemetryData.escState)); + escTelemetryData.escState = TelemetryState::CONNECTED; + // Auto-request hardware info on first connection if not already populated + if (escTelemetryData.hardware_id == 0) { + USBSerial.println("ESC: Auto-requesting hardware info"); + esc.getHardwareInfo(); + } + } + } + + // Process any pending hardware info request (set from BLE command or retry) + if (s_hwInfoRequested) { + s_hwInfoRequested = false; + USBSerial.println("ESC: Sending GetHwInfo (app-requested)"); + esc.getHardwareInfo(); + } + + // 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; @@ -302,80 +305,80 @@ void readESCTelemetry() { syncEscOutputs(); adapter.processTxRxOnce(); // Process CAN messages } - -/** - * Request ESC hardware info (HW ID, FW version, bootloader version, serial number). - * Thread-safe: sets a flag that is consumed by readESCTelemetry() on its next - * tick, keeping all CAN traffic on the throttle task. - * Called by the BLE command characteristic handler (0x01 from the app). - */ -void requestEscHardwareInfo() { - s_hwInfoRequested = true; -} - -/** - * Setup the ESP32 TWAI (Two-Wire Automotive Interface) for CAN communication - * Configures the CAN bus at 1Mbps for communication with the ESC - * @return true if setup was successful, false otherwise - */ -bool setupTWAI() { - // Check if already installed by checking status - twai_status_info_t status_info; - esp_err_t status_result = twai_get_status_info(&status_info); - - if (status_result == ESP_OK) { - // Driver is installed. We can check its state if needed. - USBSerial.printf("TWAI driver already installed. State: %d\n", status_info.state); - // If it's stopped, maybe we need to start it? For now, assume it's okay. - // if (status_info.state == TWAI_STATE_STOPPED) { twai_start(); } - return true; // Already initialized - } else if (status_result != ESP_ERR_INVALID_STATE) { - // An error other than "not installed" occurred - USBSerial.printf("Error checking TWAI status: %s\n", esp_err_to_name(status_result)); - return false; // Don't proceed if status check failed unexpectedly - } - // If status_result was ESP_ERR_INVALID_STATE, proceed with installation - - twai_general_config_t g_config = TWAI_GENERAL_CONFIG_DEFAULT( - (gpio_num_t)ESC_TX_PIN, - (gpio_num_t)ESC_RX_PIN, - TWAI_MODE_NORMAL); - twai_timing_config_t t_config = TWAI_TIMING_CONFIG_1MBITS(); - twai_filter_config_t f_config = TWAI_FILTER_CONFIG_ACCEPT_ALL(); - - esp_err_t install_err = twai_driver_install(&g_config, &t_config, &f_config); - if (install_err == ESP_OK) { - USBSerial.println("TWAI Driver installed"); - } else { - USBSerial.printf("Failed to install TWAI driver: %s\n", esp_err_to_name(install_err)); - return false; - } - - esp_err_t start_err = twai_start(); - if (start_err == ESP_OK) { - USBSerial.println("TWAI Driver started"); - } else { - USBSerial.printf("Failed to start TWAI driver: %s\n", esp_err_to_name(start_err)); - // Attempt to uninstall if start failed - twai_driver_uninstall(); - return false; - } - - // Reconfigure alerts to detect frame receive, Bus-Off error and RX queue full states - uint32_t alerts_to_enable = TWAI_ALERT_RX_DATA - | TWAI_ALERT_ERR_PASS - | TWAI_ALERT_BUS_ERROR - | TWAI_ALERT_RX_QUEUE_FULL; - if (twai_reconfigure_alerts(alerts_to_enable, NULL) == ESP_OK) { - USBSerial.println("CAN Alerts reconfigured"); - } else { - USBSerial.println("Failed to reconfigure CAN alerts"); - // Consider this non-fatal for now? Or return false? - } - - return true; -} - + +/** + * Request ESC hardware info (HW ID, FW version, bootloader version, serial number). + * Thread-safe: sets a flag that is consumed by readESCTelemetry() on its next + * tick, keeping all CAN traffic on the throttle task. + * Called by the BLE command characteristic handler (0x01 from the app). + */ +void requestEscHardwareInfo() { + s_hwInfoRequested = true; +} + +/** + * Setup the ESP32 TWAI (Two-Wire Automotive Interface) for CAN communication + * Configures the CAN bus at 1Mbps for communication with the ESC + * @return true if setup was successful, false otherwise + */ +bool setupTWAI() { + // Check if already installed by checking status + twai_status_info_t status_info; + esp_err_t status_result = twai_get_status_info(&status_info); + + if (status_result == ESP_OK) { + // Driver is installed. We can check its state if needed. + USBSerial.printf("TWAI driver already installed. State: %d\n", status_info.state); + // If it's stopped, maybe we need to start it? For now, assume it's okay. + // if (status_info.state == TWAI_STATE_STOPPED) { twai_start(); } + return true; // Already initialized + } else if (status_result != ESP_ERR_INVALID_STATE) { + // An error other than "not installed" occurred + USBSerial.printf("Error checking TWAI status: %s\n", esp_err_to_name(status_result)); + return false; // Don't proceed if status check failed unexpectedly + } + // If status_result was ESP_ERR_INVALID_STATE, proceed with installation + + twai_general_config_t g_config = TWAI_GENERAL_CONFIG_DEFAULT( + (gpio_num_t)ESC_TX_PIN, + (gpio_num_t)ESC_RX_PIN, + TWAI_MODE_NORMAL); + twai_timing_config_t t_config = TWAI_TIMING_CONFIG_1MBITS(); + twai_filter_config_t f_config = TWAI_FILTER_CONFIG_ACCEPT_ALL(); + + esp_err_t install_err = twai_driver_install(&g_config, &t_config, &f_config); + if (install_err == ESP_OK) { + USBSerial.println("TWAI Driver installed"); + } else { + USBSerial.printf("Failed to install TWAI driver: %s\n", esp_err_to_name(install_err)); + return false; + } + + esp_err_t start_err = twai_start(); + if (start_err == ESP_OK) { + USBSerial.println("TWAI Driver started"); + } else { + USBSerial.printf("Failed to start TWAI driver: %s\n", esp_err_to_name(start_err)); + // Attempt to uninstall if start failed + twai_driver_uninstall(); + return false; + } + + // Reconfigure alerts to detect frame receive, Bus-Off error and RX queue full states + uint32_t alerts_to_enable = TWAI_ALERT_RX_DATA + | TWAI_ALERT_ERR_PASS + | TWAI_ALERT_BUS_ERROR + | TWAI_ALERT_RX_QUEUE_FULL; + if (twai_reconfigure_alerts(alerts_to_enable, NULL) == ESP_OK) { + USBSerial.println("CAN Alerts reconfigured"); + } else { + USBSerial.println("Failed to reconfigure CAN alerts"); + // Consider this non-fatal for now? Or return false? + } + + return true; +} + void requestEscStatusLightMode(EscStatusLightMode mode) { sRequestedStatusLightMode = mode; } @@ -387,334 +390,334 @@ void queueEscMotorBeepArm() { 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 - */ -void dumpThrottleResponse(const sine_esc_SetThrottleSettings2Response *res) { - USBSerial.println("Got SetThrottleSettings2 response"); - - USBSerial.print("\trecv_pwm: "); - USBSerial.println(res->recv_pwm); - - USBSerial.print("\tcomm_pwm: "); - USBSerial.println(res->comm_pwm); - - USBSerial.print("\tspeed: "); - USBSerial.println(res->speed); - - USBSerial.print("\tcurrent: "); - USBSerial.println(res->current); - - USBSerial.print("\tbus_current: "); - USBSerial.println(res->bus_current); - - USBSerial.print("\tvoltage: "); - USBSerial.println(res->voltage); - - USBSerial.print("\tv_modulation: "); - USBSerial.println(res->v_modulation); - - USBSerial.print("\tmos_temp: "); - USBSerial.println(res->mos_temp); - - USBSerial.print("\tcap_temp: "); - USBSerial.println(res->cap_temp); - - USBSerial.print("\tmcu_temp: "); - USBSerial.println(res->mcu_temp); - - USBSerial.print("\trunning_error: "); - USBSerial.print(res->running_error); - USBSerial.print(" ("); - USBSerial.print(decodeRunningError(res->running_error)); - USBSerial.println(")"); - - USBSerial.print("\tselfcheck_error: "); - USBSerial.print(res->selfcheck_error); - USBSerial.print(" ("); - USBSerial.print(decodeSelfCheckError(res->selfcheck_error)); - USBSerial.println(")"); - - USBSerial.print("\tmotor_temp: "); - USBSerial.println(res->motor_temp); - - USBSerial.print("\ttime_10ms: "); - USBSerial.println(res->time_10ms); -} - -/** - * Debug function to dump all available ESC messages to serial - * Displays hardware info, throttle response, and rotation speed settings if available - */ -void dumpESCMessages(void) { - const SineEscModel &model = esc.getModel(); - - if (model.hasGetHardwareInfoResponse) { - USBSerial.println("Got HwInfo response"); - - const sine_esc_GetHwInfoResponse *b = &model.getHardwareInfoResponse; - USBSerial.print("\thardware_id: "); - USBSerial.println(b->hardware_id, HEX); - USBSerial.print("\tbootloader_version: "); - USBSerial.println(b->bootloader_version, HEX); - USBSerial.print("\tapp_version: "); - USBSerial.println(b->app_version, HEX); - } - - if (model.hasSetThrottleSettings2Response) { - dumpThrottleResponse(&model.setThrottleSettings2Response); - } - - if (model.hasSetRotationSpeedSettingsResponse) { - USBSerial.println("Got SetRotationSpeedSettings response"); - } -} - -/** - * Map a double value from one range to another - * @param x Input value to map - * @param in_min Minimum of input range - * @param in_max Maximum of input range - * @param out_min Minimum of output range - * @param out_max Maximum of output range - * @return Mapped value in output range - */ -double mapDouble(double x, double in_min, double in_max, double out_min, double out_max) { - return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; -} - -/** - * Decode running error bitmask into human-readable string - * @param errorCode 16-bit running error code from ESC - * @return String containing decoded error messages - */ -String decodeRunningError(uint16_t errorCode) { - if (errorCode == 0) { - return "no_errors"; - } - - String result = ""; - bool firstError = true; - - // Define error messages for each bit - const char* errorMessages[] = { - "over_current_protect", // Bit 0: Over current/short circuit protection occurs - "locked_rotor_protect", // Bit 1: Locked-rotor protection occurs - "over_temp_protect", // Bit 2: Over-temperature protection - "pwm_throttle_lost", // Bit 3: PWM throttle lost pulse - "no_load", // Bit 4: No load - "throttle_saturation", // Bit 5: Throttle saturation - "over_volt_protect", // Bit 6: Over voltage protection - "voltage_drop", // Bit 7: Voltage drop - "comm_throttle_loss", // Bit 8: Communication throttle loss - "undef_9", // Bit 9: Undefined - "undef_10", // Bit 10: Undefined - "undef_11", // Bit 11: Undefined - "undef_12", // Bit 12: Undefined - "undef_13", // Bit 13: Undefined - "undef_14", // Bit 14: Undefined - "undef_15" // Bit 15: Undefined - }; - - for (int i = 0; i < 16; i++) { - if (errorCode & (1 << i)) { - if (!firstError) { - result += ", "; - } - result += errorMessages[i]; - firstError = false; - } - } - - return result; -} - -/** - * Decode self-check error bitmask into human-readable string - * @param errorCode 16-bit self-check error code from ESC - * @return String containing decoded error messages - */ -String decodeSelfCheckError(uint16_t errorCode) { - if (errorCode == 0) { - return "no_errors"; - } - - String result = ""; - bool firstError = true; - - // Define error messages for each bit - const char* errorMessages[] = { - "motor_i_out_bad", // Bit 0: Motor line current output abnormal - "total_i_out_bad", // Bit 1: Total current output abnormal - "motor_v_out_bad", // Bit 2: Motor line voltage abnormal - "cap_ntc_bad", // Bit 3: Electrolytic capacitor NTC output abnormal - "mos_ntc_bad", // Bit 4: MOS Tube NTC output abnormal - "bus_v_range", // Bit 5: Bus voltage over/under voltage - "bus_v_sample_bad", // Bit 6: Bus voltage sampling abnormal - "motor_z_too_low", // Bit 7: Motor wire loop impedance too low - "motor_z_too_high", // Bit 8: Motor wire loop impedance too large - "motor_v_det1_bad", // Bit 9: Motor line voltage detection circuit abnormal 1 - "motor_v_det2_bad", // Bit 10: Motor line voltage detection circuit abnormal 2 - "motor_i_det2_bad", // Bit 11: Motor line current detection circuit abnormal 02 - "undef_12", // Bit 12: undefined - "sw_hw_incompatible", // Bit 13: Software and hardware versions incompatible - "bootloader_unsupported", // Bit 14: Boot loader unsupported - "undef_15" // Bit 15: Undefined - }; - - for (int i = 0; i < 16; i++) { - if (errorCode & (1 << i)) { - if (!firstError) { - result += ", "; - } - result += errorMessages[i]; - firstError = false; - } - } - - return result; -} - -/** - * Check if there are any running errors - * @param errorCode 16-bit running error code from ESC - * @return true if any error bits are set - */ -bool hasRunningError(uint16_t errorCode) { - return errorCode != 0; -} - -/** - * Check if there are any self-check errors - * @param errorCode 16-bit self-check error code from ESC - * @return true if any error bits are set - */ -bool hasSelfCheckError(uint16_t errorCode) { - return errorCode != 0; -} - -/** - * Check if there are any critical running errors - * Critical errors are high-priority faults that require immediate attention - * @param errorCode 16-bit running error code from ESC - * @return true if any critical error bits are set - */ -bool hasCriticalRunningError(uint16_t errorCode) { - // Define critical error bits (High level errors from documentation) - // Bits 0,1,2,6,7 are High priority and relevant for CAN communication - const uint16_t criticalBits = 0x00C7; // Bits 0,1,2,6,7 (High priority) - // Excluded bits: - // - Bit 3 (pwm_throttle_lost): Not relevant for CAN communication - // - Bit 4 (no_load): Low priority - // - Bit 5 (throttle_saturation): Middle priority - // - Bit 8 (comm_throttle_loss): Low priority, expected with CAN - - return (errorCode & criticalBits) != 0; -} - -/** - * Check if there are any warning-level running errors - * Warning errors are middle-priority faults that should be monitored but aren't critical - * @param errorCode 16-bit running error code from ESC - * @return true if any warning error bits are set - */ -bool hasWarningRunningError(uint16_t errorCode) { - // Bit 5 (throttle_saturation) is Middle priority - treat as warning - const uint16_t warningBits = 0x0020; // Bit 5 only - - return (errorCode & warningBits) != 0; -} - -/** - * Check if there are any critical self-check errors - * All self-check errors are considered critical as they indicate hardware issues - * @param errorCode 16-bit self-check error code from ESC - * @return true if any error bits are set (all self-check errors are critical) - */ -bool hasCriticalSelfCheckError(uint16_t errorCode) { - return errorCode != 0; // All self-check errors are critical -} - -// Individual error bit checkers for specific monitoring -bool hasOverCurrentError(uint16_t errorCode) { - return (errorCode & 0x0001) != 0; // Bit 0 -} - -bool hasLockedRotorError(uint16_t errorCode) { - return (errorCode & 0x0002) != 0; // Bit 1 -} - -bool hasOverTempError(uint16_t errorCode) { - return (errorCode & 0x0004) != 0; // Bit 2 -} - -bool hasOverVoltError(uint16_t errorCode) { - return (errorCode & 0x0040) != 0; // Bit 6 -} - -bool hasVoltagDropError(uint16_t errorCode) { - return (errorCode & 0x0080) != 0; // Bit 7 -} - -bool hasThrottleSatWarning(uint16_t errorCode) { - return (errorCode & 0x0020) != 0; // Bit 5 -} - -// Individual self-check error bit checkers -bool hasMotorCurrentOutError(uint16_t errorCode) { - return (errorCode & 0x0001) != 0; // Bit 0 -} - -bool hasTotalCurrentOutError(uint16_t errorCode) { - return (errorCode & 0x0002) != 0; // Bit 1 -} - -bool hasMotorVoltageOutError(uint16_t errorCode) { - return (errorCode & 0x0004) != 0; // Bit 2 -} - -bool hasCapNTCError(uint16_t errorCode) { - return (errorCode & 0x0008) != 0; // Bit 3 -} - -bool hasMosNTCError(uint16_t errorCode) { - return (errorCode & 0x0010) != 0; // Bit 4 -} - -bool hasBusVoltRangeError(uint16_t errorCode) { - return (errorCode & 0x0020) != 0; // Bit 5 -} - -bool hasBusVoltSampleError(uint16_t errorCode) { - return (errorCode & 0x0040) != 0; // Bit 6 -} - -bool hasMotorZLowError(uint16_t errorCode) { - return (errorCode & 0x0080) != 0; // Bit 7 -} - -bool hasMotorZHighError(uint16_t errorCode) { - return (errorCode & 0x0100) != 0; // Bit 8 -} - -bool hasMotorVDet1Error(uint16_t errorCode) { - return (errorCode & 0x0200) != 0; // Bit 9 -} - -bool hasMotorVDet2Error(uint16_t errorCode) { - return (errorCode & 0x0400) != 0; // Bit 10 -} - -bool hasMotorIDet2Error(uint16_t errorCode) { - return (errorCode & 0x0800) != 0; // Bit 11 -} - -bool hasSwHwIncompatError(uint16_t errorCode) { - return (errorCode & 0x2000) != 0; // Bit 13 -} - -bool hasBootloaderBadError(uint16_t errorCode) { - return (errorCode & 0x4000) != 0; // Bit 14 -} + +/** + * Debug function to dump ESC throttle response data to serial + * @param res Pointer to the throttle response structure from ESC + */ +void dumpThrottleResponse(const sine_esc_SetThrottleSettings2Response *res) { + USBSerial.println("Got SetThrottleSettings2 response"); + + USBSerial.print("\trecv_pwm: "); + USBSerial.println(res->recv_pwm); + + USBSerial.print("\tcomm_pwm: "); + USBSerial.println(res->comm_pwm); + + USBSerial.print("\tspeed: "); + USBSerial.println(res->speed); + + USBSerial.print("\tcurrent: "); + USBSerial.println(res->current); + + USBSerial.print("\tbus_current: "); + USBSerial.println(res->bus_current); + + USBSerial.print("\tvoltage: "); + USBSerial.println(res->voltage); + + USBSerial.print("\tv_modulation: "); + USBSerial.println(res->v_modulation); + + USBSerial.print("\tmos_temp: "); + USBSerial.println(res->mos_temp); + + USBSerial.print("\tcap_temp: "); + USBSerial.println(res->cap_temp); + + USBSerial.print("\tmcu_temp: "); + USBSerial.println(res->mcu_temp); + + USBSerial.print("\trunning_error: "); + USBSerial.print(res->running_error); + USBSerial.print(" ("); + USBSerial.print(decodeRunningError(res->running_error)); + USBSerial.println(")"); + + USBSerial.print("\tselfcheck_error: "); + USBSerial.print(res->selfcheck_error); + USBSerial.print(" ("); + USBSerial.print(decodeSelfCheckError(res->selfcheck_error)); + USBSerial.println(")"); + + USBSerial.print("\tmotor_temp: "); + USBSerial.println(res->motor_temp); + + USBSerial.print("\ttime_10ms: "); + USBSerial.println(res->time_10ms); +} + +/** + * Debug function to dump all available ESC messages to serial + * Displays hardware info, throttle response, and rotation speed settings if available + */ +void dumpESCMessages(void) { + const SineEscModel &model = esc.getModel(); + + if (model.hasGetHardwareInfoResponse) { + USBSerial.println("Got HwInfo response"); + + const sine_esc_GetHwInfoResponse *b = &model.getHardwareInfoResponse; + USBSerial.print("\thardware_id: "); + USBSerial.println(b->hardware_id, HEX); + USBSerial.print("\tbootloader_version: "); + USBSerial.println(b->bootloader_version, HEX); + USBSerial.print("\tapp_version: "); + USBSerial.println(b->app_version, HEX); + } + + if (model.hasSetThrottleSettings2Response) { + dumpThrottleResponse(&model.setThrottleSettings2Response); + } + + if (model.hasSetRotationSpeedSettingsResponse) { + USBSerial.println("Got SetRotationSpeedSettings response"); + } +} + +/** + * Map a double value from one range to another + * @param x Input value to map + * @param in_min Minimum of input range + * @param in_max Maximum of input range + * @param out_min Minimum of output range + * @param out_max Maximum of output range + * @return Mapped value in output range + */ +double mapDouble(double x, double in_min, double in_max, double out_min, double out_max) { + return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; +} + +/** + * Decode running error bitmask into human-readable string + * @param errorCode 16-bit running error code from ESC + * @return String containing decoded error messages + */ +String decodeRunningError(uint16_t errorCode) { + if (errorCode == 0) { + return "no_errors"; + } + + String result = ""; + bool firstError = true; + + // Define error messages for each bit + const char* errorMessages[] = { + "over_current_protect", // Bit 0: Over current/short circuit protection occurs + "locked_rotor_protect", // Bit 1: Locked-rotor protection occurs + "over_temp_protect", // Bit 2: Over-temperature protection + "pwm_throttle_lost", // Bit 3: PWM throttle lost pulse + "no_load", // Bit 4: No load + "throttle_saturation", // Bit 5: Throttle saturation + "over_volt_protect", // Bit 6: Over voltage protection + "voltage_drop", // Bit 7: Voltage drop + "comm_throttle_loss", // Bit 8: Communication throttle loss + "undef_9", // Bit 9: Undefined + "undef_10", // Bit 10: Undefined + "undef_11", // Bit 11: Undefined + "undef_12", // Bit 12: Undefined + "undef_13", // Bit 13: Undefined + "undef_14", // Bit 14: Undefined + "undef_15" // Bit 15: Undefined + }; + + for (int i = 0; i < 16; i++) { + if (errorCode & (1 << i)) { + if (!firstError) { + result += ", "; + } + result += errorMessages[i]; + firstError = false; + } + } + + return result; +} + +/** + * Decode self-check error bitmask into human-readable string + * @param errorCode 16-bit self-check error code from ESC + * @return String containing decoded error messages + */ +String decodeSelfCheckError(uint16_t errorCode) { + if (errorCode == 0) { + return "no_errors"; + } + + String result = ""; + bool firstError = true; + + // Define error messages for each bit + const char* errorMessages[] = { + "motor_i_out_bad", // Bit 0: Motor line current output abnormal + "total_i_out_bad", // Bit 1: Total current output abnormal + "motor_v_out_bad", // Bit 2: Motor line voltage abnormal + "cap_ntc_bad", // Bit 3: Electrolytic capacitor NTC output abnormal + "mos_ntc_bad", // Bit 4: MOS Tube NTC output abnormal + "bus_v_range", // Bit 5: Bus voltage over/under voltage + "bus_v_sample_bad", // Bit 6: Bus voltage sampling abnormal + "motor_z_too_low", // Bit 7: Motor wire loop impedance too low + "motor_z_too_high", // Bit 8: Motor wire loop impedance too large + "motor_v_det1_bad", // Bit 9: Motor line voltage detection circuit abnormal 1 + "motor_v_det2_bad", // Bit 10: Motor line voltage detection circuit abnormal 2 + "motor_i_det2_bad", // Bit 11: Motor line current detection circuit abnormal 02 + "undef_12", // Bit 12: undefined + "sw_hw_incompatible", // Bit 13: Software and hardware versions incompatible + "bootloader_unsupported", // Bit 14: Boot loader unsupported + "undef_15" // Bit 15: Undefined + }; + + for (int i = 0; i < 16; i++) { + if (errorCode & (1 << i)) { + if (!firstError) { + result += ", "; + } + result += errorMessages[i]; + firstError = false; + } + } + + return result; +} + +/** + * Check if there are any running errors + * @param errorCode 16-bit running error code from ESC + * @return true if any error bits are set + */ +bool hasRunningError(uint16_t errorCode) { + return errorCode != 0; +} + +/** + * Check if there are any self-check errors + * @param errorCode 16-bit self-check error code from ESC + * @return true if any error bits are set + */ +bool hasSelfCheckError(uint16_t errorCode) { + return errorCode != 0; +} + +/** + * Check if there are any critical running errors + * Critical errors are high-priority faults that require immediate attention + * @param errorCode 16-bit running error code from ESC + * @return true if any critical error bits are set + */ +bool hasCriticalRunningError(uint16_t errorCode) { + // Define critical error bits (High level errors from documentation) + // Bits 0,1,2,6,7 are High priority and relevant for CAN communication + const uint16_t criticalBits = 0x00C7; // Bits 0,1,2,6,7 (High priority) + // Excluded bits: + // - Bit 3 (pwm_throttle_lost): Not relevant for CAN communication + // - Bit 4 (no_load): Low priority + // - Bit 5 (throttle_saturation): Middle priority + // - Bit 8 (comm_throttle_loss): Low priority, expected with CAN + + return (errorCode & criticalBits) != 0; +} + +/** + * Check if there are any warning-level running errors + * Warning errors are middle-priority faults that should be monitored but aren't critical + * @param errorCode 16-bit running error code from ESC + * @return true if any warning error bits are set + */ +bool hasWarningRunningError(uint16_t errorCode) { + // Bit 5 (throttle_saturation) is Middle priority - treat as warning + const uint16_t warningBits = 0x0020; // Bit 5 only + + return (errorCode & warningBits) != 0; +} + +/** + * Check if there are any critical self-check errors + * All self-check errors are considered critical as they indicate hardware issues + * @param errorCode 16-bit self-check error code from ESC + * @return true if any error bits are set (all self-check errors are critical) + */ +bool hasCriticalSelfCheckError(uint16_t errorCode) { + return errorCode != 0; // All self-check errors are critical +} + +// Individual error bit checkers for specific monitoring +bool hasOverCurrentError(uint16_t errorCode) { + return (errorCode & 0x0001) != 0; // Bit 0 +} + +bool hasLockedRotorError(uint16_t errorCode) { + return (errorCode & 0x0002) != 0; // Bit 1 +} + +bool hasOverTempError(uint16_t errorCode) { + return (errorCode & 0x0004) != 0; // Bit 2 +} + +bool hasOverVoltError(uint16_t errorCode) { + return (errorCode & 0x0040) != 0; // Bit 6 +} + +bool hasVoltagDropError(uint16_t errorCode) { + return (errorCode & 0x0080) != 0; // Bit 7 +} + +bool hasThrottleSatWarning(uint16_t errorCode) { + return (errorCode & 0x0020) != 0; // Bit 5 +} + +// Individual self-check error bit checkers +bool hasMotorCurrentOutError(uint16_t errorCode) { + return (errorCode & 0x0001) != 0; // Bit 0 +} + +bool hasTotalCurrentOutError(uint16_t errorCode) { + return (errorCode & 0x0002) != 0; // Bit 1 +} + +bool hasMotorVoltageOutError(uint16_t errorCode) { + return (errorCode & 0x0004) != 0; // Bit 2 +} + +bool hasCapNTCError(uint16_t errorCode) { + return (errorCode & 0x0008) != 0; // Bit 3 +} + +bool hasMosNTCError(uint16_t errorCode) { + return (errorCode & 0x0010) != 0; // Bit 4 +} + +bool hasBusVoltRangeError(uint16_t errorCode) { + return (errorCode & 0x0020) != 0; // Bit 5 +} + +bool hasBusVoltSampleError(uint16_t errorCode) { + return (errorCode & 0x0040) != 0; // Bit 6 +} + +bool hasMotorZLowError(uint16_t errorCode) { + return (errorCode & 0x0080) != 0; // Bit 7 +} + +bool hasMotorZHighError(uint16_t errorCode) { + return (errorCode & 0x0100) != 0; // Bit 8 +} + +bool hasMotorVDet1Error(uint16_t errorCode) { + return (errorCode & 0x0200) != 0; // Bit 9 +} + +bool hasMotorVDet2Error(uint16_t errorCode) { + return (errorCode & 0x0400) != 0; // Bit 10 +} + +bool hasMotorIDet2Error(uint16_t errorCode) { + return (errorCode & 0x0800) != 0; // Bit 11 +} + +bool hasSwHwIncompatError(uint16_t errorCode) { + return (errorCode & 0x2000) != 0; // Bit 13 +} + +bool hasBootloaderBadError(uint16_t errorCode) { + return (errorCode & 0x4000) != 0; // Bit 14 +} From 08704dfcbe53f258e1c25cc944a011018b32d5d3 Mon Sep 17 00:00:00 2001 From: Zach Whitehead Date: Fri, 10 Apr 2026 14:26:49 -0500 Subject: [PATCH 5/6] Add BLE core stubs and include ble_core.h --- test/test_screenshots/emulator_stubs.cpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/test/test_screenshots/emulator_stubs.cpp b/test/test_screenshots/emulator_stubs.cpp index c4fce593..01daa53f 100644 --- a/test/test_screenshots/emulator_stubs.cpp +++ b/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; } From 36643440a2ed1daab354a98dc910e9e6ae526b33 Mon Sep 17 00:00:00 2001 From: Zach Whitehead Date: Fri, 10 Apr 2026 14:48:33 -0500 Subject: [PATCH 6/6] Fix CRLF line endings in esc.cpp to match master --- src/sp140/esc.cpp | 1446 ++++++++++++++++++++++----------------------- 1 file changed, 723 insertions(+), 723 deletions(-) diff --git a/src/sp140/esc.cpp b/src/sp140/esc.cpp index 251e9355..d9183f4f 100644 --- a/src/sp140/esc.cpp +++ b/src/sp140/esc.cpp @@ -1,723 +1,723 @@ -#include "sp140/esc.h" -#include "sp140/globals.h" -#include - -#pragma GCC diagnostic ignored "-Wmissing-field-initializers" -#include "driver/twai.h" - -#define ESC_RX_PIN 3 // CAN RX pin to transceiver -#define ESC_TX_PIN 2 // CAN TX pin to transceiver -#define LOCAL_NODE_ID 0x01 // The ID on the network of this device - -#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; - -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, - .running_error = 0, - .selfcheck_error = 0 -}; - -/** - * Initialize the ESC communication system - * Sets up the CAN bus (TWAI) interface and configures the ESC with default settings - * Must be called before any other ESC functions - */ -void initESC() { - escTwaiInitialized = setupTWAI(); - if (!escTwaiInitialized) { - USBSerial.println("ESC TWAI Initialization failed. ESC will not function."); - return; // Can't proceed without TWAI driver - } - - adapter.begin(memory_pool, sizeof(memory_pool)); - adapter.setLocalNodeId(LOCAL_NODE_ID); - esc.begin(0x20); // Default ID for the ESC - - // Set idle throttle only if ESC is found - const uint16_t IdleThrottle_us = 10000; // 1000us (0.1us resolution) - esc.setThrottleSettings2(IdleThrottle_us); - adapter.processTxRxOnce(); - vTaskDelay(pdMS_TO_TICKS(20)); // Wait for ESC to process the command -} - -/** - * Set the ESC throttle value - * @param throttlePWM Throttle value in microseconds (1000-2000) - * 1000 = minimum throttle, 2000 = maximum throttle - * - * Important: The ESC requires messages at least every 300ms or it will reset - */ -void setESCThrottle(int throttlePWM) { - // Input validation - if (throttlePWM < 1000 || throttlePWM > 2000) { - return; // Ignore invalid throttle values - } - - // Direct calculation: multiply by 10 to convert μs to 0.1μs resolution - uint16_t scaledThrottle = throttlePWM * 10; - esc.setThrottleSettings2(scaledThrottle); -} - -/** - * Read telemetry data from the ESC - * Updates the global escTelemetryData structure with current values - * Should be called regularly (every 20-50ms) to maintain connection - * Also monitors connection state and sets NOT_CONNECTED if timeout occurs - */ -void readESCTelemetry() { - // Only proceed if TWAI is initialized - if (!escTwaiInitialized) { return; } // NOLINT(whitespace/newline) - - const SineEscModel &model = esc.getModel(); - - if (model.hasSetThrottleSettings2Response) { - const sine_esc_SetThrottleSettings2Response *res = &model.setThrottleSettings2Response; - - uint16_t rawTime10ms = res->time_10ms; - - // Check if the timestamp from the ESC has actually changed (stale-data guard) - if (rawTime10ms != sEscLastTime10ms) { - // Unwrap the uint16 counter using unsigned subtraction — naturally correct across rollover. - // e.g. last=65000, current=100: (uint16_t)(100-65000) = 636 ticks = 6360ms elapsed. - if (!sEscFirstUpdate) { - uint16_t deltaTicks = rawTime10ms - sEscLastTime10ms; - sEscAccumulatedRuntimeMs += (uint32_t)deltaTicks * 10UL; - } - sEscFirstUpdate = false; - sEscLastTime10ms = rawTime10ms; - - // Timestamp is new, process the telemetry data - escTelemetryData.lastUpdateMs = sEscAccumulatedRuntimeMs; - escTelemetryData.esc_runtime_ms = sEscAccumulatedRuntimeMs; - - // Update telemetry data - escTelemetryData.volts = res->voltage / 10.0f; - escTelemetryData.amps = res->bus_current / 10.0f; - escTelemetryData.mos_temp = res->mos_temp / 10.0f; - escTelemetryData.cap_temp = res->cap_temp / 10.0f; - escTelemetryData.mcu_temp = res->mcu_temp / 10.0f; - // Filter motor temp - only update if sensor is connected (valid range: -20°C to 140°C) - // Disconnected sensor reads ~149°C (thermistor pulled high) - float rawMotorTemp = res->motor_temp / 10.0f; - if (isMotorTempValidC(rawMotorTemp)) { - escTelemetryData.motor_temp = rawMotorTemp; - } else { - // Store invalid motor temp as NaN. Downstream consumers can skip on isnan(). - escTelemetryData.motor_temp = NAN; - } - escTelemetryData.phase_current = res->current / 10.0f; - escTelemetryData.eRPM = res->speed; - escTelemetryData.inPWM = res->recv_pwm / 10.0f; - escTelemetryData.comm_pwm = res->comm_pwm; - escTelemetryData.v_modulation = res->v_modulation; - watts = escTelemetryData.amps * escTelemetryData.volts; - - // Store error bitmasks - escTelemetryData.running_error = res->running_error; - escTelemetryData.selfcheck_error = res->selfcheck_error; - - // Record the time of this successful communication using the local clock - lastSuccessfulCommTimeMs = millis(); - } // else: Timestamp hasn't changed, treat as stale data, don't update local timer or telemetry - - } else { - // If we are connected but don't have a response, perhaps mark as stale or log? - // For now, do nothing - } - - // Update connection state based on time since last successful communication - unsigned long currentTimeMs = millis(); - if (lastSuccessfulCommTimeMs == 0 || (currentTimeMs - lastSuccessfulCommTimeMs) > TELEMETRY_TIMEOUT_MS) { - if (escTelemetryData.escState != TelemetryState::NOT_CONNECTED) { - // Log state change only if it actually changed - USBSerial.printf("ESC State: %d -> NOT_CONNECTED (Timeout)\n", static_cast(escTelemetryData.escState)); - escTelemetryData.escState = TelemetryState::NOT_CONNECTED; - // Reset runtime accumulator so it restarts from zero on reconnect. - // Keep sEscLastTime10ms at its last real value so the stale-data guard - // continues to reject the unchanged res->time_10ms from the SINE library. - sEscAccumulatedRuntimeMs = 0; - sEscFirstUpdate = true; - } - } else { - if (escTelemetryData.escState != TelemetryState::CONNECTED) { - // Log state change only if it actually changed - USBSerial.printf("ESC State: %d -> CONNECTED\n", static_cast(escTelemetryData.escState)); - escTelemetryData.escState = TelemetryState::CONNECTED; - // Auto-request hardware info on first connection if not already populated - if (escTelemetryData.hardware_id == 0) { - USBSerial.println("ESC: Auto-requesting hardware info"); - esc.getHardwareInfo(); - } - } - } - - // Process any pending hardware info request (set from BLE command or retry) - if (s_hwInfoRequested) { - s_hwInfoRequested = false; - USBSerial.println("ESC: Sending GetHwInfo (app-requested)"); - esc.getHardwareInfo(); - } - - // 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)); - } - - syncEscOutputs(); - adapter.processTxRxOnce(); // Process CAN messages -} - -/** - * Request ESC hardware info (HW ID, FW version, bootloader version, serial number). - * Thread-safe: sets a flag that is consumed by readESCTelemetry() on its next - * tick, keeping all CAN traffic on the throttle task. - * Called by the BLE command characteristic handler (0x01 from the app). - */ -void requestEscHardwareInfo() { - s_hwInfoRequested = true; -} - -/** - * Setup the ESP32 TWAI (Two-Wire Automotive Interface) for CAN communication - * Configures the CAN bus at 1Mbps for communication with the ESC - * @return true if setup was successful, false otherwise - */ -bool setupTWAI() { - // Check if already installed by checking status - twai_status_info_t status_info; - esp_err_t status_result = twai_get_status_info(&status_info); - - if (status_result == ESP_OK) { - // Driver is installed. We can check its state if needed. - USBSerial.printf("TWAI driver already installed. State: %d\n", status_info.state); - // If it's stopped, maybe we need to start it? For now, assume it's okay. - // if (status_info.state == TWAI_STATE_STOPPED) { twai_start(); } - return true; // Already initialized - } else if (status_result != ESP_ERR_INVALID_STATE) { - // An error other than "not installed" occurred - USBSerial.printf("Error checking TWAI status: %s\n", esp_err_to_name(status_result)); - return false; // Don't proceed if status check failed unexpectedly - } - // If status_result was ESP_ERR_INVALID_STATE, proceed with installation - - twai_general_config_t g_config = TWAI_GENERAL_CONFIG_DEFAULT( - (gpio_num_t)ESC_TX_PIN, - (gpio_num_t)ESC_RX_PIN, - TWAI_MODE_NORMAL); - twai_timing_config_t t_config = TWAI_TIMING_CONFIG_1MBITS(); - twai_filter_config_t f_config = TWAI_FILTER_CONFIG_ACCEPT_ALL(); - - esp_err_t install_err = twai_driver_install(&g_config, &t_config, &f_config); - if (install_err == ESP_OK) { - USBSerial.println("TWAI Driver installed"); - } else { - USBSerial.printf("Failed to install TWAI driver: %s\n", esp_err_to_name(install_err)); - return false; - } - - esp_err_t start_err = twai_start(); - if (start_err == ESP_OK) { - USBSerial.println("TWAI Driver started"); - } else { - USBSerial.printf("Failed to start TWAI driver: %s\n", esp_err_to_name(start_err)); - // Attempt to uninstall if start failed - twai_driver_uninstall(); - return false; - } - - // Reconfigure alerts to detect frame receive, Bus-Off error and RX queue full states - uint32_t alerts_to_enable = TWAI_ALERT_RX_DATA - | TWAI_ALERT_ERR_PASS - | TWAI_ALERT_BUS_ERROR - | TWAI_ALERT_RX_QUEUE_FULL; - if (twai_reconfigure_alerts(alerts_to_enable, NULL) == ESP_OK) { - USBSerial.println("CAN Alerts reconfigured"); - } else { - USBSerial.println("Failed to reconfigure CAN alerts"); - // Consider this non-fatal for now? Or return false? - } - - 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 - */ -void dumpThrottleResponse(const sine_esc_SetThrottleSettings2Response *res) { - USBSerial.println("Got SetThrottleSettings2 response"); - - USBSerial.print("\trecv_pwm: "); - USBSerial.println(res->recv_pwm); - - USBSerial.print("\tcomm_pwm: "); - USBSerial.println(res->comm_pwm); - - USBSerial.print("\tspeed: "); - USBSerial.println(res->speed); - - USBSerial.print("\tcurrent: "); - USBSerial.println(res->current); - - USBSerial.print("\tbus_current: "); - USBSerial.println(res->bus_current); - - USBSerial.print("\tvoltage: "); - USBSerial.println(res->voltage); - - USBSerial.print("\tv_modulation: "); - USBSerial.println(res->v_modulation); - - USBSerial.print("\tmos_temp: "); - USBSerial.println(res->mos_temp); - - USBSerial.print("\tcap_temp: "); - USBSerial.println(res->cap_temp); - - USBSerial.print("\tmcu_temp: "); - USBSerial.println(res->mcu_temp); - - USBSerial.print("\trunning_error: "); - USBSerial.print(res->running_error); - USBSerial.print(" ("); - USBSerial.print(decodeRunningError(res->running_error)); - USBSerial.println(")"); - - USBSerial.print("\tselfcheck_error: "); - USBSerial.print(res->selfcheck_error); - USBSerial.print(" ("); - USBSerial.print(decodeSelfCheckError(res->selfcheck_error)); - USBSerial.println(")"); - - USBSerial.print("\tmotor_temp: "); - USBSerial.println(res->motor_temp); - - USBSerial.print("\ttime_10ms: "); - USBSerial.println(res->time_10ms); -} - -/** - * Debug function to dump all available ESC messages to serial - * Displays hardware info, throttle response, and rotation speed settings if available - */ -void dumpESCMessages(void) { - const SineEscModel &model = esc.getModel(); - - if (model.hasGetHardwareInfoResponse) { - USBSerial.println("Got HwInfo response"); - - const sine_esc_GetHwInfoResponse *b = &model.getHardwareInfoResponse; - USBSerial.print("\thardware_id: "); - USBSerial.println(b->hardware_id, HEX); - USBSerial.print("\tbootloader_version: "); - USBSerial.println(b->bootloader_version, HEX); - USBSerial.print("\tapp_version: "); - USBSerial.println(b->app_version, HEX); - } - - if (model.hasSetThrottleSettings2Response) { - dumpThrottleResponse(&model.setThrottleSettings2Response); - } - - if (model.hasSetRotationSpeedSettingsResponse) { - USBSerial.println("Got SetRotationSpeedSettings response"); - } -} - -/** - * Map a double value from one range to another - * @param x Input value to map - * @param in_min Minimum of input range - * @param in_max Maximum of input range - * @param out_min Minimum of output range - * @param out_max Maximum of output range - * @return Mapped value in output range - */ -double mapDouble(double x, double in_min, double in_max, double out_min, double out_max) { - return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; -} - -/** - * Decode running error bitmask into human-readable string - * @param errorCode 16-bit running error code from ESC - * @return String containing decoded error messages - */ -String decodeRunningError(uint16_t errorCode) { - if (errorCode == 0) { - return "no_errors"; - } - - String result = ""; - bool firstError = true; - - // Define error messages for each bit - const char* errorMessages[] = { - "over_current_protect", // Bit 0: Over current/short circuit protection occurs - "locked_rotor_protect", // Bit 1: Locked-rotor protection occurs - "over_temp_protect", // Bit 2: Over-temperature protection - "pwm_throttle_lost", // Bit 3: PWM throttle lost pulse - "no_load", // Bit 4: No load - "throttle_saturation", // Bit 5: Throttle saturation - "over_volt_protect", // Bit 6: Over voltage protection - "voltage_drop", // Bit 7: Voltage drop - "comm_throttle_loss", // Bit 8: Communication throttle loss - "undef_9", // Bit 9: Undefined - "undef_10", // Bit 10: Undefined - "undef_11", // Bit 11: Undefined - "undef_12", // Bit 12: Undefined - "undef_13", // Bit 13: Undefined - "undef_14", // Bit 14: Undefined - "undef_15" // Bit 15: Undefined - }; - - for (int i = 0; i < 16; i++) { - if (errorCode & (1 << i)) { - if (!firstError) { - result += ", "; - } - result += errorMessages[i]; - firstError = false; - } - } - - return result; -} - -/** - * Decode self-check error bitmask into human-readable string - * @param errorCode 16-bit self-check error code from ESC - * @return String containing decoded error messages - */ -String decodeSelfCheckError(uint16_t errorCode) { - if (errorCode == 0) { - return "no_errors"; - } - - String result = ""; - bool firstError = true; - - // Define error messages for each bit - const char* errorMessages[] = { - "motor_i_out_bad", // Bit 0: Motor line current output abnormal - "total_i_out_bad", // Bit 1: Total current output abnormal - "motor_v_out_bad", // Bit 2: Motor line voltage abnormal - "cap_ntc_bad", // Bit 3: Electrolytic capacitor NTC output abnormal - "mos_ntc_bad", // Bit 4: MOS Tube NTC output abnormal - "bus_v_range", // Bit 5: Bus voltage over/under voltage - "bus_v_sample_bad", // Bit 6: Bus voltage sampling abnormal - "motor_z_too_low", // Bit 7: Motor wire loop impedance too low - "motor_z_too_high", // Bit 8: Motor wire loop impedance too large - "motor_v_det1_bad", // Bit 9: Motor line voltage detection circuit abnormal 1 - "motor_v_det2_bad", // Bit 10: Motor line voltage detection circuit abnormal 2 - "motor_i_det2_bad", // Bit 11: Motor line current detection circuit abnormal 02 - "undef_12", // Bit 12: undefined - "sw_hw_incompatible", // Bit 13: Software and hardware versions incompatible - "bootloader_unsupported", // Bit 14: Boot loader unsupported - "undef_15" // Bit 15: Undefined - }; - - for (int i = 0; i < 16; i++) { - if (errorCode & (1 << i)) { - if (!firstError) { - result += ", "; - } - result += errorMessages[i]; - firstError = false; - } - } - - return result; -} - -/** - * Check if there are any running errors - * @param errorCode 16-bit running error code from ESC - * @return true if any error bits are set - */ -bool hasRunningError(uint16_t errorCode) { - return errorCode != 0; -} - -/** - * Check if there are any self-check errors - * @param errorCode 16-bit self-check error code from ESC - * @return true if any error bits are set - */ -bool hasSelfCheckError(uint16_t errorCode) { - return errorCode != 0; -} - -/** - * Check if there are any critical running errors - * Critical errors are high-priority faults that require immediate attention - * @param errorCode 16-bit running error code from ESC - * @return true if any critical error bits are set - */ -bool hasCriticalRunningError(uint16_t errorCode) { - // Define critical error bits (High level errors from documentation) - // Bits 0,1,2,6,7 are High priority and relevant for CAN communication - const uint16_t criticalBits = 0x00C7; // Bits 0,1,2,6,7 (High priority) - // Excluded bits: - // - Bit 3 (pwm_throttle_lost): Not relevant for CAN communication - // - Bit 4 (no_load): Low priority - // - Bit 5 (throttle_saturation): Middle priority - // - Bit 8 (comm_throttle_loss): Low priority, expected with CAN - - return (errorCode & criticalBits) != 0; -} - -/** - * Check if there are any warning-level running errors - * Warning errors are middle-priority faults that should be monitored but aren't critical - * @param errorCode 16-bit running error code from ESC - * @return true if any warning error bits are set - */ -bool hasWarningRunningError(uint16_t errorCode) { - // Bit 5 (throttle_saturation) is Middle priority - treat as warning - const uint16_t warningBits = 0x0020; // Bit 5 only - - return (errorCode & warningBits) != 0; -} - -/** - * Check if there are any critical self-check errors - * All self-check errors are considered critical as they indicate hardware issues - * @param errorCode 16-bit self-check error code from ESC - * @return true if any error bits are set (all self-check errors are critical) - */ -bool hasCriticalSelfCheckError(uint16_t errorCode) { - return errorCode != 0; // All self-check errors are critical -} - -// Individual error bit checkers for specific monitoring -bool hasOverCurrentError(uint16_t errorCode) { - return (errorCode & 0x0001) != 0; // Bit 0 -} - -bool hasLockedRotorError(uint16_t errorCode) { - return (errorCode & 0x0002) != 0; // Bit 1 -} - -bool hasOverTempError(uint16_t errorCode) { - return (errorCode & 0x0004) != 0; // Bit 2 -} - -bool hasOverVoltError(uint16_t errorCode) { - return (errorCode & 0x0040) != 0; // Bit 6 -} - -bool hasVoltagDropError(uint16_t errorCode) { - return (errorCode & 0x0080) != 0; // Bit 7 -} - -bool hasThrottleSatWarning(uint16_t errorCode) { - return (errorCode & 0x0020) != 0; // Bit 5 -} - -// Individual self-check error bit checkers -bool hasMotorCurrentOutError(uint16_t errorCode) { - return (errorCode & 0x0001) != 0; // Bit 0 -} - -bool hasTotalCurrentOutError(uint16_t errorCode) { - return (errorCode & 0x0002) != 0; // Bit 1 -} - -bool hasMotorVoltageOutError(uint16_t errorCode) { - return (errorCode & 0x0004) != 0; // Bit 2 -} - -bool hasCapNTCError(uint16_t errorCode) { - return (errorCode & 0x0008) != 0; // Bit 3 -} - -bool hasMosNTCError(uint16_t errorCode) { - return (errorCode & 0x0010) != 0; // Bit 4 -} - -bool hasBusVoltRangeError(uint16_t errorCode) { - return (errorCode & 0x0020) != 0; // Bit 5 -} - -bool hasBusVoltSampleError(uint16_t errorCode) { - return (errorCode & 0x0040) != 0; // Bit 6 -} - -bool hasMotorZLowError(uint16_t errorCode) { - return (errorCode & 0x0080) != 0; // Bit 7 -} - -bool hasMotorZHighError(uint16_t errorCode) { - return (errorCode & 0x0100) != 0; // Bit 8 -} - -bool hasMotorVDet1Error(uint16_t errorCode) { - return (errorCode & 0x0200) != 0; // Bit 9 -} - -bool hasMotorVDet2Error(uint16_t errorCode) { - return (errorCode & 0x0400) != 0; // Bit 10 -} - -bool hasMotorIDet2Error(uint16_t errorCode) { - return (errorCode & 0x0800) != 0; // Bit 11 -} - -bool hasSwHwIncompatError(uint16_t errorCode) { - return (errorCode & 0x2000) != 0; // Bit 13 -} - -bool hasBootloaderBadError(uint16_t errorCode) { - return (errorCode & 0x4000) != 0; // Bit 14 -} +#include "sp140/esc.h" +#include "sp140/globals.h" +#include + +#pragma GCC diagnostic ignored "-Wmissing-field-initializers" +#include "driver/twai.h" + +#define ESC_RX_PIN 3 // CAN RX pin to transceiver +#define ESC_TX_PIN 2 // CAN TX pin to transceiver +#define LOCAL_NODE_ID 0x01 // The ID on the network of this device + +#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; + +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, + .running_error = 0, + .selfcheck_error = 0 +}; + +/** + * Initialize the ESC communication system + * Sets up the CAN bus (TWAI) interface and configures the ESC with default settings + * Must be called before any other ESC functions + */ +void initESC() { + escTwaiInitialized = setupTWAI(); + if (!escTwaiInitialized) { + USBSerial.println("ESC TWAI Initialization failed. ESC will not function."); + return; // Can't proceed without TWAI driver + } + + adapter.begin(memory_pool, sizeof(memory_pool)); + adapter.setLocalNodeId(LOCAL_NODE_ID); + esc.begin(0x20); // Default ID for the ESC + + // Set idle throttle only if ESC is found + const uint16_t IdleThrottle_us = 10000; // 1000us (0.1us resolution) + esc.setThrottleSettings2(IdleThrottle_us); + adapter.processTxRxOnce(); + vTaskDelay(pdMS_TO_TICKS(20)); // Wait for ESC to process the command +} + +/** + * Set the ESC throttle value + * @param throttlePWM Throttle value in microseconds (1000-2000) + * 1000 = minimum throttle, 2000 = maximum throttle + * + * Important: The ESC requires messages at least every 300ms or it will reset + */ +void setESCThrottle(int throttlePWM) { + // Input validation + if (throttlePWM < 1000 || throttlePWM > 2000) { + return; // Ignore invalid throttle values + } + + // Direct calculation: multiply by 10 to convert μs to 0.1μs resolution + uint16_t scaledThrottle = throttlePWM * 10; + esc.setThrottleSettings2(scaledThrottle); +} + +/** + * Read telemetry data from the ESC + * Updates the global escTelemetryData structure with current values + * Should be called regularly (every 20-50ms) to maintain connection + * Also monitors connection state and sets NOT_CONNECTED if timeout occurs + */ +void readESCTelemetry() { + // Only proceed if TWAI is initialized + if (!escTwaiInitialized) { return; } // NOLINT(whitespace/newline) + + const SineEscModel &model = esc.getModel(); + + if (model.hasSetThrottleSettings2Response) { + const sine_esc_SetThrottleSettings2Response *res = &model.setThrottleSettings2Response; + + uint16_t rawTime10ms = res->time_10ms; + + // Check if the timestamp from the ESC has actually changed (stale-data guard) + if (rawTime10ms != sEscLastTime10ms) { + // Unwrap the uint16 counter using unsigned subtraction — naturally correct across rollover. + // e.g. last=65000, current=100: (uint16_t)(100-65000) = 636 ticks = 6360ms elapsed. + if (!sEscFirstUpdate) { + uint16_t deltaTicks = rawTime10ms - sEscLastTime10ms; + sEscAccumulatedRuntimeMs += (uint32_t)deltaTicks * 10UL; + } + sEscFirstUpdate = false; + sEscLastTime10ms = rawTime10ms; + + // Timestamp is new, process the telemetry data + escTelemetryData.lastUpdateMs = sEscAccumulatedRuntimeMs; + escTelemetryData.esc_runtime_ms = sEscAccumulatedRuntimeMs; + + // Update telemetry data + escTelemetryData.volts = res->voltage / 10.0f; + escTelemetryData.amps = res->bus_current / 10.0f; + escTelemetryData.mos_temp = res->mos_temp / 10.0f; + escTelemetryData.cap_temp = res->cap_temp / 10.0f; + escTelemetryData.mcu_temp = res->mcu_temp / 10.0f; + // Filter motor temp - only update if sensor is connected (valid range: -20°C to 140°C) + // Disconnected sensor reads ~149°C (thermistor pulled high) + float rawMotorTemp = res->motor_temp / 10.0f; + if (isMotorTempValidC(rawMotorTemp)) { + escTelemetryData.motor_temp = rawMotorTemp; + } else { + // Store invalid motor temp as NaN. Downstream consumers can skip on isnan(). + escTelemetryData.motor_temp = NAN; + } + escTelemetryData.phase_current = res->current / 10.0f; + escTelemetryData.eRPM = res->speed; + escTelemetryData.inPWM = res->recv_pwm / 10.0f; + escTelemetryData.comm_pwm = res->comm_pwm; + escTelemetryData.v_modulation = res->v_modulation; + watts = escTelemetryData.amps * escTelemetryData.volts; + + // Store error bitmasks + escTelemetryData.running_error = res->running_error; + escTelemetryData.selfcheck_error = res->selfcheck_error; + + // Record the time of this successful communication using the local clock + lastSuccessfulCommTimeMs = millis(); + } // else: Timestamp hasn't changed, treat as stale data, don't update local timer or telemetry + + } else { + // If we are connected but don't have a response, perhaps mark as stale or log? + // For now, do nothing + } + + // Update connection state based on time since last successful communication + unsigned long currentTimeMs = millis(); + if (lastSuccessfulCommTimeMs == 0 || (currentTimeMs - lastSuccessfulCommTimeMs) > TELEMETRY_TIMEOUT_MS) { + if (escTelemetryData.escState != TelemetryState::NOT_CONNECTED) { + // Log state change only if it actually changed + USBSerial.printf("ESC State: %d -> NOT_CONNECTED (Timeout)\n", static_cast(escTelemetryData.escState)); + escTelemetryData.escState = TelemetryState::NOT_CONNECTED; + // Reset runtime accumulator so it restarts from zero on reconnect. + // Keep sEscLastTime10ms at its last real value so the stale-data guard + // continues to reject the unchanged res->time_10ms from the SINE library. + sEscAccumulatedRuntimeMs = 0; + sEscFirstUpdate = true; + } + } else { + if (escTelemetryData.escState != TelemetryState::CONNECTED) { + // Log state change only if it actually changed + USBSerial.printf("ESC State: %d -> CONNECTED\n", static_cast(escTelemetryData.escState)); + escTelemetryData.escState = TelemetryState::CONNECTED; + // Auto-request hardware info on first connection if not already populated + if (escTelemetryData.hardware_id == 0) { + USBSerial.println("ESC: Auto-requesting hardware info"); + esc.getHardwareInfo(); + } + } + } + + // Process any pending hardware info request (set from BLE command or retry) + if (s_hwInfoRequested) { + s_hwInfoRequested = false; + USBSerial.println("ESC: Sending GetHwInfo (app-requested)"); + esc.getHardwareInfo(); + } + + // 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)); + } + + syncEscOutputs(); + adapter.processTxRxOnce(); // Process CAN messages +} + +/** + * Request ESC hardware info (HW ID, FW version, bootloader version, serial number). + * Thread-safe: sets a flag that is consumed by readESCTelemetry() on its next + * tick, keeping all CAN traffic on the throttle task. + * Called by the BLE command characteristic handler (0x01 from the app). + */ +void requestEscHardwareInfo() { + s_hwInfoRequested = true; +} + +/** + * Setup the ESP32 TWAI (Two-Wire Automotive Interface) for CAN communication + * Configures the CAN bus at 1Mbps for communication with the ESC + * @return true if setup was successful, false otherwise + */ +bool setupTWAI() { + // Check if already installed by checking status + twai_status_info_t status_info; + esp_err_t status_result = twai_get_status_info(&status_info); + + if (status_result == ESP_OK) { + // Driver is installed. We can check its state if needed. + USBSerial.printf("TWAI driver already installed. State: %d\n", status_info.state); + // If it's stopped, maybe we need to start it? For now, assume it's okay. + // if (status_info.state == TWAI_STATE_STOPPED) { twai_start(); } + return true; // Already initialized + } else if (status_result != ESP_ERR_INVALID_STATE) { + // An error other than "not installed" occurred + USBSerial.printf("Error checking TWAI status: %s\n", esp_err_to_name(status_result)); + return false; // Don't proceed if status check failed unexpectedly + } + // If status_result was ESP_ERR_INVALID_STATE, proceed with installation + + twai_general_config_t g_config = TWAI_GENERAL_CONFIG_DEFAULT( + (gpio_num_t)ESC_TX_PIN, + (gpio_num_t)ESC_RX_PIN, + TWAI_MODE_NORMAL); + twai_timing_config_t t_config = TWAI_TIMING_CONFIG_1MBITS(); + twai_filter_config_t f_config = TWAI_FILTER_CONFIG_ACCEPT_ALL(); + + esp_err_t install_err = twai_driver_install(&g_config, &t_config, &f_config); + if (install_err == ESP_OK) { + USBSerial.println("TWAI Driver installed"); + } else { + USBSerial.printf("Failed to install TWAI driver: %s\n", esp_err_to_name(install_err)); + return false; + } + + esp_err_t start_err = twai_start(); + if (start_err == ESP_OK) { + USBSerial.println("TWAI Driver started"); + } else { + USBSerial.printf("Failed to start TWAI driver: %s\n", esp_err_to_name(start_err)); + // Attempt to uninstall if start failed + twai_driver_uninstall(); + return false; + } + + // Reconfigure alerts to detect frame receive, Bus-Off error and RX queue full states + uint32_t alerts_to_enable = TWAI_ALERT_RX_DATA + | TWAI_ALERT_ERR_PASS + | TWAI_ALERT_BUS_ERROR + | TWAI_ALERT_RX_QUEUE_FULL; + if (twai_reconfigure_alerts(alerts_to_enable, NULL) == ESP_OK) { + USBSerial.println("CAN Alerts reconfigured"); + } else { + USBSerial.println("Failed to reconfigure CAN alerts"); + // Consider this non-fatal for now? Or return false? + } + + 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 + */ +void dumpThrottleResponse(const sine_esc_SetThrottleSettings2Response *res) { + USBSerial.println("Got SetThrottleSettings2 response"); + + USBSerial.print("\trecv_pwm: "); + USBSerial.println(res->recv_pwm); + + USBSerial.print("\tcomm_pwm: "); + USBSerial.println(res->comm_pwm); + + USBSerial.print("\tspeed: "); + USBSerial.println(res->speed); + + USBSerial.print("\tcurrent: "); + USBSerial.println(res->current); + + USBSerial.print("\tbus_current: "); + USBSerial.println(res->bus_current); + + USBSerial.print("\tvoltage: "); + USBSerial.println(res->voltage); + + USBSerial.print("\tv_modulation: "); + USBSerial.println(res->v_modulation); + + USBSerial.print("\tmos_temp: "); + USBSerial.println(res->mos_temp); + + USBSerial.print("\tcap_temp: "); + USBSerial.println(res->cap_temp); + + USBSerial.print("\tmcu_temp: "); + USBSerial.println(res->mcu_temp); + + USBSerial.print("\trunning_error: "); + USBSerial.print(res->running_error); + USBSerial.print(" ("); + USBSerial.print(decodeRunningError(res->running_error)); + USBSerial.println(")"); + + USBSerial.print("\tselfcheck_error: "); + USBSerial.print(res->selfcheck_error); + USBSerial.print(" ("); + USBSerial.print(decodeSelfCheckError(res->selfcheck_error)); + USBSerial.println(")"); + + USBSerial.print("\tmotor_temp: "); + USBSerial.println(res->motor_temp); + + USBSerial.print("\ttime_10ms: "); + USBSerial.println(res->time_10ms); +} + +/** + * Debug function to dump all available ESC messages to serial + * Displays hardware info, throttle response, and rotation speed settings if available + */ +void dumpESCMessages(void) { + const SineEscModel &model = esc.getModel(); + + if (model.hasGetHardwareInfoResponse) { + USBSerial.println("Got HwInfo response"); + + const sine_esc_GetHwInfoResponse *b = &model.getHardwareInfoResponse; + USBSerial.print("\thardware_id: "); + USBSerial.println(b->hardware_id, HEX); + USBSerial.print("\tbootloader_version: "); + USBSerial.println(b->bootloader_version, HEX); + USBSerial.print("\tapp_version: "); + USBSerial.println(b->app_version, HEX); + } + + if (model.hasSetThrottleSettings2Response) { + dumpThrottleResponse(&model.setThrottleSettings2Response); + } + + if (model.hasSetRotationSpeedSettingsResponse) { + USBSerial.println("Got SetRotationSpeedSettings response"); + } +} + +/** + * Map a double value from one range to another + * @param x Input value to map + * @param in_min Minimum of input range + * @param in_max Maximum of input range + * @param out_min Minimum of output range + * @param out_max Maximum of output range + * @return Mapped value in output range + */ +double mapDouble(double x, double in_min, double in_max, double out_min, double out_max) { + return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; +} + +/** + * Decode running error bitmask into human-readable string + * @param errorCode 16-bit running error code from ESC + * @return String containing decoded error messages + */ +String decodeRunningError(uint16_t errorCode) { + if (errorCode == 0) { + return "no_errors"; + } + + String result = ""; + bool firstError = true; + + // Define error messages for each bit + const char* errorMessages[] = { + "over_current_protect", // Bit 0: Over current/short circuit protection occurs + "locked_rotor_protect", // Bit 1: Locked-rotor protection occurs + "over_temp_protect", // Bit 2: Over-temperature protection + "pwm_throttle_lost", // Bit 3: PWM throttle lost pulse + "no_load", // Bit 4: No load + "throttle_saturation", // Bit 5: Throttle saturation + "over_volt_protect", // Bit 6: Over voltage protection + "voltage_drop", // Bit 7: Voltage drop + "comm_throttle_loss", // Bit 8: Communication throttle loss + "undef_9", // Bit 9: Undefined + "undef_10", // Bit 10: Undefined + "undef_11", // Bit 11: Undefined + "undef_12", // Bit 12: Undefined + "undef_13", // Bit 13: Undefined + "undef_14", // Bit 14: Undefined + "undef_15" // Bit 15: Undefined + }; + + for (int i = 0; i < 16; i++) { + if (errorCode & (1 << i)) { + if (!firstError) { + result += ", "; + } + result += errorMessages[i]; + firstError = false; + } + } + + return result; +} + +/** + * Decode self-check error bitmask into human-readable string + * @param errorCode 16-bit self-check error code from ESC + * @return String containing decoded error messages + */ +String decodeSelfCheckError(uint16_t errorCode) { + if (errorCode == 0) { + return "no_errors"; + } + + String result = ""; + bool firstError = true; + + // Define error messages for each bit + const char* errorMessages[] = { + "motor_i_out_bad", // Bit 0: Motor line current output abnormal + "total_i_out_bad", // Bit 1: Total current output abnormal + "motor_v_out_bad", // Bit 2: Motor line voltage abnormal + "cap_ntc_bad", // Bit 3: Electrolytic capacitor NTC output abnormal + "mos_ntc_bad", // Bit 4: MOS Tube NTC output abnormal + "bus_v_range", // Bit 5: Bus voltage over/under voltage + "bus_v_sample_bad", // Bit 6: Bus voltage sampling abnormal + "motor_z_too_low", // Bit 7: Motor wire loop impedance too low + "motor_z_too_high", // Bit 8: Motor wire loop impedance too large + "motor_v_det1_bad", // Bit 9: Motor line voltage detection circuit abnormal 1 + "motor_v_det2_bad", // Bit 10: Motor line voltage detection circuit abnormal 2 + "motor_i_det2_bad", // Bit 11: Motor line current detection circuit abnormal 02 + "undef_12", // Bit 12: undefined + "sw_hw_incompatible", // Bit 13: Software and hardware versions incompatible + "bootloader_unsupported", // Bit 14: Boot loader unsupported + "undef_15" // Bit 15: Undefined + }; + + for (int i = 0; i < 16; i++) { + if (errorCode & (1 << i)) { + if (!firstError) { + result += ", "; + } + result += errorMessages[i]; + firstError = false; + } + } + + return result; +} + +/** + * Check if there are any running errors + * @param errorCode 16-bit running error code from ESC + * @return true if any error bits are set + */ +bool hasRunningError(uint16_t errorCode) { + return errorCode != 0; +} + +/** + * Check if there are any self-check errors + * @param errorCode 16-bit self-check error code from ESC + * @return true if any error bits are set + */ +bool hasSelfCheckError(uint16_t errorCode) { + return errorCode != 0; +} + +/** + * Check if there are any critical running errors + * Critical errors are high-priority faults that require immediate attention + * @param errorCode 16-bit running error code from ESC + * @return true if any critical error bits are set + */ +bool hasCriticalRunningError(uint16_t errorCode) { + // Define critical error bits (High level errors from documentation) + // Bits 0,1,2,6,7 are High priority and relevant for CAN communication + const uint16_t criticalBits = 0x00C7; // Bits 0,1,2,6,7 (High priority) + // Excluded bits: + // - Bit 3 (pwm_throttle_lost): Not relevant for CAN communication + // - Bit 4 (no_load): Low priority + // - Bit 5 (throttle_saturation): Middle priority + // - Bit 8 (comm_throttle_loss): Low priority, expected with CAN + + return (errorCode & criticalBits) != 0; +} + +/** + * Check if there are any warning-level running errors + * Warning errors are middle-priority faults that should be monitored but aren't critical + * @param errorCode 16-bit running error code from ESC + * @return true if any warning error bits are set + */ +bool hasWarningRunningError(uint16_t errorCode) { + // Bit 5 (throttle_saturation) is Middle priority - treat as warning + const uint16_t warningBits = 0x0020; // Bit 5 only + + return (errorCode & warningBits) != 0; +} + +/** + * Check if there are any critical self-check errors + * All self-check errors are considered critical as they indicate hardware issues + * @param errorCode 16-bit self-check error code from ESC + * @return true if any error bits are set (all self-check errors are critical) + */ +bool hasCriticalSelfCheckError(uint16_t errorCode) { + return errorCode != 0; // All self-check errors are critical +} + +// Individual error bit checkers for specific monitoring +bool hasOverCurrentError(uint16_t errorCode) { + return (errorCode & 0x0001) != 0; // Bit 0 +} + +bool hasLockedRotorError(uint16_t errorCode) { + return (errorCode & 0x0002) != 0; // Bit 1 +} + +bool hasOverTempError(uint16_t errorCode) { + return (errorCode & 0x0004) != 0; // Bit 2 +} + +bool hasOverVoltError(uint16_t errorCode) { + return (errorCode & 0x0040) != 0; // Bit 6 +} + +bool hasVoltagDropError(uint16_t errorCode) { + return (errorCode & 0x0080) != 0; // Bit 7 +} + +bool hasThrottleSatWarning(uint16_t errorCode) { + return (errorCode & 0x0020) != 0; // Bit 5 +} + +// Individual self-check error bit checkers +bool hasMotorCurrentOutError(uint16_t errorCode) { + return (errorCode & 0x0001) != 0; // Bit 0 +} + +bool hasTotalCurrentOutError(uint16_t errorCode) { + return (errorCode & 0x0002) != 0; // Bit 1 +} + +bool hasMotorVoltageOutError(uint16_t errorCode) { + return (errorCode & 0x0004) != 0; // Bit 2 +} + +bool hasCapNTCError(uint16_t errorCode) { + return (errorCode & 0x0008) != 0; // Bit 3 +} + +bool hasMosNTCError(uint16_t errorCode) { + return (errorCode & 0x0010) != 0; // Bit 4 +} + +bool hasBusVoltRangeError(uint16_t errorCode) { + return (errorCode & 0x0020) != 0; // Bit 5 +} + +bool hasBusVoltSampleError(uint16_t errorCode) { + return (errorCode & 0x0040) != 0; // Bit 6 +} + +bool hasMotorZLowError(uint16_t errorCode) { + return (errorCode & 0x0080) != 0; // Bit 7 +} + +bool hasMotorZHighError(uint16_t errorCode) { + return (errorCode & 0x0100) != 0; // Bit 8 +} + +bool hasMotorVDet1Error(uint16_t errorCode) { + return (errorCode & 0x0200) != 0; // Bit 9 +} + +bool hasMotorVDet2Error(uint16_t errorCode) { + return (errorCode & 0x0400) != 0; // Bit 10 +} + +bool hasMotorIDet2Error(uint16_t errorCode) { + return (errorCode & 0x0800) != 0; // Bit 11 +} + +bool hasSwHwIncompatError(uint16_t errorCode) { + return (errorCode & 0x2000) != 0; // Bit 13 +} + +bool hasBootloaderBadError(uint16_t errorCode) { + return (errorCode & 0x4000) != 0; // Bit 14 +}