diff --git a/Makefile b/Makefile index 8598c43e6f..74d2188e1d 100644 --- a/Makefile +++ b/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 = \ diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 7038c7ce20..b0dfce3fd5 100644 --- a/src/main/blackbox/blackbox.c +++ b/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->amperageLatest = amperageLatestADC; + blackboxCurrent->vbatLatest = vbatLatest; + blackboxCurrent->amperageLatest = amperageLatest; #ifdef MAG for (i = 0; i < XYZ_AXIS_COUNT; i++) { @@ -1625,4 +1625,3 @@ void initBlackbox(void) } } #endif - diff --git a/src/main/build/debug.h b/src/main/build/debug.h index 408aece237..69644af5d9 100644 --- a/src/main/build/debug.h +++ b/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; diff --git a/src/main/drivers/pwm_output.h b/src/main/drivers/pwm_output.h index 02187844be..9d94a1d074 100644 --- a/src/main/drivers/pwm_output.h +++ b/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); - diff --git a/src/main/drivers/pwm_output_stm32f3xx.c b/src/main/drivers/pwm_output_stm32f3xx.c index d3bfa84984..5adf8f63b0 100644 --- a/src/main/drivers/pwm_output_stm32f3xx.c +++ b/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; diff --git a/src/main/drivers/pwm_output_stm32f4xx.c b/src/main/drivers/pwm_output_stm32f4xx.c index 2821bb1613..0773efe08b 100644 --- a/src/main/drivers/pwm_output_stm32f4xx.c +++ b/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; diff --git a/src/main/drivers/pwm_output_stm32f7xx.c b/src/main/drivers/pwm_output_stm32f7xx.c index b0d6b2367b..32ce620547 100644 --- a/src/main/drivers/pwm_output_stm32f7xx.c +++ b/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; diff --git a/src/main/fc/config.c b/src/main/fc/config.c index 19aa43ddaa..ac6a573ad1 100755 --- a/src/main/fc/config.c +++ b/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; diff --git a/src/main/fc/config.h b/src/main/fc/config.h index ff668a75d9..6f903e5d83 100644 --- a/src/main/fc/config.h +++ b/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); diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index 2b26675277..c3275fdae6 100644 --- a/src/main/fc/fc_tasks.c +++ b/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", diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index d80d010e46..986f48210a 100755 --- a/src/main/flight/mixer.c +++ b/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) diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index b329128b80..b0c530df39 100644 --- a/src/main/flight/mixer.h +++ b/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, diff --git a/src/main/io/serial.h b/src/main/io/serial.h index e2c1931efc..99027830d8 100644 --- a/src/main/io/serial.h +++ b/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 { diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 0609fddbb5..1515e3c285 100755 --- a/src/main/io/serial_cli.c +++ b/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 diff --git a/src/main/main.c b/src/main/main.c index a3cdbe2162..ef2f2aa353 100644 --- a/src/main/main.c +++ b/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 diff --git a/src/main/scheduler/scheduler.h b/src/main/scheduler/scheduler.h index d43b778aa5..026cb88af8 100644 --- a/src/main/scheduler/scheduler.h +++ b/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 diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c index b83159ebfc..302789e973 100644 --- a/src/main/sensors/battery.c +++ b/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 @@ -47,9 +51,9 @@ uint8_t batteryCellCount; 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 amperageLatestADC = 0; // most recent raw reading from current ADC +uint16_t vbat = 0; // battery voltage in 0.1V steps (filtered) +uint16_t vbatLatest = 0; // most recent unsmoothed value +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 = batteryAdcToVoltage(vbatSample); + vbat = biquadFilterApply(&vbatFilter, 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)); - amperage = currentSensorToCentiamps(amperageRaw / 8); + amperageRaw += adcGetChannel(ADC_CURRENT); + 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) { diff --git a/src/main/sensors/battery.h b/src/main/sensors/battery.h index 81867e0c18..cac334f850 100644 --- a/src/main/sensors/battery.h +++ b/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; diff --git a/src/main/target/ANYFCF7/target.h b/src/main/target/ANYFCF7/target.h index f1c5546a53..bea085ae93 100644 --- a/src/main/target/ANYFCF7/target.h +++ b/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 diff --git a/src/main/target/BETAFLIGHTF3/target.h b/src/main/target/BETAFLIGHTF3/target.h index 7c911b3669..cb581a2f78 100755 --- a/src/main/target/BETAFLIGHTF3/target.h +++ b/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 @@ -137,4 +138,4 @@ #define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) #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(4) | TIM_N(8) | TIM_N(15) | TIM_N(16) | TIM_N(17) ) \ No newline at end of file +#define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(8) | TIM_N(15) | TIM_N(16) | TIM_N(17) ) diff --git a/src/main/target/BLUEJAYF4/target.h b/src/main/target/BLUEJAYF4/target.h index 7244af85a7..921861a11c 100644 --- a/src/main/target/BLUEJAYF4/target.h +++ b/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)) - diff --git a/src/main/target/DOGE/target.h b/src/main/target/DOGE/target.h index fd2ea81494..ed75d987e8 100644 --- a/src/main/target/DOGE/target.h +++ b/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)) - diff --git a/src/main/target/FURYF3/target.h b/src/main/target/FURYF3/target.h index 52c673af86..696f373c10 100644 --- a/src/main/target/FURYF3/target.h +++ b/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)) - diff --git a/src/main/target/FURYF4/target.h b/src/main/target/FURYF4/target.h index 741866e772..1317168a70 100644 --- a/src/main/target/FURYF4/target.h +++ b/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) ) - diff --git a/src/main/target/FURYF7/target.h b/src/main/target/FURYF7/target.h index ae25088955..35410dff5c 100644 --- a/src/main/target/FURYF7/target.h +++ b/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 @@ -152,4 +153,4 @@ #define TARGET_IO_PORTE 0xffff #define USABLE_TIMER_CHANNEL_COUNT 5 -#define USED_TIMERS ( TIM_N(2) | TIM_N(3) | TIM_N(8)) \ No newline at end of file +#define USED_TIMERS ( TIM_N(2) | TIM_N(3) | TIM_N(8)) diff --git a/src/main/target/IMPULSERCF3/target.h b/src/main/target/IMPULSERCF3/target.h index b28658ebd9..ed5475b30e 100644 --- a/src/main/target/IMPULSERCF3/target.h +++ b/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)) - diff --git a/src/main/target/KISSFC/target.h b/src/main/target/KISSFC/target.h index 1b69a1146a..07dbd7b6e5 100644 --- a/src/main/target/KISSFC/target.h +++ b/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 diff --git a/src/main/target/LUX_RACE/target.h b/src/main/target/LUX_RACE/target.h index 4a3f30cbc7..2fe4428e82 100644 --- a/src/main/target/LUX_RACE/target.h +++ b/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)) - diff --git a/src/main/target/MOTOLAB/target.h b/src/main/target/MOTOLAB/target.h index a2739251a7..295bab37da 100644 --- a/src/main/target/MOTOLAB/target.h +++ b/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)) - diff --git a/src/main/target/MULTIFLITEPICO/target.h b/src/main/target/MULTIFLITEPICO/target.h index 884d847dbd..aebdde196f 100755 --- a/src/main/target/MULTIFLITEPICO/target.h +++ b/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 diff --git a/src/main/target/OMNIBUS/target.h b/src/main/target/OMNIBUS/target.h index beadee7a0d..7b9c187dbe 100644 --- a/src/main/target/OMNIBUS/target.h +++ b/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 diff --git a/src/main/target/OMNIBUSF4/target.h b/src/main/target/OMNIBUSF4/target.h index 0fba1225a4..db70896b67 100644 --- a/src/main/target/OMNIBUSF4/target.h +++ b/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 diff --git a/src/main/target/REVO/target.h b/src/main/target/REVO/target.h index 341c73dfa6..773c381a42 100644 --- a/src/main/target/REVO/target.h +++ b/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 diff --git a/src/main/target/SOULF4/target.h b/src/main/target/SOULF4/target.h index 410841e747..db3d32eef9 100644 --- a/src/main/target/SOULF4/target.h +++ b/src/main/target/SOULF4/target.h @@ -105,6 +105,7 @@ #define LED_STRIP #define USE_DSHOT +#define USE_ESC_TELEMETRY #define SPEKTRUM_BIND // USART3, diff --git a/src/main/target/SPARKY2/target.h b/src/main/target/SPARKY2/target.h index db6e953bb0..8ca1f6b3bd 100644 --- a/src/main/target/SPARKY2/target.h +++ b/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)) - diff --git a/src/main/target/SPRACINGF3/target.h b/src/main/target/SPRACINGF3/target.h index 6c71d1072b..40da2a9c74 100644 --- a/src/main/target/SPRACINGF3/target.h +++ b/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 diff --git a/src/main/target/SPRACINGF3EVO/target.h b/src/main/target/SPRACINGF3EVO/target.h index 516d023dba..7162121190 100755 --- a/src/main/target/SPRACINGF3EVO/target.h +++ b/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)) - diff --git a/src/main/target/STM32F3DISCOVERY/target.h b/src/main/target/STM32F3DISCOVERY/target.h index b761aa215b..a5ddc38d02 100644 --- a/src/main/target/STM32F3DISCOVERY/target.h +++ b/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)) - diff --git a/src/main/target/X_RACERSPI/target.h b/src/main/target/X_RACERSPI/target.h index 1b812cc8ba..72406077d4 100644 --- a/src/main/target/X_RACERSPI/target.h +++ b/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) ) - diff --git a/src/main/telemetry/esc_telemetry.c b/src/main/telemetry/esc_telemetry.c new file mode 100644 index 0000000000..9d5cde0f1e --- /dev/null +++ b/src/main/telemetry/esc_telemetry.c @@ -0,0 +1,312 @@ +#include +#include +#include + +#include + +#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 0) { + if ((feature(FEATURE_VBAT) || feature(FEATURE_ESC_TELEMETRY)) && batteryCellCount > 0) { sendVoltage(); sendVoltageAmp(); sendAmperage(); diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c index 0f794ba835..48c81bb9e4 100644 --- a/src/main/telemetry/smartport.c +++ b/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; }