From 7aeb19e48de49561060a0f142f1826261ea32dc7 Mon Sep 17 00:00:00 2001 From: Petri Mattila Date: Mon, 30 Sep 2024 15:58:30 +0100 Subject: [PATCH 1/6] Add Graupner ESC telemetry support --- src/main/cli/settings.c | 2 +- src/main/pg/esc_sensor.h | 1 + src/main/sensors/esc_sensor.c | 194 ++++++++++++++++++++++++++++++++++ 3 files changed, 196 insertions(+), 1 deletion(-) diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index 947233e7cb..596ecee322 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -473,7 +473,7 @@ const char * const lookupTableErrorRelaxType[] = { #ifdef USE_ESC_SENSOR static const char * const lookupTableEscSensorProtocol[] = { - "OFF", "BLHELI32", "HOBBYWINGV4", "HOBBYWINGV5", "SCORPION", "KONTRONIK", "OMPHOBBY", "ZTW", "APD", "OPENYGE", "FLYROTOR", "RECORD", + "OFF", "BLHELI32", "HOBBYWINGV4", "HOBBYWINGV5", "SCORPION", "KONTRONIK", "OMPHOBBY", "ZTW", "APD", "OPENYGE", "FLYROTOR", "GRAUPNER", "RECORD", }; #endif diff --git a/src/main/pg/esc_sensor.h b/src/main/pg/esc_sensor.h index c5a3ef027f..537e77915d 100644 --- a/src/main/pg/esc_sensor.h +++ b/src/main/pg/esc_sensor.h @@ -38,6 +38,7 @@ enum { ESC_SENSOR_PROTO_APD, ESC_SENSOR_PROTO_OPENYGE, ESC_SENSOR_PROTO_FLY, + ESC_SENSOR_PROTO_GRAUPNER, ESC_SENSOR_PROTO_RECORD, }; diff --git a/src/main/sensors/esc_sensor.c b/src/main/sensors/esc_sensor.c index 18ffda2022..c5fbbf5743 100644 --- a/src/main/sensors/esc_sensor.c +++ b/src/main/sensors/esc_sensor.c @@ -109,6 +109,7 @@ enum { #define ESC_SIG_TRIB 0x53 #define ESC_SIG_OPENYGE 0xA5 #define ESC_SIG_FLY 0x73 +#define ESC_SIG_GRAUPNER 0xC0 #define ESC_SIG_RESTART 0xFF static serialPort_t *escSensorPort = NULL; @@ -3125,6 +3126,191 @@ static serialReceiveCallbackPtr oygeSensorInit(bool bidirectional) } +/* + * Graupner Telemetry + * + * - Serial protocol 19200,8N1 + * - Little-Endian fields + * - Frame length 45 bytes + * - Warning flags: + * 0: Low voltage + * 1: Temp limit exceeded + * 2: Motor temp exceeded + * 3: Max current exceeded + * 4: RPM less than limit + * 5: Capacity exceeded + * 6: Current limit exceeded + * + * Frame Format + * ―――――――――――――――――――――――――――――――――――――――――――――――――――――――― + * 0-1: Sync header (0x7C 0x8C) + * 2: Warning tone + * 3: Sensor ID (0xC0) + * 4-5: Warning flags + * 6-7: Voltage + * 8-9: Min Voltage + * 10-11: Capacity + * 12: Temperature + * 13: Max temp + * 14-15: Current + * 15-16: Max Current + * 17-18: RPM + * 19-20: Max RPM + * 21-42: Reserved + * 43: End byte (0x7D) + * 44: Checksum + * + */ + +#define GRAUPNER_MIN_FRAME_LENGTH 45 +#define GRAUPNER_BOOT_DELAY 5000 +#define GRAUPNER_TELE_FRAME_PERIOD 100 +#define GRAUPNER_TELE_FRAME_TIMEOUT 200 + +static uint8_t graupnerTeleReq[] = { 0x80, 0x8C }; + +typedef struct { + uint8_t tone; + uint8_t sensor; + uint16_t flags; + uint16_t voltage; + uint16_t voltage_min; + uint16_t capacity; + int8_t temperature; + int8_t temperature_max; + uint16_t current; + uint16_t current_max; + uint16_t rpm; + uint16_t rpm_max; +} __packed GraupnerTelemetryFrame_t; + + +static uint8_t graupnerCalculateChecksum(uint8_t *ptr, size_t length) +{ + uint8_t sum = 0; + + while (length-- > 0) + sum += *ptr++; + + return sum; +} + +static bool graupnerCopySendFrame(void *req, size_t len, uint16_t framePeriod, uint16_t frameTimeout) +{ + if (len > REQUEST_BUFFER_SIZE) + return false; + + memcpy(reqbuffer, req, len); + reqLength = len; + + rrfsmFramePeriod = framePeriod; + rrfsmFrameTimeout = frameTimeout; + + return true; +} + +static void graupnerBuildNextReq(void) +{ + graupnerCopySendFrame(graupnerTeleReq, sizeof(graupnerTeleReq), GRAUPNER_TELE_FRAME_PERIOD, GRAUPNER_TELE_FRAME_TIMEOUT); +} + +static bool graupnerDecodeTeleFrame(timeUs_t currentTimeUs) +{ + const GraupnerTelemetryFrame_t *tele = (GraupnerTelemetryFrame_t*)(buffer + 2); + + escSensorData[0].id = ESC_SIG_GRAUPNER; + escSensorData[0].age = 0; + escSensorData[0].erpm = tele->rpm * 10; + escSensorData[0].voltage = tele->voltage * 100; + escSensorData[0].current = tele->current * 100; + escSensorData[0].consumption = tele->capacity * 10; + escSensorData[0].temperature = tele->temperature * 10; + escSensorData[0].status = tele->flags; + + DEBUG(ESC_SENSOR, DEBUG_ESC_1_RPM, tele->rpm * 10); + DEBUG(ESC_SENSOR, DEBUG_ESC_1_TEMP, tele->temperature * 10); + DEBUG(ESC_SENSOR, DEBUG_ESC_1_VOLTAGE, tele->voltage * 100); + DEBUG(ESC_SENSOR, DEBUG_ESC_1_CURRENT, tele->current * 100); + + DEBUG(ESC_SENSOR_DATA, DEBUG_DATA_TEMP, tele->temperature); + DEBUG(ESC_SENSOR_DATA, DEBUG_DATA_VOLTAGE, tele->voltage); + DEBUG(ESC_SENSOR_DATA, DEBUG_DATA_CURRENT, tele->current); + DEBUG(ESC_SENSOR_DATA, DEBUG_DATA_AGE, 0); + + dataUpdateUs = currentTimeUs; + + return true; +} + +static bool graupnerDecode(timeUs_t currentTimeUs) +{ + const uint8_t framesum = graupnerCalculateChecksum(buffer, 44); + const uint8_t checksum = buffer[44]; + bool ret = false; + + if (checksum == framesum) { + ret = graupnerDecodeTeleFrame(currentTimeUs); + } + + graupnerBuildNextReq(); + + return ret; +} + +static int8_t graupnerAccept(uint16_t c) +{ + if (readBytes == 1) { + // frame signature + if (c != 0x7C) + return -1; + } + else if (readBytes == 2) { + // frame signature + if (c != 0x8C) + return -1; + } + else if (readBytes == 4) { + // sensor id + if (c != 0xC0) + return -1; + } + else if (readBytes > 24 && readBytes < 44) { + // null bytes + if (c != 0) + return -1; + } + else if (readBytes == 44) { + // end byte + if (c != 0x7D) + return -1; + } + + return 0; +} + +static bool graupnerStart(timeUs_t currentTimeUs) +{ + UNUSED(currentTimeUs); + + graupnerBuildNextReq(); + return true; +} + +static serialReceiveCallbackPtr graupnerSensorInit(void) +{ + rrfsmBootDelayMs = GRAUPNER_BOOT_DELAY; + rrfsmMinFrameLength = GRAUPNER_MIN_FRAME_LENGTH; + rrfsmAccept = graupnerAccept; + rrfsmDecode = graupnerDecode; + rrfsmCrank = NULL; + rrfsmStart = graupnerStart; + + escSig = ESC_SIG_GRAUPNER; + + return rrfsmDataReceive; +} + + /* * Raw Telemetry Data Recorder */ @@ -3181,6 +3367,9 @@ void escSensorProcess(timeUs_t currentTimeUs) case ESC_SENSOR_PROTO_FLY: rrfsmSensorProcess(currentTimeUs); break; + case ESC_SENSOR_PROTO_GRAUPNER: + rrfsmSensorProcess(currentTimeUs); + break; case ESC_SENSOR_PROTO_RECORD: recordSensorProcess(currentTimeUs); break; @@ -3245,6 +3434,11 @@ bool INIT_CODE escSensorInit(void) callback = flySensorInit(escHalfDuplex); baudrate = 115200; break; + case ESC_SENSOR_PROTO_GRAUPNER: + callback = graupnerSensorInit(); + baudrate = 19200; + options |= SERIAL_BIDIR; + break; case ESC_SENSOR_PROTO_RECORD: baudrate = baudRates[portConfig->telemetry_baudrateIndex]; break; From 6106cabc3d28cb65810241877ddac4530acc716c Mon Sep 17 00:00:00 2001 From: Petri Mattila Date: Thu, 3 Oct 2024 18:38:23 +0100 Subject: [PATCH 2/6] Fix temperature coding --- src/main/sensors/esc_sensor.c | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/src/main/sensors/esc_sensor.c b/src/main/sensors/esc_sensor.c index c5fbbf5743..27b29a8360 100644 --- a/src/main/sensors/esc_sensor.c +++ b/src/main/sensors/esc_sensor.c @@ -3164,7 +3164,7 @@ static serialReceiveCallbackPtr oygeSensorInit(bool bidirectional) #define GRAUPNER_MIN_FRAME_LENGTH 45 #define GRAUPNER_BOOT_DELAY 5000 -#define GRAUPNER_TELE_FRAME_PERIOD 100 +#define GRAUPNER_TELE_FRAME_PERIOD 50 #define GRAUPNER_TELE_FRAME_TIMEOUT 200 static uint8_t graupnerTeleReq[] = { 0x80, 0x8C }; @@ -3224,17 +3224,20 @@ static bool graupnerDecodeTeleFrame(timeUs_t currentTimeUs) escSensorData[0].voltage = tele->voltage * 100; escSensorData[0].current = tele->current * 100; escSensorData[0].consumption = tele->capacity * 10; - escSensorData[0].temperature = tele->temperature * 10; + escSensorData[0].temperature = tele->temperature * 10 - 200; escSensorData[0].status = tele->flags; DEBUG(ESC_SENSOR, DEBUG_ESC_1_RPM, tele->rpm * 10); - DEBUG(ESC_SENSOR, DEBUG_ESC_1_TEMP, tele->temperature * 10); + DEBUG(ESC_SENSOR, DEBUG_ESC_1_TEMP, tele->temperature * 10 - 200); DEBUG(ESC_SENSOR, DEBUG_ESC_1_VOLTAGE, tele->voltage * 100); DEBUG(ESC_SENSOR, DEBUG_ESC_1_CURRENT, tele->current * 100); + DEBUG(ESC_SENSOR_DATA, DEBUG_DATA_RPM, tele->rpm); DEBUG(ESC_SENSOR_DATA, DEBUG_DATA_TEMP, tele->temperature); DEBUG(ESC_SENSOR_DATA, DEBUG_DATA_VOLTAGE, tele->voltage); DEBUG(ESC_SENSOR_DATA, DEBUG_DATA_CURRENT, tele->current); + DEBUG(ESC_SENSOR_DATA, DEBUG_DATA_CAPACITY, tele->capacity); + DEBUG(ESC_SENSOR_DATA, DEBUG_DATA_EXTRA, tele->flags); DEBUG(ESC_SENSOR_DATA, DEBUG_DATA_AGE, 0); dataUpdateUs = currentTimeUs; From 9e9761c55e818e1766f46d3d8be265fd09c8b96e Mon Sep 17 00:00:00 2001 From: Petri Mattila Date: Fri, 4 Oct 2024 00:12:45 +0100 Subject: [PATCH 3/6] Relax protocol --- src/main/sensors/esc_sensor.c | 5 ----- 1 file changed, 5 deletions(-) diff --git a/src/main/sensors/esc_sensor.c b/src/main/sensors/esc_sensor.c index 27b29a8360..4974911101 100644 --- a/src/main/sensors/esc_sensor.c +++ b/src/main/sensors/esc_sensor.c @@ -3277,11 +3277,6 @@ static int8_t graupnerAccept(uint16_t c) if (c != 0xC0) return -1; } - else if (readBytes > 24 && readBytes < 44) { - // null bytes - if (c != 0) - return -1; - } else if (readBytes == 44) { // end byte if (c != 0x7D) From 8cdbd095889c3a078bd08d7ccff2fb762ecb5a5c Mon Sep 17 00:00:00 2001 From: Petri Mattila Date: Fri, 4 Oct 2024 00:38:00 +0100 Subject: [PATCH 4/6] Increase telemetry speed --- src/main/sensors/esc_sensor.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/sensors/esc_sensor.c b/src/main/sensors/esc_sensor.c index 4974911101..4de65c310a 100644 --- a/src/main/sensors/esc_sensor.c +++ b/src/main/sensors/esc_sensor.c @@ -3164,7 +3164,7 @@ static serialReceiveCallbackPtr oygeSensorInit(bool bidirectional) #define GRAUPNER_MIN_FRAME_LENGTH 45 #define GRAUPNER_BOOT_DELAY 5000 -#define GRAUPNER_TELE_FRAME_PERIOD 50 +#define GRAUPNER_TELE_FRAME_PERIOD 10 #define GRAUPNER_TELE_FRAME_TIMEOUT 200 static uint8_t graupnerTeleReq[] = { 0x80, 0x8C }; From 25c71164e9a47fb401aae3ffcca3f7b489170f6b Mon Sep 17 00:00:00 2001 From: Petri Mattila Date: Mon, 7 Oct 2024 09:45:02 +0100 Subject: [PATCH 5/6] Add validateAndFixEscSensorConfig() --- src/main/config/config.c | 4 ++++ src/main/sensors/esc_sensor.c | 12 +++++++++++- src/main/sensors/esc_sensor.h | 4 +++- 3 files changed, 18 insertions(+), 2 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index 2e6dbfdd24..ce3a392020 100644 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -309,6 +309,10 @@ static void validateAndFixConfig(void) if (!findSerialPortConfig(FUNCTION_ESC_SENSOR)) { featureDisableImmediate(FEATURE_ESC_SENSOR); } + + if (featureIsConfigured(FEATURE_ESC_SENSOR)) { + validateAndFixEscSensorConfig(); + } #endif for (int i = 0; i < MAX_MODE_ACTIVATION_CONDITION_COUNT; i++) { diff --git a/src/main/sensors/esc_sensor.c b/src/main/sensors/esc_sensor.c index 4de65c310a..e32b2788ea 100644 --- a/src/main/sensors/esc_sensor.c +++ b/src/main/sensors/esc_sensor.c @@ -3383,6 +3383,17 @@ void escSensorProcess(timeUs_t currentTimeUs) } } +void INIT_CODE validateAndFixEscSensorConfig(void) +{ + switch (escSensorConfig()->protocol) { + case ESC_SENSOR_PROTO_GRAUPNER: + escSensorConfigMutable()->halfDuplex = true; + break; + default: + break; + } +} + bool INIT_CODE escSensorInit(void) { const bool escHalfDuplex = escSensorConfig()->halfDuplex; @@ -3435,7 +3446,6 @@ bool INIT_CODE escSensorInit(void) case ESC_SENSOR_PROTO_GRAUPNER: callback = graupnerSensorInit(); baudrate = 19200; - options |= SERIAL_BIDIR; break; case ESC_SENSOR_PROTO_RECORD: baudrate = baudRates[portConfig->telemetry_baudrateIndex]; diff --git a/src/main/sensors/esc_sensor.h b/src/main/sensors/esc_sensor.h index 63c34e2063..2c2488e4c2 100644 --- a/src/main/sensors/esc_sensor.h +++ b/src/main/sensors/esc_sensor.h @@ -46,10 +46,12 @@ typedef struct { #define ESC_BATTERY_AGE_MAX 10 +#define ESC_SENSOR_COMBINED 255 + bool escSensorInit(void); void escSensorProcess(timeUs_t currentTime); -#define ESC_SENSOR_COMBINED 255 +void validateAndFixEscSensorConfig(void); bool isEscSensorActive(void); From 33a397096b3387496d8f09288013b7754fc70352 Mon Sep 17 00:00:00 2001 From: Petri Mattila Date: Mon, 7 Oct 2024 09:47:29 +0100 Subject: [PATCH 6/6] Remove unnecessary rrfsmCrank = NULL --- src/main/sensors/esc_sensor.c | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/sensors/esc_sensor.c b/src/main/sensors/esc_sensor.c index e32b2788ea..c7666d99dd 100644 --- a/src/main/sensors/esc_sensor.c +++ b/src/main/sensors/esc_sensor.c @@ -3300,7 +3300,6 @@ static serialReceiveCallbackPtr graupnerSensorInit(void) rrfsmMinFrameLength = GRAUPNER_MIN_FRAME_LENGTH; rrfsmAccept = graupnerAccept; rrfsmDecode = graupnerDecode; - rrfsmCrank = NULL; rrfsmStart = graupnerStart; escSig = ESC_SIG_GRAUPNER;