From b53cbbefc3c71e802487ca4d3deecf872c7f5d87 Mon Sep 17 00:00:00 2001 From: Bas Delfos Date: Wed, 23 Nov 2016 21:58:29 +0100 Subject: [PATCH] Moved ESC telemetry to sensors --- Makefile | 2 +- src/main/build/debug.h | 2 +- src/main/fc/config.h | 2 +- src/main/fc/fc_tasks.c | 29 ++-- src/main/io/serial_cli.c | 4 +- src/main/main.c | 8 +- src/main/scheduler/scheduler.h | 4 +- src/main/sensors/battery.c | 33 ++-- .../esc_telemetry.c => sensors/esc_sensor.c} | 142 +++++++++--------- src/main/sensors/esc_sensor.h | 10 ++ src/main/target/ANYFCF7/target.h | 2 +- src/main/target/BETAFLIGHTF3/target.h | 3 +- src/main/target/BLUEJAYF4/target.h | 3 +- src/main/target/DOGE/target.h | 3 +- src/main/target/FURYF3/target.h | 3 +- src/main/target/FURYF4/target.h | 3 +- src/main/target/FURYF7/target.h | 2 +- src/main/target/IMPULSERCF3/target.h | 3 +- src/main/target/KISSFC/target.h | 3 +- src/main/target/LUX_RACE/target.h | 3 +- src/main/target/MOTOLAB/target.h | 3 +- src/main/target/MULTIFLITEPICO/target.h | 3 +- src/main/target/OMNIBUS/target.h | 3 +- src/main/target/OMNIBUSF4/target.h | 3 +- src/main/target/REVO/target.h | 3 +- src/main/target/SPARKY2/target.h | 3 +- src/main/target/SPRACINGF3/target.h | 3 +- src/main/target/SPRACINGF3EVO/target.h | 3 +- src/main/target/STM32F3DISCOVERY/target.h | 3 +- src/main/target/X_RACERSPI/target.h | 3 +- src/main/telemetry/esc_telemetry.h | 12 -- src/main/telemetry/frsky.c | 2 +- src/main/telemetry/smartport.c | 4 +- 33 files changed, 154 insertions(+), 158 deletions(-) rename src/main/{telemetry/esc_telemetry.c => sensors/esc_sensor.c} (54%) create mode 100644 src/main/sensors/esc_sensor.h delete mode 100644 src/main/telemetry/esc_telemetry.h diff --git a/Makefile b/Makefile index f2edc40d5..58d269cca 100644 --- a/Makefile +++ b/Makefile @@ -593,7 +593,7 @@ HIGHEND_SRC = \ telemetry/smartport.c \ telemetry/ltm.c \ telemetry/mavlink.c \ - telemetry/esc_telemetry.c \ + sensors/esc_sensor.c \ ifeq ($(TARGET),$(filter $(TARGET),$(F4_TARGETS))) VCP_SRC = \ diff --git a/src/main/build/debug.h b/src/main/build/debug.h index 6b2e7ca9f..cd6fc1d78 100644 --- a/src/main/build/debug.h +++ b/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 diff --git a/src/main/fc/config.h b/src/main/fc/config.h index c5fe4ecf6..0fe4f9e61 100644 --- a/src/main/fc/config.h +++ b/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); diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index f043290f6..c795c18d8 100644 --- a/src/main/fc/fc_tasks.c +++ b/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" @@ -111,7 +111,7 @@ static void taskUpdateBattery(timeUs_t currentTimeUs) { #ifdef USE_ADC 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 - setTaskEnabled(TASK_ESC_TELEMETRY, feature(FEATURE_ESC_TELEMETRY)); +#ifdef USE_ESC_SENSOR + 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 - [TASK_ESC_TELEMETRY] = { - .taskName = "ESC_TELEMETRY", - .taskFunc = taskEscTelemetry, - .desiredPeriod = 1000000 / 100, // 100 Hz +#ifdef USE_ESC_SENSOR + [TASK_ESC_SENSOR] = { + .taskName = "ESC_SENSOR", + .taskFunc = escSensorProcess, + .desiredPeriod = TASK_PERIOD_HZ(100), // 100 Hz every 10ms .staticPriority = TASK_PRIORITY_LOW, }, #endif diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 159df98af..25eaecb8f 100755 --- a/src/main/io/serial_cli.c +++ b/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 @@ -519,7 +519,7 @@ static const char * const lookupTableDebug[DEBUG_COUNT] = { "VELOCITY", "DFILTER", "ANGLERATE", - "ESC_TELEMETRY", + "ESC_SENSOR", "SCHEDULER", }; diff --git a/src/main/main.c b/src/main/main.c index cf1060a54..d87052746 100644 --- a/src/main/main.c +++ b/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 - if (feature(FEATURE_ESC_TELEMETRY)) { - escTelemetryInit(); +#ifdef USE_ESC_SENSOR + if (feature(FEATURE_ESC_SENSOR)) { + escSensorInit(); } #endif diff --git a/src/main/scheduler/scheduler.h b/src/main/scheduler/scheduler.h index 36f5559ef..133943a3f 100644 --- a/src/main/scheduler/scheduler.h +++ b/src/main/scheduler/scheduler.h @@ -94,8 +94,8 @@ typedef enum { #ifdef USE_BST TASK_BST_MASTER_PROCESS, #endif -#ifdef USE_ESC_TELEMETRY - TASK_ESC_TELEMETRY, +#ifdef USE_ESC_SENSOR + TASK_ESC_SENSOR, #endif #ifdef CMS TASK_CMS, diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c index 3a8a59a8a..a240570fc 100644 --- a/src/main/sensors/battery.c +++ b/src/main/sensors/battery.c @@ -34,8 +34,7 @@ #include "config/feature.h" #include "sensors/battery.h" - -#include "telemetry/esc_telemetry.h" +#include "sensors/esc_sensor.h" #include "fc/rc_controls.h" #include "io/beeper.h" @@ -84,9 +83,9 @@ static void updateBatteryVoltage(void) vBatFilterIsInitialised = true; } - #ifdef USE_ESC_TELEMETRY - if (batteryConfig->batteryMeterType == BATTERY_SENSOR_ESC && isEscTelemetryActive()) { - vbatLatest = getEscTelemetryVbat(); + #ifdef USE_ESC_SENSOR + if (feature(FEATURE_ESC_SENSOR) && batteryConfig->batteryMeterType == BATTERY_SENSOR_ESC) { + vbatLatest = getEscSensorVbat(); if (debugMode == DEBUG_BATTERY) { debug[0] = -1; } @@ -270,11 +269,9 @@ void updateCurrentMeter(int32_t lastUpdateAt, rxConfig_t *rxConfig, uint16_t dea updateCurrentDrawn(lastUpdateAt); - updateConsumptionWarning(); - break; case CURRENT_SENSOR_VIRTUAL: - amperageLatest = (int32_t)batteryConfig->currentMeterOffset; + amperage = (int32_t)batteryConfig->currentMeterOffset; if (ARMING_FLAG(ARMED)) { throttleStatus_e throttleStatus = calculateThrottleStatus(rxConfig, deadband3d_throttle); int throttleOffset = (int32_t)rcCommand[THROTTLE] - 1000; @@ -282,30 +279,24 @@ void updateCurrentMeter(int32_t lastUpdateAt, rxConfig_t *rxConfig, uint16_t dea throttleOffset = 0; } int throttleFactor = throttleOffset + (throttleOffset * throttleOffset / 50); - amperageLatest += throttleFactor * (int32_t)batteryConfig->currentMeterScale / 1000; + amperage += throttleFactor * (int32_t)batteryConfig->currentMeterScale / 1000; } - amperage = amperageLatest; updateCurrentDrawn(lastUpdateAt); - updateConsumptionWarning(); - break; case CURRENT_SENSOR_ESC: - #ifdef USE_ESC_TELEMETRY - if (isEscTelemetryActive()) { - amperageLatest = getEscTelemetryCurrent(); - amperage = amperageLatest; - mAhDrawn = getEscTelemetryConsumption(); + #ifdef USE_ESC_SENSOR + if (feature(FEATURE_ESC_SENSOR)) + { + amperage = getEscSensorCurrent(); + mAhDrawn = getEscSensorConsumption(); - updateConsumptionWarning(); + updateCurrentDrawn(lastUpdateAt); } - - break; #endif case CURRENT_SENSOR_NONE: amperage = 0; - amperageLatest = 0; break; } diff --git a/src/main/telemetry/esc_telemetry.c b/src/main/sensors/esc_sensor.c similarity index 54% rename from src/main/telemetry/esc_telemetry.c rename to src/main/sensors/esc_sensor.c index 0bde741ab..0e2e562ce 100644 --- a/src/main/telemetry/esc_telemetry.c +++ b/src/main/sensors/esc_sensor.c @@ -21,7 +21,7 @@ #include "sensors/battery.h" -#include "esc_telemetry.h" +#include "esc_sensor.h" #include "build/debug.h" @@ -68,64 +68,64 @@ typedef struct { } esc_telemetry_t; typedef enum { - ESC_TLM_FRAME_PENDING = 1 << 0, // 1 - ESC_TLM_FRAME_COMPLETE = 1 << 1 // 2 + ESC_SENSOR_FRAME_PENDING = 1 << 0, // 1 + ESC_SENSOR_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; + ESC_SENSOR_TRIGGER_WAIT = 0, + ESC_SENSOR_TRIGGER_READY = 1 << 0, // 1 + ESC_SENSOR_TRIGGER_PENDING = 1 << 1, // 2 +} escSensorTriggerState_t; -#define ESC_TLM_BAUDRATE 115200 -#define ESC_TLM_BUFFSIZE 10 +#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_TLM_BUFFSIZE] = { 0, }; +static uint8_t tlm[ESC_SENSOR_BUFFSIZE] = { 0, }; static uint8_t tlmFramePosition = 0; -static serialPort_t *escTelemetryPort = NULL; -static esc_telemetry_t escTelemetryData[MAX_SUPPORTED_MOTORS]; +static serialPort_t *escSensorPort = NULL; +static esc_telemetry_t escSensorData[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 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 escTelemetryDataReceive(uint16_t c); +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 isEscTelemetryActive(void) +bool isEscSensorActive(void) { - return escTelemetryEnabled; + return escSensorEnabled; } -int16_t getEscTelemetryVbat(void) +int16_t getEscSensorVbat(void) { return escVbat / 10; } -int16_t getEscTelemetryCurrent(void) +int16_t getEscSensorCurrent(void) { return escCurrent; } -int16_t getEscTelemetryConsumption(void) +int16_t getEscSensorConsumption(void) { return escConsumption; } -bool escTelemetryInit(void) +bool escSensorInit(void) { serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_TELEMETRY_ESC); if (!portConfig) { @@ -135,35 +135,33 @@ bool escTelemetryInit(void) portOptions_t options = (SERIAL_NOT_INVERTED); // Initialize serial port - escTelemetryPort = openSerialPort(portConfig->identifier, FUNCTION_TELEMETRY_ESC, escTelemetryDataReceive, ESC_TLM_BAUDRATE, MODE_RX, options); + escSensorPort = openSerialPort(portConfig->identifier, FUNCTION_TELEMETRY_ESC, escSensorDataReceive, ESC_SENSOR_BAUDRATE, MODE_RX, options); - if (escTelemetryPort) { - escTelemetryEnabled = true; - batteryConfig()->currentMeterType = CURRENT_SENSOR_ESC; - batteryConfig()->batteryMeterType = BATTERY_SENSOR_ESC; + if (escSensorPort) { + escSensorEnabled = true; } - return escTelemetryPort != NULL; + return escSensorPort != NULL; } -void freeEscTelemetryPort(void) +void freeEscSensorPort(void) { - closeSerialPort(escTelemetryPort); - escTelemetryPort = NULL; - escTelemetryEnabled = false; + closeSerialPort(escSensorPort); + escSensorPort = NULL; + escSensorEnabled = false; } // Receive ISR callback -static void escTelemetryDataReceive(uint16_t c) +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 (escTelemetryTriggerState == ESC_TLM_TRIGGER_WAIT) return; + if (escSensorTriggerState == ESC_SENSOR_TRIGGER_WAIT) return; tlm[tlmFramePosition] = (uint8_t)c; - if (tlmFramePosition == ESC_TLM_BUFFSIZE - 1) { + if (tlmFramePosition == ESC_SENSOR_BUFFSIZE - 1) { tlmFrameDone = true; tlmFramePosition = 0; } else { @@ -171,9 +169,9 @@ static void escTelemetryDataReceive(uint16_t c) } } -uint8_t escTelemetryFrameStatus(void) +uint8_t escSensorFrameStatus(void) { - uint8_t frameStatus = ESC_TLM_FRAME_PENDING; + uint8_t frameStatus = ESC_SENSOR_FRAME_PENDING; uint16_t chksum, tlmsum; if (!tlmFrameDone) { @@ -183,28 +181,28 @@ uint8_t escTelemetryFrameStatus(void) tlmFrameDone = false; // Get CRC8 checksum - chksum = get_crc8(tlm, ESC_TLM_BUFFSIZE - 1); - tlmsum = tlm[ESC_TLM_BUFFSIZE - 1]; // last byte contains CRC value + chksum = get_crc8(tlm, ESC_SENSOR_BUFFSIZE - 1); + tlmsum = tlm[ESC_SENSOR_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]; + 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_TLM_FRAME_COMPLETE; + frameStatus = ESC_SENSOR_FRAME_COMPLETE; } return frameStatus; } -void escTelemetryProcess(timeUs_t currentTimeUs) +void escSensorProcess(timeUs_t currentTimeUs) { const timeMs_t currentTimeMs = currentTimeUs / 1000; - if (!escTelemetryEnabled) { + if (!escSensorEnabled) { return; } @@ -212,66 +210,66 @@ void escTelemetryProcess(timeUs_t currentTimeUs) if (millis() < ESC_BOOTTIME) { return; } - else if (escTelemetryTriggerState == ESC_TLM_TRIGGER_WAIT) { + else if (escSensorTriggerState == ESC_SENSOR_TRIGGER_WAIT) { // Ready for starting requesting telemetry - escTelemetryTriggerState = ESC_TLM_TRIGGER_READY; - escTelemetryMotor = 0; + escSensorTriggerState = ESC_SENSOR_TRIGGER_READY; + escSensorMotor = 0; escTriggerTimestamp = currentTimeMs; escTriggerLastTimestamp = escTriggerTimestamp; } - else if (escTelemetryTriggerState == ESC_TLM_TRIGGER_READY) { - if (debugMode == DEBUG_ESC_TELEMETRY) debug[0] = escTelemetryMotor+1; + else if (escSensorTriggerState == ESC_SENSOR_TRIGGER_READY) { + if (debugMode == DEBUG_ESC_SENSOR) debug[0] = escSensorMotor+1; - motorDmaOutput_t * const motor = getMotorDmaOutput(escTelemetryMotor); + motorDmaOutput_t * const motor = getMotorDmaOutput(escSensorMotor); motor->requestTelemetry = true; - escTelemetryTriggerState = ESC_TLM_TRIGGER_PENDING; + escSensorTriggerState = ESC_SENSOR_TRIGGER_PENDING; } if (escTriggerTimestamp + ESC_REQUEST_TIMEOUT < currentTimeMs) { // ESC did not repond in time, retry timeoutRetryCount++; escTriggerTimestamp = currentTimeMs; - escTelemetryTriggerState = ESC_TLM_TRIGGER_READY; + escSensorTriggerState = ESC_SENSOR_TRIGGER_READY; if (timeoutRetryCount == 4) { // Not responding after 3 times, skip motor - escTelemetryData[escTelemetryMotor].skipped = true; + escSensorData[escSensorMotor].skipped = true; selectNextMotor(); } - if (debugMode == DEBUG_ESC_TELEMETRY) debug[1]++; + if (debugMode == DEBUG_ESC_SENSOR) debug[1]++; } // Get received frame status - uint8_t state = escTelemetryFrameStatus(); + uint8_t state = escSensorFrameStatus(); - if (state == ESC_TLM_FRAME_COMPLETE) { + if (state == ESC_SENSOR_FRAME_COMPLETE) { // Wait until all ESCs are processed - if (escTelemetryMotor == getMotorCount()-1) { + if (escSensorMotor == 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 (!escSensorData[i].skipped) { + escVbat = i > 0 ? ((escVbat + escSensorData[i].voltage) / 2) : escSensorData[i].voltage; + escCurrent = escCurrent + escSensorData[i].current; + escConsumption = escConsumption + escSensorData[i].consumption; } } } - if (debugMode == DEBUG_ESC_TELEMETRY) debug[2] = escVbat; - if (debugMode == DEBUG_ESC_TELEMETRY) debug[3] = escCurrent; + if (debugMode == DEBUG_ESC_SENSOR) debug[2] = escVbat; + if (debugMode == DEBUG_ESC_SENSOR) debug[3] = escCurrent; selectNextMotor(); - escTelemetryTriggerState = ESC_TLM_TRIGGER_READY; + escSensorTriggerState = ESC_SENSOR_TRIGGER_READY; } if (escTriggerLastTimestamp + 10000 < currentTimeMs) { // ESCs did not respond for 10 seconds // Disable ESC telemetry and fallback to onboard vbat sensor - freeEscTelemetryPort(); + freeEscSensorPort(); escVbat = 0; escCurrent = 0; escConsumption = 0; @@ -280,9 +278,9 @@ void escTelemetryProcess(timeUs_t currentTimeUs) static void selectNextMotor(void) { - escTelemetryMotor++; - if (escTelemetryMotor == getMotorCount()) { - escTelemetryMotor = 0; + escSensorMotor++; + if (escSensorMotor == getMotorCount()) { + escSensorMotor = 0; } escTriggerTimestamp = millis(); escTriggerLastTimestamp = escTriggerTimestamp; diff --git a/src/main/sensors/esc_sensor.h b/src/main/sensors/esc_sensor.h new file mode 100644 index 000000000..4ea61ee13 --- /dev/null +++ b/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); diff --git a/src/main/target/ANYFCF7/target.h b/src/main/target/ANYFCF7/target.h index bea085ae9..007488d4f 100644 --- a/src/main/target/ANYFCF7/target.h +++ b/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 diff --git a/src/main/target/BETAFLIGHTF3/target.h b/src/main/target/BETAFLIGHTF3/target.h index f22a23573..bfd47caef 100755 --- a/src/main/target/BETAFLIGHTF3/target.h +++ b/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 diff --git a/src/main/target/BLUEJAYF4/target.h b/src/main/target/BLUEJAYF4/target.h index ad588a23a..5ab4925f4 100644 --- a/src/main/target/BLUEJAYF4/target.h +++ b/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 diff --git a/src/main/target/DOGE/target.h b/src/main/target/DOGE/target.h index 5b360ed8d..1f10fcb0f 100644 --- a/src/main/target/DOGE/target.h +++ b/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 diff --git a/src/main/target/FURYF3/target.h b/src/main/target/FURYF3/target.h index a858e9732..5497f9021 100644 --- a/src/main/target/FURYF3/target.h +++ b/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 diff --git a/src/main/target/FURYF4/target.h b/src/main/target/FURYF4/target.h index 790d3096d..4d35a98fb 100644 --- a/src/main/target/FURYF4/target.h +++ b/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 diff --git a/src/main/target/FURYF7/target.h b/src/main/target/FURYF7/target.h index 35410dff5..2cbffeaed 100644 --- a/src/main/target/FURYF7/target.h +++ b/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 diff --git a/src/main/target/IMPULSERCF3/target.h b/src/main/target/IMPULSERCF3/target.h index 9689883a5..79382b81c 100644 --- a/src/main/target/IMPULSERCF3/target.h +++ b/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 diff --git a/src/main/target/KISSFC/target.h b/src/main/target/KISSFC/target.h index 2551072af..b7a523d9c 100644 --- a/src/main/target/KISSFC/target.h +++ b/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 diff --git a/src/main/target/LUX_RACE/target.h b/src/main/target/LUX_RACE/target.h index fa6eb8a9f..a6ca94cd7 100644 --- a/src/main/target/LUX_RACE/target.h +++ b/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 diff --git a/src/main/target/MOTOLAB/target.h b/src/main/target/MOTOLAB/target.h index ef18dbd7c..52a6bebef 100644 --- a/src/main/target/MOTOLAB/target.h +++ b/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 diff --git a/src/main/target/MULTIFLITEPICO/target.h b/src/main/target/MULTIFLITEPICO/target.h index e56633eaf..7e5a0f045 100755 --- a/src/main/target/MULTIFLITEPICO/target.h +++ b/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 diff --git a/src/main/target/OMNIBUS/target.h b/src/main/target/OMNIBUS/target.h index 166a57035..560858c50 100644 --- a/src/main/target/OMNIBUS/target.h +++ b/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 diff --git a/src/main/target/OMNIBUSF4/target.h b/src/main/target/OMNIBUSF4/target.h index 597091dbc..d83800187 100644 --- a/src/main/target/OMNIBUSF4/target.h +++ b/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 diff --git a/src/main/target/REVO/target.h b/src/main/target/REVO/target.h index 710590d1d..b01bc2466 100644 --- a/src/main/target/REVO/target.h +++ b/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 diff --git a/src/main/target/SPARKY2/target.h b/src/main/target/SPARKY2/target.h index 5a33df421..f404c6ac4 100644 --- a/src/main/target/SPARKY2/target.h +++ b/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 diff --git a/src/main/target/SPRACINGF3/target.h b/src/main/target/SPRACINGF3/target.h index 3d906295e..40e65a5c5 100644 --- a/src/main/target/SPRACINGF3/target.h +++ b/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 diff --git a/src/main/target/SPRACINGF3EVO/target.h b/src/main/target/SPRACINGF3EVO/target.h index ce7e3b368..eb2d21437 100755 --- a/src/main/target/SPRACINGF3EVO/target.h +++ b/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 diff --git a/src/main/target/STM32F3DISCOVERY/target.h b/src/main/target/STM32F3DISCOVERY/target.h index a43b50faf..1e7670eb3 100644 --- a/src/main/target/STM32F3DISCOVERY/target.h +++ b/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 diff --git a/src/main/target/X_RACERSPI/target.h b/src/main/target/X_RACERSPI/target.h index 8ade29012..f530e660f 100644 --- a/src/main/target/X_RACERSPI/target.h +++ b/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 diff --git a/src/main/telemetry/esc_telemetry.h b/src/main/telemetry/esc_telemetry.h deleted file mode 100644 index ca9c2a227..000000000 --- a/src/main/telemetry/esc_telemetry.h +++ /dev/null @@ -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); diff --git a/src/main/telemetry/frsky.c b/src/main/telemetry/frsky.c index c2a5c0e5d..5f1700bea 100644 --- a/src/main/telemetry/frsky.c +++ b/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(); diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c index 496013f14..4d1d19718 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) || 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; }