Merge pull request #1646 from basdelfos/esc_telemetry_as_sensor

ESC telemetry as sensor

.gitignore

@@ -13,6 +13,7 @@
 obj/
 patches/
 startup_stm32f10x_md_gcc.s
+.idea
 
 # script-generated files
 docs/Manual.pdf

Makefile

@@ -593,7 +593,7 @@ HIGHEND_SRC = \
             telemetry/smartport.c \
             telemetry/ltm.c \
             telemetry/mavlink.c \
-            telemetry/esc_telemetry.c \
+            sensors/esc_sensor.c \
 
 SPEED_OPTIMISED_SRC = \
             common/encoding.c \

src/main/build/debug.h

@@ -59,7 +59,7 @@ typedef enum {
     DEBUG_VELOCITY,
     DEBUG_DTERM_FILTER,
     DEBUG_ANGLERATE,
-    DEBUG_ESC_TELEMETRY,
+    DEBUG_ESC_SENSOR,
     DEBUG_SCHEDULER,
     DEBUG_STACK,
     DEBUG_COUNT

src/main/fc/config.h

@@ -55,7 +55,7 @@ typedef enum {
     FEATURE_VTX = 1 << 24,
     FEATURE_RX_SPI = 1 << 25,
     FEATURE_SOFTSPI = 1 << 26,
-    FEATURE_ESC_TELEMETRY = 1 << 27
+    FEATURE_ESC_SENSOR = 1 << 27,
 } features_e;
 
 void beeperOffSet(uint32_t mask);

src/main/fc/fc_msp.c

@@ -882,6 +882,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
         sbufWriteU8(dst, batteryConfig()->vbatmincellvoltage);
         sbufWriteU8(dst, batteryConfig()->vbatmaxcellvoltage);
         sbufWriteU8(dst, batteryConfig()->vbatwarningcellvoltage);
+        sbufWriteU8(dst, batteryConfig()->batteryMeterType);
         break;
 
     case MSP_CURRENT_METER_CONFIG:
@@ -1662,6 +1663,9 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
         batteryConfig()->vbatmincellvoltage = sbufReadU8(src);  // vbatlevel_warn1 in MWC2.3 GUI
         batteryConfig()->vbatmaxcellvoltage = sbufReadU8(src);  // vbatlevel_warn2 in MWC2.3 GUI
         batteryConfig()->vbatwarningcellvoltage = sbufReadU8(src);  // vbatlevel when buzzer starts to alert
+        if (dataSize > 4) {
+            batteryConfig()->batteryMeterType = sbufReadU8(src);
+        }
         break;
 
     case MSP_SET_CURRENT_METER_CONFIG:

src/main/fc/fc_tasks.c

@@ -63,11 +63,11 @@
 #include "sensors/compass.h"
 #include "sensors/gyro.h"
 #include "sensors/sonar.h"
+#include "sensors/esc_sensor.h"
 
 #include "scheduler/scheduler.h"
 
 #include "telemetry/telemetry.h"
-#include "telemetry/esc_telemetry.h"
 
 #include "config/feature.h"
 #include "config/config_profile.h"
@@ -109,9 +109,9 @@ static void taskHandleSerial(timeUs_t currentTimeUs)
 
 static void taskUpdateBattery(timeUs_t currentTimeUs)
 {
-#ifdef USE_ADC
+#if defined(USE_ADC) || defined(USE_ESC_SENSOR)
     static uint32_t vbatLastServiced = 0;
-    if (feature(FEATURE_VBAT) || feature(FEATURE_ESC_TELEMETRY)) {
+    if (feature(FEATURE_VBAT) || feature(FEATURE_ESC_SENSOR)) {
         if (cmp32(currentTimeUs, vbatLastServiced) >= VBATINTERVAL) {
             vbatLastServiced = currentTimeUs;
             updateBattery();
@@ -120,7 +120,7 @@ static void taskUpdateBattery(timeUs_t currentTimeUs)
 #endif
 
     static uint32_t ibatLastServiced = 0;
-    if (feature(FEATURE_CURRENT_METER) || feature(FEATURE_ESC_TELEMETRY)) {
+    if (feature(FEATURE_CURRENT_METER) || feature(FEATURE_ESC_SENSOR)) {
         const int32_t ibatTimeSinceLastServiced = cmp32(currentTimeUs, ibatLastServiced);
 
         if (ibatTimeSinceLastServiced >= IBATINTERVAL) {
@@ -203,15 +203,6 @@ static void taskTelemetry(timeUs_t currentTimeUs)
 }
 #endif
 
-#ifdef USE_ESC_TELEMETRY
-static void taskEscTelemetry(timeUs_t currentTimeUs)
-{
-    if (feature(FEATURE_ESC_TELEMETRY)) {
-        escTelemetryProcess(currentTimeUs);
-     }
- }
-#endif
-
 void fcTasksInit(void)
 {
     schedulerInit();
@@ -277,8 +268,8 @@ void fcTasksInit(void)
 #ifdef USE_BST
     setTaskEnabled(TASK_BST_MASTER_PROCESS, true);
 #endif
-#ifdef USE_ESC_TELEMETRY
+#ifdef USE_ESC_SENSOR
-    setTaskEnabled(TASK_ESC_TELEMETRY, feature(FEATURE_ESC_TELEMETRY));
+    setTaskEnabled(TASK_ESC_SENSOR, feature(FEATURE_ESC_SENSOR));
 #endif
 #ifdef CMS
 #ifdef USE_MSP_DISPLAYPORT
@@ -450,11 +441,11 @@ cfTask_t cfTasks[TASK_COUNT] = {
     },
 #endif
 
-#ifdef USE_ESC_TELEMETRY
+#ifdef USE_ESC_SENSOR
-    [TASK_ESC_TELEMETRY] = {
+    [TASK_ESC_SENSOR] = {
-        .taskName = "ESC_TELEMETRY",
+        .taskName = "ESC_SENSOR",
-        .taskFunc = taskEscTelemetry,
+        .taskFunc = escSensorProcess,
-        .desiredPeriod = 1000000 / 100,         // 100 Hz
+        .desiredPeriod = TASK_PERIOD_HZ(100),       // 100 Hz every 10ms
         .staticPriority = TASK_PRIORITY_LOW,
     },
 #endif

src/main/io/serial.h

@@ -37,7 +37,7 @@ typedef enum {
     FUNCTION_BLACKBOX            = (1 << 7), // 128
 
     FUNCTION_TELEMETRY_MAVLINK   = (1 << 9), // 512
-    FUNCTION_TELEMETRY_ESC       = (1 << 10) // 1024
+    FUNCTION_ESC_SENSOR          = (1 << 10) // 1024
 } serialPortFunction_e;
 
 typedef enum {

src/main/io/serial_cli.c

@@ -234,7 +234,7 @@ static const char * const featureNames[] = {
     "SONAR", "TELEMETRY", "CURRENT_METER", "3D", "RX_PARALLEL_PWM",
     "RX_MSP", "RSSI_ADC", "LED_STRIP", "DISPLAY", "OSD",
     "BLACKBOX", "CHANNEL_FORWARDING", "TRANSPONDER", "AIRMODE",
-    "SDCARD", "VTX", "RX_SPI", "SOFTSPI", "ESC_TELEMETRY", NULL
+    "SDCARD", "VTX", "RX_SPI", "SOFTSPI", "ESC_SENSOR", NULL
 };
 
 // sync this with rxFailsafeChannelMode_e
@@ -415,7 +415,11 @@ static const char * const lookupTableGPSSBASMode[] = {
 #endif
 
 static const char * const lookupTableCurrentSensor[] = {
-    "NONE", "ADC", "VIRTUAL"
+    "NONE", "ADC", "VIRTUAL", "ESC"
+};
+
+static const char * const lookupTableBatterySensor[] = {
+    "ADC", "ESC"
 };
 
 #ifdef USE_SERVOS
@@ -519,7 +523,7 @@ static const char * const lookupTableDebug[DEBUG_COUNT] = {
     "VELOCITY",
     "DFILTER",
     "ANGLERATE",
-    "ESC_TELEMETRY",
+    "ESC_SENSOR",
     "SCHEDULER",
 };
 
@@ -571,6 +575,7 @@ typedef enum {
     TABLE_BLACKBOX_DEVICE,
 #endif
     TABLE_CURRENT_SENSOR,
+    TABLE_BATTERY_SENSOR,
 #ifdef USE_SERVOS
     TABLE_GIMBAL_MODE,
 #endif
@@ -611,6 +616,7 @@ static const lookupTableEntry_t lookupTables[] = {
     { lookupTableBlackboxDevice, sizeof(lookupTableBlackboxDevice) / sizeof(char *) },
 #endif
     { lookupTableCurrentSensor, sizeof(lookupTableCurrentSensor) / sizeof(char *) },
+    { lookupTableBatterySensor, sizeof(lookupTableBatterySensor) / sizeof(char *) },
 #ifdef USE_SERVOS
     { lookupTableGimbalMode, sizeof(lookupTableGimbalMode) / sizeof(char *) },
 #endif
@@ -799,6 +805,7 @@ const clivalue_t valueTable[] = {
     { "current_meter_offset",       VAR_INT16 | MASTER_VALUE,  &batteryConfig()->currentMeterOffset, .config.minmax = { -16000, 16000 } },
     { "multiwii_current_meter_output", VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP,  &batteryConfig()->multiwiiCurrentMeterOutput, .config.lookup = { TABLE_OFF_ON } },
     { "current_meter_type",         VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP,  &batteryConfig()->currentMeterType, .config.lookup = { TABLE_CURRENT_SENSOR } },
+    { "battery_meter_type",         VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP,  &batteryConfig()->batteryMeterType, .config.lookup = { TABLE_BATTERY_SENSOR } },
     { "battery_notpresent_level",   VAR_UINT8  | MASTER_VALUE, &batteryConfig()->batterynotpresentlevel, .config.minmax = { 0, 200 } },
     { "use_vbat_alerts",            VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, &batteryConfig()->useVBatAlerts, .config.lookup = { TABLE_OFF_ON } },
     { "use_consumption_alerts",     VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, &batteryConfig()->useConsumptionAlerts, .config.lookup = { TABLE_OFF_ON } },

src/main/main.c

@@ -104,7 +104,7 @@
 #include "sensors/initialisation.h"
 
 #include "telemetry/telemetry.h"
-#include "telemetry/esc_telemetry.h"
+#include "sensors/esc_sensor.h"
 
 #include "flight/pid.h"
 #include "flight/imu.h"
@@ -493,9 +493,9 @@ void init(void)
     }
 #endif
 
-#ifdef USE_ESC_TELEMETRY
+#ifdef USE_ESC_SENSOR
-    if (feature(FEATURE_ESC_TELEMETRY)) {
+    if (feature(FEATURE_ESC_SENSOR)) {
-        escTelemetryInit();
+        escSensorInit();
     }
 #endif
 

src/main/msp/msp_protocol.h

@@ -59,7 +59,7 @@
 #define MSP_PROTOCOL_VERSION                0
 
 #define API_VERSION_MAJOR                   1 // increment when major changes are made
-#define API_VERSION_MINOR                   22 // increment when any change is made, reset to zero when major changes are released after changing API_VERSION_MAJOR
+#define API_VERSION_MINOR                   23 // increment when any change is made, reset to zero when major changes are released after changing API_VERSION_MAJOR
 
 #define API_VERSION_LENGTH                  2
 

src/main/scheduler/scheduler.h

@@ -94,8 +94,8 @@ typedef enum {
 #ifdef USE_BST
     TASK_BST_MASTER_PROCESS,
 #endif
-#ifdef USE_ESC_TELEMETRY
+#ifdef USE_ESC_SENSOR
-    TASK_ESC_TELEMETRY,
+    TASK_ESC_SENSOR,
 #endif
 #ifdef CMS
     TASK_CMS,

src/main/sensors/battery.c

@@ -34,8 +34,7 @@
 #include "config/feature.h"
 
 #include "sensors/battery.h"
-
+#include "sensors/esc_sensor.h"
-#include "telemetry/esc_telemetry.h"
 
 #include "fc/rc_controls.h"
 #include "io/beeper.h"
@@ -84,9 +83,9 @@ static void updateBatteryVoltage(void)
         vBatFilterIsInitialised = true;
     }
 
-    #ifdef USE_ESC_TELEMETRY
+    #ifdef USE_ESC_SENSOR
-    if (batteryConfig->batteryMeterType == BATTERY_SENSOR_ESC && isEscTelemetryActive()) {
+    if (feature(FEATURE_ESC_SENSOR) && batteryConfig->batteryMeterType == BATTERY_SENSOR_ESC) {
-        vbatLatest = getEscTelemetryVbat();
+        vbatLatest = getEscSensorVbat();
         if (debugMode == DEBUG_BATTERY) {
             debug[0] = -1;
         }
@@ -292,11 +291,11 @@ void updateCurrentMeter(int32_t lastUpdateAt, rxConfig_t *rxConfig, uint16_t dea
 
             break;
         case CURRENT_SENSOR_ESC:
-            #ifdef USE_ESC_TELEMETRY
+            #ifdef USE_ESC_SENSOR
-            if (isEscTelemetryActive()) {
+            if (feature(FEATURE_ESC_SENSOR)) {
-                amperageLatest = getEscTelemetryCurrent();
+                amperageLatest = getEscSensorCurrent();
                 amperage = amperageLatest;
-                mAhDrawn = getEscTelemetryConsumption();
+                mAhDrawn = getEscSensorConsumption();
 
                 updateConsumptionWarning();
             }

src/main/sensors/battery.h

@@ -31,12 +31,12 @@ typedef enum {
     CURRENT_SENSOR_NONE = 0,
     CURRENT_SENSOR_ADC,
     CURRENT_SENSOR_VIRTUAL,
-    CURRENT_SENSOR_ESC
+    CURRENT_SENSOR_ESC,
+    CURRENT_SENSOR_MAX = CURRENT_SENSOR_ESC
 } currentSensor_e;
 
 typedef enum {
-    BATTERY_SENSOR_NONE = 0,
+    BATTERY_SENSOR_ADC = 0,
-    BATTERY_SENSOR_ADC,
     BATTERY_SENSOR_ESC
 } batterySensor_e;
 

src/main/sensors/esc_sensor.c

@@ -0,0 +1,313 @@
+#include <stdbool.h>
+#include <stdint.h>
+#include <stdlib.h>
+
+#include <platform.h>
+
+#include "fc/config.h"
+#include "config/feature.h"
+#include "config/config_master.h"
+
+#include "common/utils.h"
+
+#include "drivers/system.h"
+#include "drivers/serial.h"
+#include "drivers/serial_uart.h"
+#include "drivers/pwm_output.h"
+
+#include "io/serial.h"
+
+#include "flight/mixer.h"
+
+#include "sensors/battery.h"
+
+#include "esc_sensor.h"
+
+#include "build/debug.h"
+
+/*
+KISS ESC TELEMETRY PROTOCOL
+---------------------------
+
+One transmission will have 10 times 8-bit bytes sent with 115200 baud and 3.6V.
+
+Byte 0: Temperature
+Byte 1: Voltage high byte
+Byte 2: Voltage low byte
+Byte 3: Current high byte
+Byte 4: Current low byte
+Byte 5: Consumption high byte
+Byte 6: Consumption low byte
+Byte 7: Rpm high byte
+Byte 8: Rpm low byte
+Byte 9: 8-bit CRC
+
+*/
+
+/*
+DEBUG INFORMATION
+-----------------
+
+set debug_mode = DEBUG_ESC_TELEMETRY in cli
+
+0: current motor index requested
+1: number of timeouts
+2: voltage
+3: current
+*/
+
+#ifdef USE_DSHOT
+
+typedef struct {
+    bool skipped;
+    int16_t temperature;
+    int16_t voltage;
+    int16_t current;
+    int16_t consumption;
+    int16_t rpm;
+} esc_telemetry_t;
+
+typedef enum {
+    ESC_SENSOR_FRAME_PENDING = 1 << 0,     // 1
+    ESC_SENSOR_FRAME_COMPLETE = 1 << 1     // 2
+} escTlmFrameState_t;
+
+typedef enum {
+    ESC_SENSOR_TRIGGER_WAIT = 0,
+    ESC_SENSOR_TRIGGER_READY = 1 << 0,     // 1
+    ESC_SENSOR_TRIGGER_PENDING = 1 << 1,   // 2
+} escSensorTriggerState_t;
+
+#define ESC_SENSOR_BAUDRATE 115200
+#define ESC_SENSOR_BUFFSIZE 10
+#define ESC_BOOTTIME 5000               // 5 seconds
+#define ESC_REQUEST_TIMEOUT 100         // 100 ms (data transfer takes only 900us)
+
+static bool tlmFrameDone = false;
+static uint8_t tlm[ESC_SENSOR_BUFFSIZE] = { 0, };
+static uint8_t tlmFramePosition = 0;
+static serialPort_t *escSensorPort = NULL;
+static esc_telemetry_t escSensorData[MAX_SUPPORTED_MOTORS];
+static uint32_t escTriggerTimestamp = -1;
+static uint32_t escLastResponseTimestamp;
+static uint8_t timeoutRetryCount = 0;
+static uint8_t totalRetryCount = 0;
+
+static uint8_t escSensorMotor = 0;      // motor index
+static bool escSensorEnabled = false;
+static escSensorTriggerState_t escSensorTriggerState = ESC_SENSOR_TRIGGER_WAIT;
+
+static int16_t escVbat = 0;
+static int16_t escCurrent = 0;
+static int16_t escConsumption = 0;
+
+static void escSensorDataReceive(uint16_t c);
+static uint8_t update_crc8(uint8_t crc, uint8_t crc_seed);
+static uint8_t get_crc8(uint8_t *Buf, uint8_t BufLen);
+static void selectNextMotor(void);
+
+bool isEscSensorActive(void)
+{
+    return escSensorEnabled;
+}
+
+int16_t getEscSensorVbat(void)
+{
+    return escVbat / 10;
+}
+
+int16_t getEscSensorCurrent(void)
+{
+    return escCurrent;
+}
+
+int16_t getEscSensorConsumption(void)
+{
+    return escConsumption;
+}
+
+bool escSensorInit(void)
+{
+    serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_ESC_SENSOR);
+    if (!portConfig) {
+        return false;
+    }
+
+    portOptions_t options = (SERIAL_NOT_INVERTED);
+
+    // Initialize serial port
+    escSensorPort = openSerialPort(portConfig->identifier, FUNCTION_ESC_SENSOR, escSensorDataReceive, ESC_SENSOR_BAUDRATE, MODE_RX, options);
+
+    if (escSensorPort) {
+        escSensorEnabled = true;
+    }
+
+    return escSensorPort != NULL;
+}
+
+void freeEscSensorPort(void)
+{
+    closeSerialPort(escSensorPort);
+    escSensorPort = NULL;
+    escSensorEnabled = false;
+}
+
+// Receive ISR callback
+static void escSensorDataReceive(uint16_t c)
+{
+    // KISS ESC sends some data during startup, ignore this for now (maybe future use)
+    // startup data could be firmware version and serialnumber
+
+    if (escSensorTriggerState == ESC_SENSOR_TRIGGER_WAIT) return;
+
+    tlm[tlmFramePosition] = (uint8_t)c;
+
+    if (tlmFramePosition == ESC_SENSOR_BUFFSIZE - 1) {
+        tlmFrameDone = true;
+        tlmFramePosition = 0;
+    } else {
+        tlmFramePosition++;
+    }
+}
+
+uint8_t escSensorFrameStatus(void)
+{
+    uint8_t frameStatus = ESC_SENSOR_FRAME_PENDING;
+    uint16_t chksum, tlmsum;
+
+    if (!tlmFrameDone) {
+        return frameStatus;
+    }
+
+    tlmFrameDone = false;
+
+    // Get CRC8 checksum
+    chksum = get_crc8(tlm, ESC_SENSOR_BUFFSIZE - 1);
+    tlmsum = tlm[ESC_SENSOR_BUFFSIZE - 1];     // last byte contains CRC value
+
+    if (chksum == tlmsum) {
+        escSensorData[escSensorMotor].skipped = false;
+        escSensorData[escSensorMotor].temperature = tlm[0];
+        escSensorData[escSensorMotor].voltage = tlm[1] << 8 | tlm[2];
+        escSensorData[escSensorMotor].current = tlm[3] << 8 | tlm[4];
+        escSensorData[escSensorMotor].consumption = tlm[5] << 8 | tlm[6];
+        escSensorData[escSensorMotor].rpm = tlm[7] << 8 | tlm[8];
+
+        frameStatus = ESC_SENSOR_FRAME_COMPLETE;
+    }
+
+    return frameStatus;
+}
+
+void escSensorProcess(timeUs_t currentTimeUs)
+{
+    const timeMs_t currentTimeMs = currentTimeUs / 1000;
+
+    if (!escSensorEnabled) {
+        return;
+    }
+
+    // Wait period of time before requesting telemetry (let the system boot first)
+    if (currentTimeMs < ESC_BOOTTIME) {
+        return;
+    }
+    else if (escSensorTriggerState == ESC_SENSOR_TRIGGER_WAIT) {
+        // Ready for starting requesting telemetry
+        escSensorTriggerState = ESC_SENSOR_TRIGGER_READY;
+        escSensorMotor = 0;
+        escTriggerTimestamp = currentTimeMs;
+        escLastResponseTimestamp = escTriggerTimestamp;
+    }
+    else if (escSensorTriggerState == ESC_SENSOR_TRIGGER_READY) {
+        DEBUG_SET(DEBUG_ESC_SENSOR, 0, escSensorMotor+1);
+
+        motorDmaOutput_t * const motor = getMotorDmaOutput(escSensorMotor);
+        motor->requestTelemetry = true;
+        escSensorTriggerState = ESC_SENSOR_TRIGGER_PENDING;
+    }
+    else if (escSensorTriggerState == ESC_SENSOR_TRIGGER_PENDING) {
+
+        if (escTriggerTimestamp + ESC_REQUEST_TIMEOUT < currentTimeMs) {
+            // ESC did not repond in time, retry
+            timeoutRetryCount++;
+            escTriggerTimestamp = currentTimeMs;
+            escSensorTriggerState = ESC_SENSOR_TRIGGER_READY;
+
+            if (timeoutRetryCount == 4) {
+                // Not responding after 3 times, skip motor
+                escSensorData[escSensorMotor].skipped = true;
+                selectNextMotor();
+            }
+
+            DEBUG_SET(DEBUG_ESC_SENSOR, 1, ++totalRetryCount);
+        }
+
+        // Get received frame status
+        uint8_t state = escSensorFrameStatus();
+
+        if (state == ESC_SENSOR_FRAME_COMPLETE) {
+            // Wait until all ESCs are processed
+            if (escSensorMotor == getMotorCount()-1) {
+                escCurrent = 0;
+                escConsumption = 0;
+                escVbat = 0;
+
+                for (int i = 0; i < getMotorCount(); i++) {
+                    if (!escSensorData[i].skipped) {
+                        escVbat =  i > 0 ? ((escVbat + escSensorData[i].voltage) / 2) : escSensorData[i].voltage;
+                        escCurrent = escCurrent + escSensorData[i].current;
+                        escConsumption = escConsumption + escSensorData[i].consumption;
+                    }
+                }
+            }
+
+            DEBUG_SET(DEBUG_ESC_SENSOR, 2, escVbat);
+            DEBUG_SET(DEBUG_ESC_SENSOR, 3, escCurrent);
+
+            selectNextMotor();
+            escSensorTriggerState = ESC_SENSOR_TRIGGER_READY;
+            escLastResponseTimestamp = currentTimeMs;
+        }
+    }
+
+    if (escLastResponseTimestamp + 10000 < currentTimeMs) {
+        // ESCs did not respond for 10 seconds
+        // Disable ESC telemetry and reset voltage and current to let the use know something is wrong
+        freeEscSensorPort();
+        escVbat = 0;
+        escCurrent = 0;
+    }
+}
+
+static void selectNextMotor(void)
+{
+    escSensorMotor++;
+    if (escSensorMotor == getMotorCount()) {
+        escSensorMotor = 0;
+    }
+    timeoutRetryCount = 0;
+    escTriggerTimestamp = millis();
+}
+
+//-- CRC
+
+static uint8_t update_crc8(uint8_t crc, uint8_t crc_seed)
+{
+    uint8_t crc_u = crc;
+    crc_u ^= crc_seed;
+
+    for (int i=0; i<8; i++) {
+        crc_u = ( crc_u & 0x80 ) ? 0x7 ^ ( crc_u << 1 ) : ( crc_u << 1 );
+    }
+
+    return (crc_u);
+}
+
+static uint8_t get_crc8(uint8_t *Buf, uint8_t BufLen)
+{
+    uint8_t crc = 0;
+    for(int i=0; i<BufLen; i++) crc = update_crc8(Buf[i], crc);
+    return (crc);
+}
+
+#endif

src/main/sensors/esc_sensor.h

@@ -0,0 +1,10 @@
+#pragma once
+
+uint8_t escSensorFrameStatus(void);
+bool escSensorInit(void);
+bool isEscSensorActive(void);
+int16_t getEscSensorVbat(void);
+int16_t getEscSensorCurrent(void);
+int16_t getEscSensorConsumption(void);
+
+void escSensorProcess(uint32_t currentTime);

src/main/target/ANYFCF7/target.h

@@ -24,7 +24,7 @@
 #define USBD_PRODUCT_STRING "AnyFCF7"
 
 #define USE_DSHOT
-#define USE_ESC_TELEMETRY
+#define USE_ESC_SENSOR
 
 #define LED0   PB7
 #define LED1   PB6

src/main/target/BETAFLIGHTF3/target.h

@@ -44,7 +44,8 @@
 #define MPU_INT_EXTI                PC13
 #define USE_EXTI
 
-#define USE_ESC_TELEMETRY
+#define USE_DSHOT
+#define USE_ESC_SENSOR
 #define REMAP_TIM16_DMA
 #define REMAP_TIM17_DMA
 

src/main/target/BLUEJAYF4/target.h

@@ -144,7 +144,8 @@
 #define USE_ADC
 #define VBAT_ADC_PIN            PC3
 
-#define USE_ESC_TELEMETRY
+#define USE_DSHOT
+#define USE_ESC_SENSOR
 #define LED_STRIP
 
 #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT

src/main/target/DOGE/target.h

@@ -125,7 +125,8 @@
 #define ENSURE_MPU_DATA_READY_IS_LOW
 
 #define LED_STRIP
-#define USE_ESC_TELEMETRY
+#define USE_DSHOT
+#define USE_ESC_SENSOR
 
 #define DEFAULT_RX_FEATURE      FEATURE_RX_PPM
 

src/main/target/FURYF3/target.h

@@ -161,7 +161,8 @@
 
 #define USE_SERIAL_4WAY_BLHELI_INTERFACE
 
-#define USE_ESC_TELEMETRY
+#define USE_DSHOT
+#define USE_ESC_SENSOR
 #define REMAP_TIM17_DMA
 
 #define TARGET_IO_PORTA         0xffff

src/main/target/FURYF4/target.h

@@ -175,7 +175,8 @@
 
 #define USE_SERIAL_4WAY_BLHELI_INTERFACE
 
-#define USE_ESC_TELEMETRY
+#define USE_DSHOT
+#define USE_ESC_SENSOR
 
 #define TARGET_IO_PORTA         0xffff
 #define TARGET_IO_PORTB         0xffff

src/main/target/FURYF7/target.h

@@ -24,7 +24,7 @@
 #define USBD_PRODUCT_STRING "FuryF7"
 
 #define USE_DSHOT
-#define USE_ESC_TELEMETRY
+#define USE_ESC_SENSOR
 
 #define LED0                    PB5
 #define LED1                    PB4

src/main/target/IMPULSERCF3/target.h

@@ -46,7 +46,8 @@
 #define USE_FLASHFS
 #define USE_FLASH_M25P16
 
-#define USE_ESC_TELEMETRY
+#define USE_DSHOT
+#define USE_ESC_SENSOR
 #define REMAP_TIM17_DMA
 
 #define USE_VCP

src/main/target/KISSFC/target.h

@@ -23,7 +23,8 @@
 
 #define SBUS_PORT_OPTIONS (SERIAL_STOPBITS_2 | SERIAL_PARITY_EVEN | SERIAL_INVERTED | SERIAL_BIDIR)
 
-#define USE_ESC_TELEMETRY
+#define USE_DSHOT
+#define USE_ESC_SENSOR
 
 #define USE_ESCSERIAL
 #define ESCSERIAL_TIMER_TX_HARDWARE 6

src/main/target/LUX_RACE/target.h

@@ -46,7 +46,8 @@
 #define USE_MPU_DATA_READY_SIGNAL
 #define ENSURE_MPU_DATA_READY_IS_LOW
 
-#define USE_ESC_TELEMETRY
+#define USE_DSHOT
+#define USE_ESC_SENSOR
 
 #define USE_SPI
 #define USE_SPI_DEVICE_1

src/main/target/MOTOLAB/target.h

@@ -95,7 +95,8 @@
 #define RSSI_ADC_PIN            PB2
 
 #define LED_STRIP
-#define USE_ESC_TELEMETRY
+#define USE_DSHOT
+#define USE_ESC_SENSOR
 
 #define SPEKTRUM_BIND
 // USART2, PB4

src/main/target/MULTIFLITEPICO/target.h

@@ -105,7 +105,8 @@
 #define CURRENT_METER_ADC_PIN   PA5
 #define RSSI_ADC_PIN            PB2
 
-#define USE_ESC_TELEMETRY
+#define USE_DSHOT
+#define USE_ESC_SENSOR
 #define REMAP_TIM17_DMA
 
 // UART1 TX uses DMA1_Channel4, which is also used by dshot on motor 4

src/main/target/OMNIBUS/target.h

@@ -128,7 +128,8 @@
 // Divide to under 25MHz for normal operation:
 #define SDCARD_SPI_FULL_SPEED_CLOCK_DIVIDER     2
 
-#define USE_ESC_TELEMETRY
+#define USE_DSHOT
+#define USE_ESC_SENSOR
 
 // DSHOT output 4 uses DMA1_Channel5, so don't use it for the SDCARD until we find an alternative
 #ifndef USE_DSHOT

src/main/target/OMNIBUSF4/target.h

@@ -155,7 +155,8 @@
 #define VBAT_ADC_PIN            PC2
 //#define RSSI_ADC_PIN            PA0
 
-#define USE_ESC_TELEMETRY
+#define USE_DSHOT
+#define USE_ESC_SENSOR
 
 #define LED_STRIP
 

src/main/target/REVO/target.h

@@ -41,7 +41,8 @@
 
 #endif
 
-#define USE_ESC_TELEMETRY
+#define USE_DSHOT
+#define USE_ESC_SENSOR
 
 #define LED0                    PB5
 // Disable LED1, conflicts with AirbotF4/Flip32F4/Revolt beeper

src/main/target/SPARKY2/target.h

@@ -35,7 +35,8 @@
 #define INVERTER                PC6
 #define INVERTER_USART          USART6
 
-#define USE_ESC_TELEMETRY
+#define USE_DSHOT
+#define USE_ESC_SENSOR
 
 // MPU9250 interrupt
 #define USE_EXTI

src/main/target/SPRACINGF3/target.h

@@ -115,7 +115,8 @@
 #define CURRENT_METER_ADC_PIN   PA5
 #define RSSI_ADC_PIN            PB2
 
-#define USE_ESC_TELEMETRY
+#define USE_DSHOT
+#define USE_ESC_SENSOR
 #define REMAP_TIM17_DMA
 
 // UART1 TX uses DMA1_Channel4, which is also used by dshot on motor 4

src/main/target/SPRACINGF3EVO/target.h

@@ -35,7 +35,8 @@
 #define USE_MAG_DATA_READY_SIGNAL
 #define ENSURE_MAG_DATA_READY_IS_HIGH
 
-#define USE_ESC_TELEMETRY
+#define USE_DSHOT
+#define USE_ESC_SENSOR
 
 #define GYRO
 #define USE_GYRO_SPI_MPU6500

src/main/target/STM32F3DISCOVERY/target.h

@@ -169,7 +169,8 @@
 #define RSSI_ADC_PIN            PC2
 #define EXTERNAL1_ADC_PIN       PC3
 
-#define USE_ESC_TELEMETRY
+#define USE_DSHOT
+#define USE_ESC_SENSOR
 
 #define LED_STRIP
 

src/main/target/X_RACERSPI/target.h

@@ -101,7 +101,8 @@
 #define CURRENT_METER_ADC_PIN   PA5
 #define RSSI_ADC_PIN            PB2
 
-#define USE_ESC_TELEMETRY
+#define USE_DSHOT
+#define USE_ESC_SENSOR
 
 #define REMAP_TIM17_DMA
 // UART1 TX uses DMA1_Channel4, which is also used by dshot on motor 4

src/main/telemetry/esc_telemetry.c

@@ -1,312 +0,0 @@
-#include <stdbool.h>
-#include <stdint.h>
-#include <stdlib.h>
-
-#include <platform.h>
-
-#include "fc/config.h"
-#include "config/feature.h"
-#include "config/config_master.h"
-
-#include "common/utils.h"
-
-#include "drivers/system.h"
-#include "drivers/serial.h"
-#include "drivers/serial_uart.h"
-#include "drivers/pwm_output.h"
-
-#include "io/serial.h"
-
-#include "flight/mixer.h"
-
-#include "sensors/battery.h"
-
-#include "esc_telemetry.h"
-
-#include "build/debug.h"
-
-/*
-KISS ESC TELEMETRY PROTOCOL
----------------------------
-
-One transmission will have 10 times 8-bit bytes sent with 115200 baud and 3.6V.
-
-Byte 0: Temperature
-Byte 1: Voltage high byte
-Byte 2: Voltage low byte
-Byte 3: Current high byte
-Byte 4: Current low byte
-Byte 5: Consumption high byte
-Byte 6: Consumption low byte
-Byte 7: Rpm high byte
-Byte 8: Rpm low byte
-Byte 9: 8-bit CRC
-
-*/
-
-/*
-DEBUG INFORMATION
------------------
-
-set debug_mode = DEBUG_ESC_TELEMETRY in cli
-
-0: current motor index requested
-1: number of timeouts
-2: voltage
-3: current
-*/
-
-#ifdef USE_DSHOT
-
-typedef struct {
-    bool skipped;
-    int16_t temperature;
-    int16_t voltage;
-    int16_t current;
-    int16_t consumption;
-    int16_t rpm;
-} esc_telemetry_t;
-
-typedef enum {
-    ESC_TLM_FRAME_PENDING = 1 << 0,     // 1
-    ESC_TLM_FRAME_COMPLETE = 1 << 1     // 2
-} escTlmFrameState_t;
-
-typedef enum {
-    ESC_TLM_TRIGGER_WAIT = 0,
-    ESC_TLM_TRIGGER_READY = 1 << 0,     // 1
-    ESC_TLM_TRIGGER_PENDING = 1 << 1    // 2
-} escTlmTriggerState_t;
-
-#define ESC_TLM_BAUDRATE 115200
-#define ESC_TLM_BUFFSIZE 10
-#define ESC_BOOTTIME 5000               // 5 seconds
-#define ESC_REQUEST_TIMEOUT 100         // 100 ms (data transfer takes only 900us)
-
-static bool tlmFrameDone = false;
-static uint8_t tlm[ESC_TLM_BUFFSIZE] = { 0, };
-static uint8_t tlmFramePosition = 0;
-static serialPort_t *escTelemetryPort = NULL;
-static esc_telemetry_t escTelemetryData[MAX_SUPPORTED_MOTORS];
-static uint32_t escTriggerTimestamp = -1;
-static uint32_t escTriggerLastTimestamp = -1;
-static uint8_t timeoutRetryCount = 0;
-
-static uint8_t escTelemetryMotor = 0;      // motor index
-static bool escTelemetryEnabled = false;
-static escTlmTriggerState_t escTelemetryTriggerState = ESC_TLM_TRIGGER_WAIT;
-
-static int16_t escVbat = 0;
-static int16_t escCurrent = 0;
-static int16_t escConsumption = 0;
-
-static void escTelemetryDataReceive(uint16_t c);
-static uint8_t update_crc8(uint8_t crc, uint8_t crc_seed);
-static uint8_t get_crc8(uint8_t *Buf, uint8_t BufLen);
-static void selectNextMotor(void);
-
-bool isEscTelemetryActive(void)
-{
-    return escTelemetryEnabled;
-}
-
-int16_t getEscTelemetryVbat(void)
-{
-    return escVbat / 10;
-}
-
-int16_t getEscTelemetryCurrent(void)
-{
-    return escCurrent;
-}
-
-int16_t getEscTelemetryConsumption(void)
-{
-    return escConsumption;
-}
-
-bool escTelemetryInit(void)
-{
-    serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_TELEMETRY_ESC);
-    if (!portConfig) {
-        return false;
-    }
-
-    portOptions_t options = (SERIAL_NOT_INVERTED);
-
-    // Initialize serial port
-    escTelemetryPort = openSerialPort(portConfig->identifier, FUNCTION_TELEMETRY_ESC, escTelemetryDataReceive, ESC_TLM_BAUDRATE, MODE_RX, options);
-
-    if (escTelemetryPort) {
-        escTelemetryEnabled = true;
-        batteryConfig()->currentMeterType = CURRENT_SENSOR_ESC;
-        batteryConfig()->batteryMeterType = BATTERY_SENSOR_ESC;
-    }
-
-    return escTelemetryPort != NULL;
-}
-
-void freeEscTelemetryPort(void)
-{
-    closeSerialPort(escTelemetryPort);
-    escTelemetryPort = NULL;
-    escTelemetryEnabled = false;
-}
-
-// Receive ISR callback
-static void escTelemetryDataReceive(uint16_t c)
-{
-    // KISS ESC sends some data during startup, ignore this for now (maybe future use)
-    // startup data could be firmware version and serialnumber
-
-    if (escTelemetryTriggerState == ESC_TLM_TRIGGER_WAIT) return;
-
-    tlm[tlmFramePosition] = (uint8_t)c;
-
-    if (tlmFramePosition == ESC_TLM_BUFFSIZE - 1) {
-        tlmFrameDone = true;
-        tlmFramePosition = 0;
-    } else {
-        tlmFramePosition++;
-    }
-}
-
-uint8_t escTelemetryFrameStatus(void)
-{
-    uint8_t frameStatus = ESC_TLM_FRAME_PENDING;
-    uint16_t chksum, tlmsum;
-
-    if (!tlmFrameDone) {
-        return frameStatus;
-    }
-
-    tlmFrameDone = false;
-
-    // Get CRC8 checksum
-    chksum = get_crc8(tlm, ESC_TLM_BUFFSIZE - 1);
-    tlmsum = tlm[ESC_TLM_BUFFSIZE - 1];     // last byte contains CRC value
-
-    if (chksum == tlmsum) {
-        escTelemetryData[escTelemetryMotor].skipped = false;
-        escTelemetryData[escTelemetryMotor].temperature = tlm[0];
-        escTelemetryData[escTelemetryMotor].voltage = tlm[1] << 8 | tlm[2];
-        escTelemetryData[escTelemetryMotor].current = tlm[3] << 8 | tlm[4];
-        escTelemetryData[escTelemetryMotor].consumption = tlm[5] << 8 | tlm[6];
-        escTelemetryData[escTelemetryMotor].rpm = tlm[7] << 8 | tlm[8];
-
-        frameStatus = ESC_TLM_FRAME_COMPLETE;
-    }
-
-    return frameStatus;
-}
-
-void escTelemetryProcess(timeUs_t currentTimeUs)
-{
-    const timeMs_t currentTimeMs = currentTimeUs / 1000;
-
-    if (!escTelemetryEnabled) {
-        return;
-    }
-
-    // Wait period of time before requesting telemetry (let the system boot first)
-    if (millis() < ESC_BOOTTIME) {
-        return;
-    }
-    else if (escTelemetryTriggerState == ESC_TLM_TRIGGER_WAIT) {
-        // Ready for starting requesting telemetry
-        escTelemetryTriggerState = ESC_TLM_TRIGGER_READY;
-        escTelemetryMotor = 0;
-        escTriggerTimestamp = currentTimeMs;
-        escTriggerLastTimestamp = escTriggerTimestamp;
-    }
-    else if (escTelemetryTriggerState == ESC_TLM_TRIGGER_READY) {
-        if (debugMode == DEBUG_ESC_TELEMETRY) debug[0] = escTelemetryMotor+1;
-
-        motorDmaOutput_t * const motor = getMotorDmaOutput(escTelemetryMotor);
-        motor->requestTelemetry = true;
-        escTelemetryTriggerState = ESC_TLM_TRIGGER_PENDING;
-    }
-
-    if (escTriggerTimestamp + ESC_REQUEST_TIMEOUT < currentTimeMs) {
-        // ESC did not repond in time, retry
-        timeoutRetryCount++;
-        escTriggerTimestamp = currentTimeMs;
-        escTelemetryTriggerState = ESC_TLM_TRIGGER_READY;
-
-        if (timeoutRetryCount == 4) {
-            // Not responding after 3 times, skip motor
-            escTelemetryData[escTelemetryMotor].skipped = true;
-            selectNextMotor();
-        }
-
-        if (debugMode == DEBUG_ESC_TELEMETRY) debug[1]++;
-    }
-
-    // Get received frame status
-    uint8_t state = escTelemetryFrameStatus();
-
-    if (state == ESC_TLM_FRAME_COMPLETE) {
-        // Wait until all ESCs are processed
-        if (escTelemetryMotor == getMotorCount()-1) {
-            escCurrent = 0;
-            escConsumption = 0;
-            escVbat = 0;
-
-            for (int i = 0; i < getMotorCount(); i++) {
-                if (!escTelemetryData[i].skipped) {
-                    escVbat =  i > 0 ? ((escVbat + escTelemetryData[i].voltage) / 2) : escTelemetryData[i].voltage;
-                    escCurrent = escCurrent + escTelemetryData[i].current;
-                    escConsumption = escConsumption + escTelemetryData[i].consumption;
-                }
-            }
-        }
-
-        if (debugMode == DEBUG_ESC_TELEMETRY) debug[2] = escVbat;
-        if (debugMode == DEBUG_ESC_TELEMETRY) debug[3] = escCurrent;
-
-        selectNextMotor();
-        escTelemetryTriggerState = ESC_TLM_TRIGGER_READY;
-    }
-
-    if (escTriggerLastTimestamp + 10000 < currentTimeMs) {
-        // ESCs did not respond for 10 seconds
-        // Disable ESC telemetry and fallback to onboard vbat sensor
-        freeEscTelemetryPort();
-        escVbat = 0;
-        escCurrent = 0;
-        escConsumption = 0;
-    }
-}
-
-static void selectNextMotor(void)
-{
-    escTelemetryMotor++;
-    if (escTelemetryMotor == getMotorCount()) {
-        escTelemetryMotor = 0;
-    }
-    escTriggerTimestamp = millis();
-    escTriggerLastTimestamp = escTriggerTimestamp;
-}
-
-//-- CRC
-
-static uint8_t update_crc8(uint8_t crc, uint8_t crc_seed)
-{
-    uint8_t crc_u = crc;
-    crc_u ^= crc_seed;
-
-    for (int i=0; i<8; i++) {
-        crc_u = ( crc_u & 0x80 ) ? 0x7 ^ ( crc_u << 1 ) : ( crc_u << 1 );
-    }
-
-    return (crc_u);
-}
-
-static uint8_t get_crc8(uint8_t *Buf, uint8_t BufLen)
-{
-    uint8_t crc = 0;
-    for(int i=0; i<BufLen; i++) crc = update_crc8(Buf[i], crc);
-    return (crc);
-}
-
-#endif

src/main/telemetry/esc_telemetry.h

@@ -1,12 +0,0 @@
-#pragma once
-
-#include "common/time.h"
-
-uint8_t escTelemetryFrameStatus(void);
-bool escTelemetryInit(void);
-bool isEscTelemetryActive(void);
-int16_t getEscTelemetryVbat(void);
-int16_t getEscTelemetryCurrent(void);
-int16_t getEscTelemetryConsumption(void);
-
-void escTelemetryProcess(timeUs_t currentTimeUs);

src/main/telemetry/frsky.c

@@ -529,7 +529,7 @@ void handleFrSkyTelemetry(rxConfig_t *rxConfig, uint16_t deadband3d_throttle)
         sendTemperature1();
         sendThrottleOrBatterySizeAsRpm(rxConfig, deadband3d_throttle);
 
-        if ((feature(FEATURE_VBAT) || feature(FEATURE_ESC_TELEMETRY)) && batteryCellCount > 0) {
+        if ((feature(FEATURE_VBAT) || feature(FEATURE_ESC_SENSOR)) && batteryCellCount > 0) {
             sendVoltage();
             sendVoltageAmp();
             sendAmperage();

src/main/telemetry/smartport.c

@@ -646,7 +646,7 @@ void handleSmartPortTelemetry(void)
                 }
                 break;
             case FSSP_DATAID_CURRENT    :
-                if (feature(FEATURE_CURRENT_METER) || feature(FEATURE_ESC_TELEMETRY)) {
+                if (feature(FEATURE_CURRENT_METER) || feature(FEATURE_ESC_SENSOR)) {
                     smartPortSendPackage(id, amperage / 10); // given in 10mA steps, unknown requested unit
                     smartPortHasRequest = 0;
                 }
@@ -659,7 +659,7 @@ void handleSmartPortTelemetry(void)
                 }
                 break;
             case FSSP_DATAID_FUEL       :
-                if (feature(FEATURE_CURRENT_METER) || feature(FEATURE_ESC_TELEMETRY)) {
+                if (feature(FEATURE_CURRENT_METER) || feature(FEATURE_ESC_SENSOR)) {
                     smartPortSendPackage(id, mAhDrawn); // given in mAh, unknown requested unit
                     smartPortHasRequest = 0;
                 }