Skip to content

Commit 96d643d

Browse files
zjwhiteheadzjwhitehead
committed
Add ESC status light and motor beep control
Introduce ESC status light modes and motor beep queuing. Add EscStatusLightMode and PendingEscTone enums, requestEscStatusLightMode(), queueEscMotorBeepArm()/Disarm(), and syncEscOutputs() to manage LED patterns and motor tones via the SINE ESC interface. Wire up calls: sync outputs from readESCTelemetry(), request ESC mode in syncESCTelemetry(), and queue beeps on arm/disarm. Rename controller LED color macros (STATUS_LED_*) and update usages, and bump SINE-ESC-CAN dependency hash in platformio.ini. Minor formatting and telemetry integration changes across esc.h, esc.cpp, utilities.h, and sp140.ino.
1 parent 7aa6e9c commit 96d643d

5 files changed

Lines changed: 195 additions & 44 deletions

File tree

‎inc/sp140/esc.h‎

Lines changed: 23 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -3,25 +3,32 @@
33

44
#include <Arduino.h>
55

6-
// Motor temp validity range (disconnected/invalid readings are represented as NaN)
7-
constexpr float MOTOR_TEMP_VALID_MIN_C = -20.0f;
8-
constexpr float MOTOR_TEMP_VALID_MAX_C = 140.0f;
9-
10-
inline bool isMotorTempValidC(float tempC) {
11-
return tempC > MOTOR_TEMP_VALID_MIN_C && tempC <= MOTOR_TEMP_VALID_MAX_C;
12-
}
6+
// Motor temp validity range (disconnected/invalid readings are represented as NaN)
7+
constexpr float MOTOR_TEMP_VALID_MIN_C = -20.0f;
8+
constexpr float MOTOR_TEMP_VALID_MAX_C = 140.0f;
9+
10+
inline bool isMotorTempValidC(float tempC) {
11+
return tempC > MOTOR_TEMP_VALID_MIN_C && tempC <= MOTOR_TEMP_VALID_MAX_C;
12+
}
1313
#include "sp140/structs.h"
1414
#include "../../inc/sp140/esp32s3-config.h"
1515
#include <SineEsc.h>
1616
#include <CanardAdapter.h>
1717

18+
enum class EscStatusLightMode : uint8_t {
19+
OFF = 0,
20+
READY,
21+
FLIGHT,
22+
CAUTION,
23+
};
24+
1825
void initESC();
1926
void setESCThrottle(int throttlePWM);
2027
void readESCTelemetry();
2128
bool setupTWAI();
2229

23-
// ESC Error Decoding Functions
24-
String decodeRunningError(uint16_t errorCode);
30+
// ESC Error Decoding Functions
31+
String decodeRunningError(uint16_t errorCode);
2532
String decodeSelfCheckError(uint16_t errorCode);
2633
bool hasRunningError(uint16_t errorCode);
2734
bool hasSelfCheckError(uint16_t errorCode);
@@ -53,6 +60,13 @@ bool hasMotorIDet2Error(uint16_t errorCode);
5360
bool hasSwHwIncompatError(uint16_t errorCode);
5461
bool hasBootloaderBadError(uint16_t errorCode);
5562

63+
// ESC LED control
64+
void requestEscStatusLightMode(EscStatusLightMode mode);
65+
66+
// ESC motor beep
67+
void queueEscMotorBeepArm();
68+
void queueEscMotorBeepDisarm();
69+
5670
// for debugging
5771
void dumpThrottleResponse(const sine_esc_SetThrottleSettings2Response *res);
5872
void dumpESCMessages(); // dumps all messages to USBSerial

‎inc/sp140/utilities.h‎

Lines changed: 10 additions & 18 deletions
Original file line numberDiff line numberDiff line change
@@ -1,22 +1,14 @@
1-
#ifndef INC_SP140_UTILITIES_H_
2-
#define INC_SP140_UTILITIES_H_
3-
4-
#include <Arduino.h>
5-
6-
// Function to get unique chip ID
7-
String chipId();
1+
#ifndef INC_SP140_UTILITIES_H_
2+
#define INC_SP140_UTILITIES_H_
83

9-
// Definitions for main rainbow colors in WRGB format for NeoPixel.
10-
// The 32-bit color value is WRGB. W (White) is ignored for RGB pixels.
11-
// The next bytes are R (Red), G (Green), and B (Blue).
12-
// For example, YELLOW is 0x00FFFF00, with FF for Red and Green, and 00 for Blue.
4+
#include <Arduino.h>
135

14-
#define LED_RED 0x00FF0000
15-
#define LED_ORANGE 0x00FF7F00
16-
#define LED_YELLOW 0x00FFFF00
17-
#define LED_GREEN 0x0000FF00
18-
#define LED_BLUE 0x000000FF
19-
#define LED_INDIGO 0x004B0082
20-
#define LED_VIOLET 0x008000FF
6+
// Function to get unique chip ID
7+
String chipId();
8+
9+
// Controller status LED colors in WRGB format for NeoPixel.
10+
#define STATUS_LED_RED 0x00FF0000
11+
#define STATUS_LED_YELLOW 0x00FFFF00
12+
#define STATUS_LED_GREEN 0x0000FF00
2113

2214
#endif // INC_SP140_UTILITIES_H_

‎platformio.ini‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -52,7 +52,7 @@ lib_deps =
5252
adafruit/Adafruit CAN@0.2.3
5353
adafruit/Adafruit MCP2515@0.2.1
5454
https://github.com/rlogiacco/CircularBuffer@1.4.0
55-
https://github.com/openppg/SINE-ESC-CAN#8caa93996b5d000fe10ca5265bd1c472dfdf885b
55+
https://github.com/openppg/SINE-ESC-CAN#2ab56a4e5b52e4456317c8ee3e3d802b232c6148
5656
https://github.com/openppg/ANT-BMS-CAN#fd54852bc6f1c9608e37af9ca7c13ea4135c095b
5757
lvgl/lvgl@^8.4.0
5858
h2zero/NimBLE-Arduino@^2.3.9

‎src/sp140/esc.cpp‎

Lines changed: 146 additions & 13 deletions
Original file line numberDiff line numberDiff line change
@@ -16,6 +16,126 @@ static uint8_t memory_pool[1024] __attribute__((aligned(8)));
1616
static SineEsc esc(adapter);
1717
static unsigned long lastSuccessfulCommTimeMs = 0; // Store millis() time of last successful ESC comm
1818

19+
enum class PendingEscTone : uint8_t {
20+
NONE = 0,
21+
ARM,
22+
DISARM,
23+
};
24+
25+
static volatile EscStatusLightMode sRequestedStatusLightMode =
26+
EscStatusLightMode::OFF;
27+
static EscStatusLightMode sLastSentStatusLightMode = EscStatusLightMode::OFF;
28+
static unsigned long sLastStatusLightSendMs = 0;
29+
static bool sHaveSentStatusLight = false;
30+
static volatile PendingEscTone sPendingEscTone = PendingEscTone::NONE;
31+
32+
namespace {
33+
34+
constexpr uint8_t kEscToneLow = 3;
35+
constexpr uint8_t kEscToneHigh = 6;
36+
constexpr uint8_t kEscToneVolumePct = 80;
37+
constexpr uint8_t kEscToneDuration10ms = 10;
38+
39+
// Caller must pass ARM or DISARM (never NONE).
40+
void buildEscMotorTone(uint8_t* out, PendingEscTone tone) {
41+
if (tone == PendingEscTone::ARM) {
42+
SineEsc::makeBeepEntry(&out[0], kEscToneLow, kEscToneDuration10ms, kEscToneVolumePct);
43+
SineEsc::makeBeepEntry(&out[3], kEscToneHigh, kEscToneDuration10ms, kEscToneVolumePct);
44+
} else {
45+
SineEsc::makeBeepEntry(&out[0], kEscToneHigh, kEscToneDuration10ms, kEscToneVolumePct);
46+
SineEsc::makeBeepEntry(&out[3], kEscToneLow, kEscToneDuration10ms, kEscToneVolumePct);
47+
}
48+
}
49+
50+
unsigned long escStatusLightRefreshMs(EscStatusLightMode mode) {
51+
switch (mode) {
52+
case EscStatusLightMode::FLIGHT:
53+
return 1700;
54+
case EscStatusLightMode::READY:
55+
case EscStatusLightMode::CAUTION:
56+
return 1000;
57+
case EscStatusLightMode::OFF:
58+
default:
59+
return 0;
60+
}
61+
}
62+
63+
void sendEscStatusLight(EscStatusLightMode mode) {
64+
switch (mode) {
65+
case EscStatusLightMode::READY: {
66+
const uint16_t pattern[] = {
67+
SineEsc::makeLedControlEntry(SineEsc::LED_GREEN_BREATH, 20),
68+
};
69+
esc.setLedControl(pattern, 1);
70+
break;
71+
}
72+
case EscStatusLightMode::FLIGHT: {
73+
const uint16_t pattern[] = {
74+
SineEsc::makeLedControlEntry(SineEsc::LED_GREEN, 1),
75+
SineEsc::makeLedControlEntry(SineEsc::LED_OFF, 2),
76+
SineEsc::makeLedControlEntry(SineEsc::LED_GREEN, 1),
77+
SineEsc::makeLedControlEntry(SineEsc::LED_OFF, 30),
78+
};
79+
esc.setLedControl(pattern, 4);
80+
break;
81+
}
82+
case EscStatusLightMode::CAUTION: {
83+
const uint16_t pattern[] = {
84+
SineEsc::makeLedControlEntry(SineEsc::LED_YELLOW_BREATH, 20),
85+
};
86+
esc.setLedControl(pattern, 1);
87+
break;
88+
}
89+
case EscStatusLightMode::OFF:
90+
default: {
91+
const uint16_t pattern[] = {
92+
SineEsc::makeLedControlEntry(SineEsc::LED_OFF, 20),
93+
};
94+
esc.setLedControl(pattern, 1);
95+
break;
96+
}
97+
}
98+
}
99+
100+
void syncEscOutputs() {
101+
const bool escConnected =
102+
escTwaiInitialized &&
103+
escTelemetryData.escState == TelemetryState::CONNECTED;
104+
105+
if (!escConnected) {
106+
sPendingEscTone = PendingEscTone::NONE;
107+
sHaveSentStatusLight = false;
108+
sLastSentStatusLightMode = EscStatusLightMode::OFF;
109+
sLastStatusLightSendMs = 0;
110+
return;
111+
}
112+
113+
const PendingEscTone pendingTone = sPendingEscTone;
114+
if (pendingTone != PendingEscTone::NONE) {
115+
uint8_t beepData[6];
116+
buildEscMotorTone(beepData, pendingTone);
117+
esc.setMotorSound(beepData, 2);
118+
sPendingEscTone = PendingEscTone::NONE;
119+
}
120+
121+
const EscStatusLightMode requestedMode = sRequestedStatusLightMode;
122+
const unsigned long now = millis();
123+
const bool needsRefresh =
124+
sHaveSentStatusLight &&
125+
escStatusLightRefreshMs(requestedMode) > 0 &&
126+
(now - sLastStatusLightSendMs) >= escStatusLightRefreshMs(requestedMode);
127+
128+
if (!sHaveSentStatusLight || requestedMode != sLastSentStatusLightMode ||
129+
needsRefresh) {
130+
sendEscStatusLight(requestedMode);
131+
sLastSentStatusLightMode = requestedMode;
132+
sLastStatusLightSendMs = now;
133+
sHaveSentStatusLight = true;
134+
}
135+
}
136+
137+
} // namespace
138+
19139

20140
STR_ESC_TELEMETRY_140 escTelemetryData = {
21141
.escState = TelemetryState::NOT_CONNECTED,
@@ -95,15 +215,15 @@ void readESCTelemetry() {
95215
escTelemetryData.mos_temp = res->mos_temp / 10.0f;
96216
escTelemetryData.cap_temp = res->cap_temp / 10.0f;
97217
escTelemetryData.mcu_temp = res->mcu_temp / 10.0f;
98-
// Filter motor temp - only update if sensor is connected (valid range: -20°C to 140°C)
99-
// Disconnected sensor reads ~149°C (thermistor pulled high)
100-
float rawMotorTemp = res->motor_temp / 10.0f;
101-
if (isMotorTempValidC(rawMotorTemp)) {
102-
escTelemetryData.motor_temp = rawMotorTemp;
103-
} else {
104-
// Store invalid motor temp as NaN. Downstream consumers can skip on isnan().
105-
escTelemetryData.motor_temp = NAN;
106-
}
218+
// Filter motor temp - only update if sensor is connected (valid range: -20°C to 140°C)
219+
// Disconnected sensor reads ~149°C (thermistor pulled high)
220+
float rawMotorTemp = res->motor_temp / 10.0f;
221+
if (isMotorTempValidC(rawMotorTemp)) {
222+
escTelemetryData.motor_temp = rawMotorTemp;
223+
} else {
224+
// Store invalid motor temp as NaN. Downstream consumers can skip on isnan().
225+
escTelemetryData.motor_temp = NAN;
226+
}
107227
escTelemetryData.eRPM = res->speed;
108228
escTelemetryData.inPWM = res->recv_pwm / 10.0f;
109229
watts = escTelemetryData.amps * escTelemetryData.volts;
@@ -138,6 +258,7 @@ void readESCTelemetry() {
138258
}
139259
}
140260

261+
syncEscOutputs();
141262
adapter.processTxRxOnce(); // Process CAN messages
142263
}
143264

@@ -204,6 +325,18 @@ bool setupTWAI() {
204325
return true;
205326
}
206327

328+
void requestEscStatusLightMode(EscStatusLightMode mode) {
329+
sRequestedStatusLightMode = mode;
330+
}
331+
332+
void queueEscMotorBeepArm() {
333+
sPendingEscTone = PendingEscTone::ARM;
334+
}
335+
336+
void queueEscMotorBeepDisarm() {
337+
sPendingEscTone = PendingEscTone::DISARM;
338+
}
339+
207340
/**
208341
* Debug function to dump ESC throttle response data to serial
209342
* @param res Pointer to the throttle response structure from ESC
@@ -301,10 +434,10 @@ double mapDouble(double x, double in_min, double in_max, double out_min, double
301434
return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
302435
}
303436

304-
/**
305-
* Decode running error bitmask into human-readable string
306-
* @param errorCode 16-bit running error code from ESC
307-
* @return String containing decoded error messages
437+
/**
438+
* Decode running error bitmask into human-readable string
439+
* @param errorCode 16-bit running error code from ESC
440+
* @return String containing decoded error messages
308441
*/
309442
String decodeRunningError(uint16_t errorCode) {
310443
if (errorCode == 0) {

‎src/sp140/sp140.ino‎

Lines changed: 15 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -85,7 +85,7 @@ UnifiedBatteryData unifiedBatteryData = {0.0f, 0.0f, 0.0f};
8585
// Throttle PWM smoothing buffer is managed in throttle.cpp
8686

8787
Adafruit_NeoPixel pixels(1, 21, NEO_GRB + NEO_KHZ800);
88-
uint32_t led_color = LED_RED; // current LED color
88+
uint32_t led_color = STATUS_LED_RED; // current LED color
8989

9090
// Global variable for device state
9191
volatile DeviceState currentState = DISARMED;
@@ -634,7 +634,7 @@ void setup() {
634634
setupWatchdog();
635635
setup140();
636636

637-
setLEDColor(LED_YELLOW); // Booting up
637+
setLEDColor(STATUS_LED_YELLOW); // Booting up
638638

639639
// First initialize the shared SPI bus
640640
setupSPI(board_config);
@@ -701,7 +701,7 @@ void setup() {
701701
if (digitalRead(board_config.button_top) == LOW) { // LOW means pressed since it's INPUT_PULLUP
702702
perfModeSwitch();
703703
}
704-
setLEDColor(LED_GREEN);
704+
setLEDColor(STATUS_LED_GREEN);
705705

706706
// Show LVGL splash screen
707707
if (xSemaphoreTake(lvglMutex, portMAX_DELAY) == pdTRUE) {
@@ -926,6 +926,7 @@ void resumeLEDTask() {
926926
void runDisarmAlert() {
927927
u_int16_t disarm_melody[] = { 2637, 2093 };
928928
playMelody(disarm_melody, 2);
929+
queueEscMotorBeepDisarm();
929930
pulseVibeMotor();
930931
}
931932

@@ -1121,6 +1122,16 @@ void syncESCTelemetry() {
11211122
escTelemetryData.escState = TelemetryState::NOT_CONNECTED;
11221123
}
11231124

1125+
EscStatusLightMode escStatusLightMode = EscStatusLightMode::OFF;
1126+
if (escTelemetryData.escState == TelemetryState::CONNECTED) {
1127+
if (currentState == DISARMED) {
1128+
escStatusLightMode = EscStatusLightMode::READY;
1129+
} else {
1130+
escStatusLightMode = EscStatusLightMode::FLIGHT;
1131+
}
1132+
}
1133+
requestEscStatusLightMode(escStatusLightMode);
1134+
11241135
// Send ESC telemetry data to queue for BLE updates
11251136
if (escTelemetryQueue != NULL) {
11261137
xQueueOverwrite(escTelemetryQueue, &escTelemetryData); // Always use latest data
@@ -1149,6 +1160,7 @@ bool armSystem() {
11491160
vTaskSuspend(blinkLEDTaskHandle);
11501161
setLEDs(HIGH); // solid LED while armed
11511162
playMelody(arm_melody, 2);
1163+
queueEscMotorBeepArm();
11521164
// runVibePattern(arm_vibes, 7);
11531165
pulseVibeMotor(); // Ensure this is the active call
11541166
return true;

0 commit comments

Comments
 (0)