Fixed current updates and review comments

src/main/drivers/pwm_output.h

@@ -52,6 +52,10 @@ typedef enum {
 #define PWM_BRUSHED_TIMER_MHZ 24
 #endif
 
+#ifdef USE_DSHOT
+#define MAX_DSHOT_MOTORS      4
+#endif
+
 #define MOTOR_DMA_BUFFER_SIZE 18 /* resolution + frame reset (2us) */
 
 typedef struct {

src/main/fc/fc_tasks.c

@@ -103,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();
@@ -112,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) {

src/main/sensors/battery.c

@@ -42,6 +42,8 @@
 
 #include "rx/rx.h"
 
+#include "common/utils.h"
+
 #define VBATT_LPF_FREQ  0.4f
 
 // Battery monitoring stuff
@@ -67,25 +69,28 @@ static uint16_t batteryAdcToVoltage(uint16_t src)
 
 static void updateBatteryVoltage(void)
 {
+    #ifdef USE_ESC_TELEMETRY
     if (batteryConfig->batteryMeterType == BATTERY_SENSOR_ESC) {
         vbat = getEscTelemetryVbat();
     }
-    else {
+    #else
-        static biquadFilter_t vbatFilter;
-        static bool vbatFilterIsInitialised;
 
-        // store the battery voltage with some other recent battery voltage readings
+    static biquadFilter_t vbatFilter;
-        uint16_t vbatSample = vbatLatestADC = adcGetChannel(ADC_BATTERY);
+    static bool vbatFilterIsInitialised;
 
-        if (debugMode == DEBUG_BATTERY) debug[0] = vbatSample;
+    // store the battery voltage with some other recent battery voltage readings
+    uint16_t vbatSample = vbatLatestADC = adcGetChannel(ADC_BATTERY);
 
-        if (!vbatFilterIsInitialised) {
+    if (debugMode == DEBUG_BATTERY) debug[0] = vbatSample;
-            biquadFilterInitLPF(&vbatFilter, VBATT_LPF_FREQ, 50000); //50HZ Update
+
-            vbatFilterIsInitialised = true;
+    if (!vbatFilterIsInitialised) {
-        }
+        biquadFilterInitLPF(&vbatFilter, VBATT_LPF_FREQ, 50000); //50HZ Update
-        vbatSample = biquadFilterApply(&vbatFilter, vbatSample);
+        vbatFilterIsInitialised = true;
-        vbat = batteryAdcToVoltage(vbatSample);
     }
+    vbatSample = biquadFilterApply(&vbatFilter, vbatSample);
+    vbat = batteryAdcToVoltage(vbatSample);
+
+    #endif
 
     if (debugMode == DEBUG_BATTERY) debug[1] = vbat;
 }
@@ -184,8 +189,14 @@ 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;
 
@@ -208,18 +219,21 @@ void updateCurrentMeter(int32_t lastUpdateAt, rxConfig_t *rxConfig, uint16_t dea
         case CURRENT_SENSOR_NONE:
             amperage = 0;
             break;
+        #ifdef USE_ESC_TELEMETRY
         case CURRENT_SENSOR_ESC:
             amperage = getEscTelemetryCurrent();
             break;
+        #endif
     }
 
+    #ifdef USE_ESC_TELEMETRY
     if (batteryConfig->currentMeterType == CURRENT_SENSOR_ESC) {
         mAhDrawn = getEscTelemetryConsumption();
     }
-    else {
+    #else
-        mAhdrawnRaw += (amperage * lastUpdateAt) / 1000;
+    mAhdrawnRaw += (amperage * lastUpdateAt) / 1000;
-        mAhDrawn = mAhdrawnRaw / (3600 * 100);
+    mAhDrawn = mAhdrawnRaw / (3600 * 100);
-    }
+    #endif
 }
 
 float calculateVbatPidCompensation(void) {

src/main/telemetry/esc_telemetry.c

@@ -58,11 +58,11 @@ set debug_mode = DEBUG_ESC_TELEMETRY in cli
 
 typedef struct {
     bool skipped;
-    uint8_t temperature;
+    int16_t temperature;
-    uint16_t voltage;
+    int16_t voltage;
-    uint16_t current;
+    int16_t current;
-    uint16_t consumption;
+    int16_t consumption;
-    uint16_t rpm;
+    int16_t rpm;
 } esc_telemetry_t;
 
 typedef enum {
@@ -93,9 +93,9 @@ static uint8_t escTelemetryMotor = 0;      // motor index 0 - 3
 static bool escTelemetryEnabled = false;
 static escTlmTriggerState_t escTelemetryTriggerState = ESC_TLM_TRIGGER_WAIT;
 
-static uint16_t escVbat = 0;
+static int16_t escVbat = 0;
-static uint16_t escCurrent = 0;
+static int16_t escCurrent = 0;
-static uint16_t escConsumption = 0;
+static int16_t escConsumption = 0;
 
 static void escTelemetryDataReceive(uint16_t c);
 static uint8_t update_crc8(uint8_t crc, uint8_t crc_seed);
@@ -107,17 +107,17 @@ bool isEscTelemetryActive(void)
     return escTelemetryEnabled;
 }
 
-uint16_t getEscTelemetryVbat(void)
+int16_t getEscTelemetryVbat(void)
 {
     return escVbat / 10;
 }
 
-uint16_t getEscTelemetryCurrent(void)
+int16_t getEscTelemetryCurrent(void)
 {
     return escCurrent;
 }
 
-uint16_t getEscTelemetryConsumption(void)
+int16_t getEscTelemetryConsumption(void)
 {
     return escConsumption;
 }
@@ -199,7 +199,7 @@ uint8_t escTelemetryFrameStatus(void)
 
 void escTelemetryProcess(uint32_t currentTime)
 {
-    UNUSED(currentTime);
+    uint32_t currentTimeMs = currentTime / 1000;
 
     if (!escTelemetryEnabled) {
         return;
@@ -215,7 +215,7 @@ void escTelemetryProcess(uint32_t currentTime)
         // Ready for starting requesting telemetry
         escTelemetryTriggerState = ESC_TLM_TRIGGER_READY;
         escTelemetryMotor = 0;
-        escTriggerTimestamp = millis();
+        escTriggerTimestamp = currentTimeMs;
     }
     else if (escTelemetryTriggerState == ESC_TLM_TRIGGER_READY)
     {
@@ -226,7 +226,7 @@ void escTelemetryProcess(uint32_t currentTime)
         escTelemetryTriggerState = ESC_TLM_TRIGGER_PENDING;
     }
 
-    if (escTriggerTimestamp + ESC_REQUEST_TIMEOUT < millis())
+    if (escTriggerTimestamp + ESC_REQUEST_TIMEOUT < currentTimeMs)
     {
         // ESC did not repond in time, skip to next motor
         escTelemetryData[escTelemetryMotor].skipped = true;
@@ -244,10 +244,10 @@ void escTelemetryProcess(uint32_t currentTime)
         // Wait until all ESCs are processed
         if (firstCycleComplete)
         {
-            uint8_t i;
+            int i;
             escCurrent = 0;
             escConsumption = 0;
-            for (i = 0; i < 4; i++)             // Motor count for Dshot limited to 4
+            for (i = 0; i < MAX_DSHOT_MOTORS; i++)             // Motor count for Dshot limited to 4
             {
                 if (!escTelemetryData[i].skipped)
                 {
@@ -269,7 +269,7 @@ void escTelemetryProcess(uint32_t currentTime)
 static void selectNextMotor(void)
 {
     escTelemetryMotor++;
-    if (escTelemetryMotor >= 4) {           // Motor count for Dshot limited to 4
+    if (escTelemetryMotor >= MAX_DSHOT_MOTORS) {           // Motor count for Dshot limited to 4
         escTelemetryMotor = 0;
         firstCycleComplete = true;
     }
@@ -280,19 +280,17 @@ static void selectNextMotor(void)
 
 static uint8_t update_crc8(uint8_t crc, uint8_t crc_seed)
 {
-    uint8_t crc_u, i;
+    uint8_t crc_u = crc;
-
-    crc_u = crc;
     crc_u ^= crc_seed;
 
-    for ( i=0; i<8; i++) crc_u = ( crc_u & 0x80 ) ? 0x7 ^ ( crc_u << 1 ) : ( crc_u << 1 );
+    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, i;
+    uint8_t crc = 0;
-    for( i=0; i<BufLen; i++) crc = update_crc8(Buf[i], crc);
+    for(int i=0; i<BufLen; i++) crc = update_crc8(Buf[i], crc);
     return (crc);
 }

src/main/telemetry/esc_telemetry.h

@@ -3,8 +3,8 @@
 uint8_t escTelemetryFrameStatus(void);
 bool escTelemetryInit(void);
 bool isEscTelemetryActive(void);
-uint16_t getEscTelemetryVbat(void);
+int16_t getEscTelemetryVbat(void);
-uint16_t getEscTelemetryCurrent(void);
+int16_t getEscTelemetryCurrent(void);
-uint16_t getEscTelemetryConsumption(void);
+int16_t getEscTelemetryConsumption(void);
 
 void escTelemetryProcess(uint32_t currentTime);