Merge pull request #1525 from basdelfos/esc_telemetry_dshot_rebase

Implementation of KISS ESC Telemetry protocol (rebase)

Makefile

@@ -589,7 +589,8 @@ HIGHEND_SRC = \
             telemetry/hott.c \
             telemetry/smartport.c \
             telemetry/ltm.c \
-            telemetry/mavlink.c
+            telemetry/mavlink.c \
+            telemetry/esc_telemetry.c \
 
 ifeq ($(TARGET),$(filter $(TARGET),$(F4_TARGETS)))
 VCP_SRC = \

src/main/blackbox/blackbox.c

@@ -808,7 +808,7 @@ void startBlackbox(void)
         blackboxHistory[1] = &blackboxHistoryRing[1];
         blackboxHistory[2] = &blackboxHistoryRing[2];
 
-        vbatReference = vbatLatestADC;
+        vbatReference = vbatLatest;
 
         //No need to clear the content of blackboxHistoryRing since our first frame will be an intra which overwrites it
 
@@ -1005,8 +1005,8 @@ static void loadMainState(uint32_t currentTime)
         blackboxCurrent->motor[i] = motor[i];
     }
 
-    blackboxCurrent->vbatLatest = vbatLatestADC;
+    blackboxCurrent->vbatLatest = vbatLatest;
-    blackboxCurrent->amperageLatest = amperageLatestADC;
+    blackboxCurrent->amperageLatest = amperageLatest;
 
 #ifdef MAG
     for (i = 0; i < XYZ_AXIS_COUNT; i++) {
@@ -1625,4 +1625,3 @@ void initBlackbox(void)
     }
 }
 #endif
-

src/main/build/debug.h

@@ -59,5 +59,6 @@ typedef enum {
     DEBUG_VELOCITY,
     DEBUG_DTERM_FILTER,
     DEBUG_ANGLERATE,
+    DEBUG_ESC_TELEMETRY,
     DEBUG_COUNT
 } debugType_e;

src/main/drivers/pwm_output.h

@@ -64,6 +64,7 @@ typedef struct {
     const timerHardware_t *timerHardware;
     uint16_t value;
     uint16_t timerDmaSource;
+    volatile bool requestTelemetry;
 #if defined(STM32F3) || defined(STM32F4) || defined(STM32F7)
     uint32_t dmaBuffer[MOTOR_DMA_BUFFER_SIZE];
 #else
@@ -75,6 +76,8 @@ typedef struct {
 #endif
 } motorDmaOutput_t;
 
+motorDmaOutput_t *getMotorDmaOutput(uint8_t index);
+
 extern bool pwmMotorsEnabled;
 
 struct timerHardware_s;
@@ -111,4 +114,3 @@ pwmOutputPort_t *pwmGetMotors(void);
 bool pwmIsSynced(void);
 void pwmDisableMotors(void);
 void pwmEnableMotors(void);
-

src/main/drivers/pwm_output_stm32f3xx.c

@@ -46,6 +46,11 @@ static uint8_t dmaMotorTimerCount = 0;
 static motorDmaTimer_t dmaMotorTimers[MAX_DMA_TIMERS];
 static motorDmaOutput_t dmaMotors[MAX_SUPPORTED_MOTORS];
 
+motorDmaOutput_t *getMotorDmaOutput(uint8_t index)
+{
+    return &dmaMotors[index];
+}
+
 uint8_t getTimerIndex(TIM_TypeDef *timer)
 {
     for (int i = 0; i < dmaMotorTimerCount; i++) {
@@ -66,7 +71,9 @@ void pwmWriteDigital(uint8_t index, uint16_t value)
 
     motorDmaOutput_t * const motor = &dmaMotors[index];
 
-    uint16_t packet = (value << 1) | 0;                            // Here goes telemetry bit (false for now)
+    uint16_t packet = (value << 1) | (motor->requestTelemetry ? 1 : 0);
+    motor->requestTelemetry = false;    // reset telemetry request to make sure it's triggered only once in a row
+
     // compute checksum
     int csum = 0;
     int csum_data = packet;

src/main/drivers/pwm_output_stm32f4xx.c

@@ -45,6 +45,11 @@ static uint8_t dmaMotorTimerCount = 0;
 static motorDmaTimer_t dmaMotorTimers[MAX_DMA_TIMERS];
 static motorDmaOutput_t dmaMotors[MAX_SUPPORTED_MOTORS];
 
+motorDmaOutput_t *getMotorDmaOutput(uint8_t index)
+{
+    return &dmaMotors[index];
+}
+
 uint8_t getTimerIndex(TIM_TypeDef *timer)
 {
     for (int i = 0; i < dmaMotorTimerCount; i++) {
@@ -64,7 +69,9 @@ void pwmWriteDigital(uint8_t index, uint16_t value)
 
     motorDmaOutput_t * const motor = &dmaMotors[index];
 
-    uint16_t packet = (value << 1) | 0;                            // Here goes telemetry bit (false for now)
+    uint16_t packet = (value << 1) | (motor->requestTelemetry ? 1 : 0);
+    motor->requestTelemetry = false;    // reset telemetry request to make sure it's triggered only once in a row
+
     // compute checksum
     int csum = 0;
     int csum_data = packet;

src/main/drivers/pwm_output_stm32f7xx.c

@@ -44,6 +44,11 @@ static uint8_t dmaMotorTimerCount = 0;
 static motorDmaTimer_t dmaMotorTimers[MAX_DMA_TIMERS];
 static motorDmaOutput_t dmaMotors[MAX_SUPPORTED_MOTORS];
 
+motorDmaOutput_t *getMotorDmaOutput(uint8_t index)
+{
+    return &dmaMotors[index];
+}
+
 uint8_t getTimerIndex(TIM_TypeDef *timer)
 {
     for (int i = 0; i < dmaMotorTimerCount; i++) {
@@ -64,7 +69,9 @@ void pwmWriteDigital(uint8_t index, uint16_t value)
 
     motorDmaOutput_t * const motor = &dmaMotors[index];
 
-    uint16_t packet = (value << 1) | 0;                            // Here goes telemetry bit (false for now)
+    uint16_t packet = (value << 1) | (motor->requestTelemetry ? 1 : 0);
+    motor->requestTelemetry = false;    // reset telemetry request to make sure it's triggered only once in a row
+
     // compute checksum
     int csum = 0;
     int csum_data = packet;

src/main/fc/config.c

@@ -424,6 +424,7 @@ void resetBatteryConfig(batteryConfig_t *batteryConfig)
     batteryConfig->vbatmincellvoltage = 33;
     batteryConfig->vbatwarningcellvoltage = 35;
     batteryConfig->vbathysteresis = 1;
+    batteryConfig->batteryMeterType = BATTERY_SENSOR_ADC;
     batteryConfig->currentMeterOffset = 0;
     batteryConfig->currentMeterScale = 400; // for Allegro ACS758LCB-100U (40mV/A)
     batteryConfig->batteryCapacity = 0;

src/main/fc/config.h

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

src/main/fc/fc_tasks.c

@@ -66,6 +66,7 @@
 #include "scheduler/scheduler.h"
 
 #include "telemetry/telemetry.h"
+#include "telemetry/esc_telemetry.h"
 
 #include "config/feature.h"
 #include "config/config_profile.h"
@@ -102,7 +103,7 @@ static void taskUpdateBattery(uint32_t currentTime)
 {
 #ifdef USE_ADC
     static uint32_t vbatLastServiced = 0;
-    if (feature(FEATURE_VBAT)) {
+    if (feature(FEATURE_VBAT) || feature(FEATURE_ESC_TELEMETRY)) {
         if (cmp32(currentTime, vbatLastServiced) >= VBATINTERVAL) {
             vbatLastServiced = currentTime;
             updateBattery();
@@ -111,7 +112,7 @@ static void taskUpdateBattery(uint32_t currentTime)
 #endif
 
     static uint32_t ibatLastServiced = 0;
-    if (feature(FEATURE_CURRENT_METER)) {
+    if (feature(FEATURE_CURRENT_METER) || feature(FEATURE_ESC_TELEMETRY)) {
         const int32_t ibatTimeSinceLastServiced = cmp32(currentTime, ibatLastServiced);
 
         if (ibatTimeSinceLastServiced >= IBATINTERVAL) {
@@ -194,6 +195,15 @@ static void taskTelemetry(uint32_t currentTime)
 }
 #endif
 
+#ifdef USE_ESC_TELEMETRY
+static void taskEscTelemetry(uint32_t currentTime)
+{
+    if (feature(FEATURE_ESC_TELEMETRY)) {
+        escTelemetryProcess(currentTime);
+     }
+ }
+#endif
+
 void fcTasksInit(void)
 {
     schedulerInit();
@@ -254,6 +264,9 @@ void fcTasksInit(void)
 #ifdef USE_BST
     setTaskEnabled(TASK_BST_MASTER_PROCESS, true);
 #endif
+#ifdef USE_ESC_TELEMETRY
+    setTaskEnabled(TASK_ESC_TELEMETRY, feature(FEATURE_ESC_TELEMETRY));
+#endif
 #ifdef CMS
 #ifdef USE_MSP_DISPLAYPORT
     setTaskEnabled(TASK_CMS, true);
@@ -421,6 +434,15 @@ cfTask_t cfTasks[TASK_COUNT] = {
     },
 #endif
 
+#ifdef USE_ESC_TELEMETRY
+    [TASK_ESC_TELEMETRY] = {
+        .taskName = "ESC_TELEMETRY",
+        .taskFunc = taskEscTelemetry,
+        .desiredPeriod = 1000000 / 100,         // 100 Hz
+        .staticPriority = TASK_PRIORITY_LOW,
+    },
+#endif
+
 #ifdef CMS
     [TASK_CMS] = {
         .taskName = "CMS",

src/main/flight/mixer.c

@@ -240,6 +240,10 @@ static motorMixer_t *customMixers;
 static uint16_t disarmMotorOutput, minMotorOutputNormal, maxMotorOutputNormal, deadbandMotor3dHigh, deadbandMotor3dLow;
 static float rcCommandThrottleRange;
 
+uint8_t getMotorCount() {
+    return motorCount;
+}
+
 bool isMotorProtocolDshot(void) {
 #ifdef USE_DSHOT
     if (motorConfig->motorPwmProtocol == PWM_TYPE_DSHOT150 || motorConfig->motorPwmProtocol == PWM_TYPE_DSHOT300 || motorConfig->motorPwmProtocol == PWM_TYPE_DSHOT600)

src/main/flight/mixer.h

@@ -115,6 +115,8 @@ extern int16_t motor_disarmed[MAX_SUPPORTED_MOTORS];
 struct motorConfig_s;
 struct rxConfig_s;
 
+uint8_t getMotorCount();
+
 void mixerUseConfigs(
         flight3DConfig_t *flight3DConfigToUse,
         struct motorConfig_s *motorConfigToUse,

src/main/io/serial.h

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

src/main/io/serial_cli.c

@@ -231,7 +231,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", NULL
+    "SDCARD", "VTX", "RX_SPI", "SOFTSPI", "ESC_TELEMETRY", NULL
 };
 
 // sync this with rxFailsafeChannelMode_e
@@ -513,6 +513,7 @@ static const char * const lookupTableDebug[DEBUG_COUNT] = {
     "VELOCITY",
     "DFILTER",
     "ANGLERATE",
+    "ESC_TELEMETRY",
 };
 
 #ifdef OSD
@@ -3600,14 +3601,14 @@ static void cliTasks(char *cmdline)
                 subTaskFrequency = (int)(1000000.0f / ((float)cycleTime));
                 taskFrequency = subTaskFrequency / masterConfig.pid_process_denom;
                 if (masterConfig.pid_process_denom > 1) {
-                    cliPrintf("%02d - (%12s) ", taskId, taskInfo.taskName);
+                    cliPrintf("%02d - (%13s) ", taskId, taskInfo.taskName);
                 } else {
                     taskFrequency = subTaskFrequency;
-                    cliPrintf("%02d - (%8s/%3s) ", taskId, taskInfo.subTaskName, taskInfo.taskName);
+                    cliPrintf("%02d - (%9s/%3s) ", taskId, taskInfo.subTaskName, taskInfo.taskName);
                 }
             } else {
                 taskFrequency = (int)(1000000.0f / ((float)taskInfo.latestDeltaTime));
-                cliPrintf("%02d - (%12s) ", taskId, taskInfo.taskName);
+                cliPrintf("%02d - (%13s) ", taskId, taskInfo.taskName);
             }
             const int maxLoad = (taskInfo.maxExecutionTime * taskFrequency + 5000) / 1000;
             const int averageLoad = (taskInfo.averageExecutionTime * taskFrequency + 5000) / 1000;
@@ -3619,11 +3620,11 @@ static void cliTasks(char *cmdline)
                     taskFrequency, taskInfo.maxExecutionTime, taskInfo.averageExecutionTime,
                     maxLoad/10, maxLoad%10, averageLoad/10, averageLoad%10, taskInfo.totalExecutionTime / 1000);
             if (taskId == TASK_GYROPID && masterConfig.pid_process_denom > 1) {
-                cliPrintf("   - (%12s) %6d\r\n", taskInfo.subTaskName, subTaskFrequency);
+                cliPrintf("   - (%13s) %6d\r\n", taskInfo.subTaskName, subTaskFrequency);
             }
         }
     }
-    cliPrintf("Total (excluding SERIAL) %22d.%1d%% %4d.%1d%%\r\n", maxLoadSum/10, maxLoadSum%10, averageLoadSum/10, averageLoadSum%10);
+    cliPrintf("Total (excluding SERIAL) %23d.%1d%% %4d.%1d%%\r\n", maxLoadSum/10, maxLoadSum%10, averageLoadSum/10, averageLoadSum%10);
 }
 #endif
 

src/main/main.c

@@ -103,6 +103,7 @@
 #include "sensors/initialisation.h"
 
 #include "telemetry/telemetry.h"
+#include "telemetry/esc_telemetry.h"
 
 #include "flight/pid.h"
 #include "flight/imu.h"
@@ -490,6 +491,12 @@ void init(void)
     }
 #endif
 
+#ifdef USE_ESC_TELEMETRY
+    if (feature(FEATURE_ESC_TELEMETRY)) {
+        escTelemetryInit();
+    }
+#endif
+
 #ifdef USB_CABLE_DETECTION
     usbCableDetectInit();
 #endif

src/main/scheduler/scheduler.h

@@ -85,6 +85,9 @@ typedef enum {
 #ifdef USE_BST
     TASK_BST_MASTER_PROCESS,
 #endif
+#ifdef USE_ESC_TELEMETRY
+    TASK_ESC_TELEMETRY,
+#endif
 #ifdef CMS
     TASK_CMS,
 #endif

src/main/sensors/battery.c

@@ -35,11 +35,15 @@
 
 #include "sensors/battery.h"
 
+#include "telemetry/esc_telemetry.h"
+
 #include "fc/rc_controls.h"
 #include "io/beeper.h"
 
 #include "rx/rx.h"
 
+#include "common/utils.h"
+
 #define VBATT_LPF_FREQ  0.4f
 
 // Battery monitoring stuff
@@ -48,8 +52,8 @@ uint16_t batteryWarningVoltage;
 uint16_t batteryCriticalVoltage;
 
 uint16_t vbat = 0;                  // battery voltage in 0.1V steps (filtered)
-uint16_t vbatLatestADC = 0;         // most recent unsmoothed raw reading from vbat ADC
+uint16_t vbatLatest = 0;            // most recent unsmoothed value
-uint16_t amperageLatestADC = 0;     // most recent raw reading from current ADC
+uint16_t amperageLatest = 0;        // most recent value
 
 int32_t amperage = 0;               // amperage read by current sensor in centiampere (1/100th A)
 int32_t mAhDrawn = 0;               // milliampere hours drawn from the battery since start
@@ -69,7 +73,17 @@ static void updateBatteryVoltage(void)
     static bool vbatFilterIsInitialised;
 
     // store the battery voltage with some other recent battery voltage readings
-    uint16_t vbatSample = vbatLatestADC = adcGetChannel(ADC_BATTERY);
+    uint16_t vbatSample;
+
+    #ifdef USE_ESC_TELEMETRY
+    if (batteryConfig->batteryMeterType == BATTERY_SENSOR_ESC && isEscTelemetryActive()) {
+        vbatSample = vbatLatest = getEscTelemetryVbat();
+    }
+    else
+    #endif
+    {
+        vbatSample = vbatLatest = batteryAdcToVoltage(adcGetChannel(ADC_BATTERY));
+    }
 
     if (debugMode == DEBUG_BATTERY) debug[0] = vbatSample;
 
@@ -77,8 +91,7 @@ static void updateBatteryVoltage(void)
         biquadFilterInitLPF(&vbatFilter, VBATT_LPF_FREQ, 50000); //50HZ Update
         vbatFilterIsInitialised = true;
     }
-    vbatSample = biquadFilterApply(&vbatFilter, vbatSample);
+    vbat = biquadFilterApply(&vbatFilter, vbatSample);
-    vbat = batteryAdcToVoltage(vbatSample);
 
     if (debugMode == DEBUG_BATTERY) debug[1] = vbat;
 }
@@ -87,12 +100,12 @@ static void updateBatteryVoltage(void)
 
 void updateBattery(void)
 {
-    uint16_t vbatPreviousADC = vbatLatestADC;
+    uint16_t vbatPrevious = vbatLatest;
     updateBatteryVoltage();
-    uint16_t vbatMeasured = batteryAdcToVoltage(vbatLatestADC);
+    uint16_t vbatMeasured = vbatLatest;
 
     /* battery has just been connected*/
-    if (batteryState == BATTERY_NOT_PRESENT && (ARMING_FLAG(ARMED) || (vbat > batteryConfig->batterynotpresentlevel && ABS(vbatMeasured - batteryAdcToVoltage(vbatPreviousADC)) <= VBAT_STABLE_MAX_DELTA))) {
+    if (batteryState == BATTERY_NOT_PRESENT && (ARMING_FLAG(ARMED) || (vbat > batteryConfig->batterynotpresentlevel && ABS(vbatMeasured - vbatPrevious) <= VBAT_STABLE_MAX_DELTA))) {
         /* Actual battery state is calculated below, this is really BATTERY_PRESENT */
         batteryState = BATTERY_OK;
 
@@ -105,13 +118,16 @@ void updateBattery(void)
         batteryWarningVoltage = batteryCellCount * batteryConfig->vbatwarningcellvoltage;
         batteryCriticalVoltage = batteryCellCount * batteryConfig->vbatmincellvoltage;
     /* battery has been disconnected - can take a while for filter cap to disharge so we use a threshold of batteryConfig->batterynotpresentlevel */
-    } else if (batteryState != BATTERY_NOT_PRESENT && !ARMING_FLAG(ARMED) && vbat <= batteryConfig->batterynotpresentlevel && ABS(vbatMeasured - batteryAdcToVoltage(vbatPreviousADC)) <= VBAT_STABLE_MAX_DELTA) {
+    } else if (batteryState != BATTERY_NOT_PRESENT && !ARMING_FLAG(ARMED) && vbat <= batteryConfig->batterynotpresentlevel && ABS(vbatMeasured - vbatPrevious) <= VBAT_STABLE_MAX_DELTA) {
         batteryState = BATTERY_NOT_PRESENT;
         batteryCellCount = 0;
         batteryWarningVoltage = 0;
         batteryCriticalVoltage = 0;
     }
 
+    if (debugMode == DEBUG_BATTERY) debug[2] = batteryState;
+    if (debugMode == DEBUG_BATTERY) debug[3] = batteryCellCount;
+
     switch(batteryState)
     {
         case BATTERY_OK:
@@ -177,16 +193,22 @@ static int32_t currentSensorToCentiamps(uint16_t src)
 
 void updateCurrentMeter(int32_t lastUpdateAt, rxConfig_t *rxConfig, uint16_t deadband3d_throttle)
 {
-    static int32_t amperageRaw = 0;
+    #ifdef USE_ESC_TELEMETRY
+    UNUSED(lastUpdateAt);
+    #else
     static int64_t mAhdrawnRaw = 0;
+    #endif
+
+    static int32_t amperageRaw = 0;
+
     int32_t throttleOffset = (int32_t)rcCommand[THROTTLE] - 1000;
     int32_t throttleFactor = 0;
 
     switch(batteryConfig->currentMeterType) {
         case CURRENT_SENSOR_ADC:
             amperageRaw -= amperageRaw / 8;
-            amperageRaw += (amperageLatestADC = adcGetChannel(ADC_CURRENT));
+            amperageRaw += adcGetChannel(ADC_CURRENT);
-            amperage = currentSensorToCentiamps(amperageRaw / 8);
+            amperage = amperageLatest = currentSensorToCentiamps(amperageRaw / 8);
             break;
         case CURRENT_SENSOR_VIRTUAL:
             amperage = (int32_t)batteryConfig->currentMeterOffset;
@@ -201,10 +223,20 @@ void updateCurrentMeter(int32_t lastUpdateAt, rxConfig_t *rxConfig, uint16_t dea
         case CURRENT_SENSOR_NONE:
             amperage = 0;
             break;
+        case CURRENT_SENSOR_ESC:
+            #ifdef USE_ESC_TELEMETRY
+            if (batteryConfig->currentMeterType == CURRENT_SENSOR_ESC && isEscTelemetryActive()) {
+                amperage = getEscTelemetryCurrent();
+                mAhDrawn = getEscTelemetryConsumption();
+            }
+            #endif
+            break;
     }
 
+    #ifndef USE_ESC_TELEMETRY
     mAhdrawnRaw += (amperage * lastUpdateAt) / 1000;
     mAhDrawn = mAhdrawnRaw / (3600 * 100);
+    #endif
 }
 
 float calculateVbatPidCompensation(void) {

src/main/sensors/battery.h

@@ -31,9 +31,17 @@ typedef enum {
     CURRENT_SENSOR_NONE = 0,
     CURRENT_SENSOR_ADC,
     CURRENT_SENSOR_VIRTUAL,
-    CURRENT_SENSOR_MAX = CURRENT_SENSOR_VIRTUAL
+    CURRENT_SENSOR_ESC,
+    CURRENT_SENSOR_MAX = CURRENT_SENSOR_ESC
 } currentSensor_e;
 
+typedef enum {
+    BATTERY_SENSOR_NONE = 0,
+    BATTERY_SENSOR_ADC,
+    BATTERY_SENSOR_ESC,
+    BATTERY_SENSOR_MAX = BATTERY_SENSOR_ESC
+} batterySensor_e;
+
 typedef struct batteryConfig_s {
     uint8_t vbatscale;                      // adjust this to match battery voltage to reported value
     uint8_t vbatresdivval;                  // resistor divider R2 (default NAZE 10(K))
@@ -42,10 +50,11 @@ typedef struct batteryConfig_s {
     uint8_t vbatmincellvoltage;             // minimum voltage per cell, this triggers battery critical alarm, in 0.1V units, default is 33 (3.3V)
     uint8_t vbatwarningcellvoltage;         // warning voltage per cell, this triggers battery warning alarm, in 0.1V units, default is 35 (3.5V)
     uint8_t vbathysteresis;                 // hysteresis for alarm, default 1 = 0.1V
+    batterySensor_e batteryMeterType;       // type of battery meter uses, either ADC or ESC
 
     int16_t currentMeterScale;             // scale the current sensor output voltage to milliamps. Value in 1/10th mV/A
     uint16_t currentMeterOffset;            // offset of the current sensor in millivolt steps
-    currentSensor_e  currentMeterType;      // type of current meter used, either ADC or virtual
+    currentSensor_e  currentMeterType;      // type of current meter used, either ADC, Virtual or ESC
 
     // FIXME this doesn't belong in here since it's a concern of MSP, not of the battery code.
     uint8_t multiwiiCurrentMeterOutput;     // if set to 1 output the amperage in milliamp steps instead of 0.01A steps via msp
@@ -62,10 +71,10 @@ typedef enum {
 
 extern uint16_t vbat;
 extern uint16_t vbatRaw;
-extern uint16_t vbatLatestADC;
+extern uint16_t vbatLatest;
 extern uint8_t batteryCellCount;
 extern uint16_t batteryWarningVoltage;
-extern uint16_t amperageLatestADC;
+extern uint16_t amperageLatest;
 extern int32_t amperage;
 extern int32_t mAhDrawn;
 

src/main/target/ANYFCF7/target.h

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

src/main/target/BETAFLIGHTF3/target.h

@@ -45,6 +45,7 @@
 #define USE_EXTI
 
 #define USE_DSHOT
+#define USE_ESC_TELEMETRY
 #define REMAP_TIM16_DMA
 #define REMAP_TIM17_DMA
 

src/main/target/BLUEJAYF4/target.h

@@ -145,6 +145,7 @@
 #define VBAT_ADC_PIN            PC3
 
 #define USE_DSHOT
+#define USE_ESC_TELEMETRY
 #define LED_STRIP
 
 #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT
@@ -166,4 +167,3 @@
 
 #define USABLE_TIMER_CHANNEL_COUNT 7
 #define USED_TIMERS             ( TIM_N(2) | TIM_N(3) | TIM_N(5) | TIM_N(8) | TIM_N(9))
-

src/main/target/DOGE/target.h

@@ -126,6 +126,7 @@
 
 #define LED_STRIP
 #define USE_DSHOT
+#define USE_ESC_TELEMETRY
 
 #define DEFAULT_RX_FEATURE      FEATURE_RX_PPM
 
@@ -146,4 +147,3 @@
 // only 6 outputs available on hardware
 #define USABLE_TIMER_CHANNEL_COUNT 10
 #define USED_TIMERS             (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(15))
-

src/main/target/FURYF3/target.h

@@ -162,6 +162,7 @@
 #define USE_SERIAL_4WAY_BLHELI_INTERFACE
 
 #define USE_DSHOT
+#define USE_ESC_TELEMETRY
 #define REMAP_TIM17_DMA
 
 #define TARGET_IO_PORTA         0xffff
@@ -171,4 +172,3 @@
 
 #define USABLE_TIMER_CHANNEL_COUNT 8
 #define USED_TIMERS             (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(8) | TIM_N(16) | TIM_N(17))
-

src/main/target/FURYF4/target.h

@@ -176,6 +176,7 @@
 #define USE_SERIAL_4WAY_BLHELI_INTERFACE
 
 #define USE_DSHOT
+#define USE_ESC_TELEMETRY
 
 #define TARGET_IO_PORTA         0xffff
 #define TARGET_IO_PORTB         0xffff
@@ -184,4 +185,3 @@
 
 #define USABLE_TIMER_CHANNEL_COUNT 6
 #define USED_TIMERS             ( TIM_N(2) | TIM_N(3) | TIM_N(5) | TIM_N(8) )
-

src/main/target/FURYF7/target.h

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

src/main/target/IMPULSERCF3/target.h

@@ -47,6 +47,7 @@
 #define USE_FLASH_M25P16
 
 #define USE_DSHOT
+#define USE_ESC_TELEMETRY
 #define REMAP_TIM17_DMA
 
 #define USE_VCP
@@ -105,4 +106,3 @@
 #define TARGET_IO_PORTF         (BIT(0)|BIT(1)|BIT(3)|BIT(4))
 
 #define USED_TIMERS             (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(16) |TIM_N(17))
-

src/main/target/KISSFC/target.h

@@ -24,6 +24,10 @@
 #define SBUS_PORT_OPTIONS (SERIAL_STOPBITS_2 | SERIAL_PARITY_EVEN | SERIAL_INVERTED | SERIAL_BIDIR)
 
 #define USE_DSHOT
+#define USE_ESC_TELEMETRY
+
+#define USE_ESCSERIAL
+#define ESCSERIAL_TIMER_TX_HARDWARE 6
 
 #define LED0                    PB1
 

src/main/target/LUX_RACE/target.h

@@ -47,6 +47,7 @@
 #define ENSURE_MPU_DATA_READY_IS_LOW
 
 #define USE_DSHOT
+#define USE_ESC_TELEMETRY
 
 #define USE_SPI
 #define USE_SPI_DEVICE_1
@@ -173,4 +174,3 @@
 #define USABLE_TIMER_CHANNEL_COUNT 12
 #endif
 #define USED_TIMERS             (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(8) | TIM_N(15))
-

src/main/target/MOTOLAB/target.h

@@ -103,6 +103,7 @@
 
 #define LED_STRIP
 #define USE_DSHOT
+#define USE_ESC_TELEMETRY
 
 #define SPEKTRUM_BIND
 // USART2, PB4
@@ -120,4 +121,3 @@
 
 #define USABLE_TIMER_CHANNEL_COUNT 10
 #define USED_TIMERS             (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(15) | TIM_N(17))
-

src/main/target/MULTIFLITEPICO/target.h

@@ -106,6 +106,7 @@
 #define RSSI_ADC_PIN            PB2
 
 #define USE_DSHOT
+#define USE_ESC_TELEMETRY
 #define REMAP_TIM17_DMA
 
 // UART1 TX uses DMA1_Channel4, which is also used by dshot on motor 4

src/main/target/OMNIBUS/target.h

@@ -138,6 +138,7 @@
 #define SDCARD_SPI_FULL_SPEED_CLOCK_DIVIDER     2
 
 #define USE_DSHOT
+#define USE_ESC_TELEMETRY
 
 // 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

@@ -123,6 +123,7 @@
 //#define RSSI_ADC_PIN            PA0
 
 #define USE_DSHOT
+#define USE_ESC_TELEMETRY
 
 #define LED_STRIP
 

src/main/target/REVO/target.h

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

src/main/target/SOULF4/target.h

@@ -105,6 +105,7 @@
 
 #define LED_STRIP
 #define USE_DSHOT
+#define USE_ESC_TELEMETRY
 
 #define SPEKTRUM_BIND
 // USART3,

src/main/target/SPARKY2/target.h

@@ -36,6 +36,7 @@
 #define INVERTER_USART          USART6
 
 #define USE_DSHOT
+#define USE_ESC_TELEMETRY
 
 // MPU9250 interrupt
 #define USE_EXTI
@@ -132,4 +133,3 @@
 
 #define USABLE_TIMER_CHANNEL_COUNT 11
 #define USED_TIMERS  ( TIM_N(2) | TIM_N(3) | TIM_N(5) | TIM_N(12) | TIM_N(8) | TIM_N(9))
-

src/main/target/SPRACINGF3/target.h

@@ -116,6 +116,7 @@
 #define RSSI_ADC_PIN            PB2
 
 #define USE_DSHOT
+#define USE_ESC_TELEMETRY
 #define REMAP_TIM17_DMA
 
 // UART1 TX uses DMA1_Channel4, which is also used by dshot on motor 4

src/main/target/SPRACINGF3EVO/target.h

@@ -36,6 +36,7 @@
 #define ENSURE_MAG_DATA_READY_IS_HIGH
 
 #define USE_DSHOT
+#define USE_ESC_TELEMETRY
 
 #define GYRO
 #define USE_GYRO_SPI_MPU6500
@@ -156,4 +157,3 @@
 
 #define USABLE_TIMER_CHANNEL_COUNT 12 // PPM, 8 PWM, UART3 RX/TX, LED Strip
 #define USED_TIMERS             (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(8) | TIM_N(15))
-

src/main/target/STM32F3DISCOVERY/target.h

@@ -170,6 +170,7 @@
 #define EXTERNAL1_ADC_PIN       PC3
 
 #define USE_DSHOT
+#define USE_ESC_TELEMETRY
 
 #define LED_STRIP
 
@@ -192,4 +193,3 @@
 
 #define USABLE_TIMER_CHANNEL_COUNT 14
 #define USED_TIMERS             (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(8) | TIM_N(16) | TIM_N(17))
-

src/main/target/X_RACERSPI/target.h

@@ -102,6 +102,8 @@
 #define RSSI_ADC_PIN            PB2
 
 #define USE_DSHOT
+#define USE_ESC_TELEMETRY
+
 #define REMAP_TIM17_DMA
 // UART1 TX uses DMA1_Channel4, which is also used by dshot on motor 4
 #if defined(USE_UART1_TX_DMA) && defined(USE_DSHOT)
@@ -129,4 +131,3 @@
 
 #define USABLE_TIMER_CHANNEL_COUNT 17
 #define USED_TIMERS             ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(15) | TIM_N(16) | TIM_N(17) )
-

src/main/telemetry/esc_telemetry.c

@@ -0,0 +1,312 @@
+#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;
+        masterConfig.batteryConfig.currentMeterType = CURRENT_SENSOR_ESC;
+        masterConfig.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(uint32_t currentTime)
+{
+    uint32_t currentTimeMs = currentTime / 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

@@ -0,0 +1,10 @@
+#pragma once
+
+uint8_t escTelemetryFrameStatus(void);
+bool escTelemetryInit(void);
+bool isEscTelemetryActive(void);
+int16_t getEscTelemetryVbat(void);
+int16_t getEscTelemetryCurrent(void);
+int16_t getEscTelemetryConsumption(void);
+
+void escTelemetryProcess(uint32_t currentTime);

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) && batteryCellCount > 0) {
+        if ((feature(FEATURE_VBAT) || feature(FEATURE_ESC_TELEMETRY)) && 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)) {
+                if (feature(FEATURE_CURRENT_METER) || feature(FEATURE_ESC_TELEMETRY)) {
                     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)) {
+                if (feature(FEATURE_CURRENT_METER) || feature(FEATURE_ESC_TELEMETRY)) {
                     smartPortSendPackage(id, mAhDrawn); // given in mAh, unknown requested unit
                     smartPortHasRequest = 0;
                 }