From aa1ae88ddf55a048c2106fa2840b659119c2a1e1 Mon Sep 17 00:00:00 2001 From: Bas Delfos Date: Wed, 23 Nov 2016 20:01:20 +0100 Subject: [PATCH 01/14] Gitignore .idea folder --- .gitignore | 1 + 1 file changed, 1 insertion(+) diff --git a/.gitignore b/.gitignore index 86b85dc44..67e2f3cbe 100644 --- a/.gitignore +++ b/.gitignore @@ -13,6 +13,7 @@ obj/ patches/ startup_stm32f10x_md_gcc.s +.idea # script-generated files docs/Manual.pdf From b53cbbefc3c71e802487ca4d3deecf872c7f5d87 Mon Sep 17 00:00:00 2001 From: Bas Delfos Date: Wed, 23 Nov 2016 21:58:29 +0100 Subject: [PATCH 02/14] 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; } From 1561481e521ae06ec4cda41b65024eb0c2fde6f1 Mon Sep 17 00:00:00 2001 From: Bas Delfos Date: Wed, 23 Nov 2016 22:44:22 +0100 Subject: [PATCH 03/14] Enable battery task when ESC sensor is available --- src/main/fc/fc_tasks.c | 2 +- src/main/sensors/battery.c | 2 ++ 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index c795c18d8..cb04a8f23 100644 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -109,7 +109,7 @@ static void taskHandleSerial(timeUs_t currentTimeUs) static void taskUpdateBattery(timeUs_t currentTimeUs) { -#ifdef USE_ADC +#if defined(USE_ADC) || defined(USE_ESC_SENSOR) static uint32_t vbatLastServiced = 0; if (feature(FEATURE_VBAT) || feature(FEATURE_ESC_SENSOR)) { if (cmp32(currentTimeUs, vbatLastServiced) >= VBATINTERVAL) { diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c index a240570fc..e9591610a 100644 --- a/src/main/sensors/battery.c +++ b/src/main/sensors/battery.c @@ -295,6 +295,8 @@ void updateCurrentMeter(int32_t lastUpdateAt, rxConfig_t *rxConfig, uint16_t dea updateCurrentDrawn(lastUpdateAt); } #endif + + break; case CURRENT_SENSOR_NONE: amperage = 0; From 036c7d0c8986b64dd196253ee3eff81ed0654ba3 Mon Sep 17 00:00:00 2001 From: Bas Delfos Date: Wed, 23 Nov 2016 23:51:57 +0100 Subject: [PATCH 04/14] Fixed review comments --- src/main/sensors/esc_sensor.c | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/src/main/sensors/esc_sensor.c b/src/main/sensors/esc_sensor.c index 0e2e562ce..ee0217507 100644 --- a/src/main/sensors/esc_sensor.c +++ b/src/main/sensors/esc_sensor.c @@ -91,6 +91,7 @@ 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 totalRetryCount = 0; static uint8_t escSensorMotor = 0; // motor index static bool escSensorEnabled = false; @@ -218,7 +219,7 @@ void escSensorProcess(timeUs_t currentTimeUs) escTriggerLastTimestamp = escTriggerTimestamp; } else if (escSensorTriggerState == ESC_SENSOR_TRIGGER_READY) { - if (debugMode == DEBUG_ESC_SENSOR) debug[0] = escSensorMotor+1; + DEBUG_SET(DEBUG_ESC_SENSOR, 0, escSensorMotor+1); motorDmaOutput_t * const motor = getMotorDmaOutput(escSensorMotor); motor->requestTelemetry = true; @@ -237,7 +238,7 @@ void escSensorProcess(timeUs_t currentTimeUs) selectNextMotor(); } - if (debugMode == DEBUG_ESC_SENSOR) debug[1]++; + DEBUG_SET(DEBUG_ESC_SENSOR, 1, ++totalRetryCount); } // Get received frame status @@ -259,8 +260,8 @@ void escSensorProcess(timeUs_t currentTimeUs) } } - if (debugMode == DEBUG_ESC_SENSOR) debug[2] = escVbat; - if (debugMode == DEBUG_ESC_SENSOR) debug[3] = escCurrent; + DEBUG_SET(DEBUG_ESC_SENSOR, 2, escVbat); + DEBUG_SET(DEBUG_ESC_SENSOR, 3, escCurrent); selectNextMotor(); escSensorTriggerState = ESC_SENSOR_TRIGGER_READY; From 7e26afddf13795d954a9e3e6e2fbf07a2e9e9ad8 Mon Sep 17 00:00:00 2001 From: Bas Delfos Date: Thu, 24 Nov 2016 20:49:23 +0100 Subject: [PATCH 05/14] Remove fallback on ADC sensor when ESC sensor fails --- src/main/sensors/battery.c | 7 +++++-- src/main/sensors/esc_sensor.c | 3 +-- 2 files changed, 6 insertions(+), 4 deletions(-) diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c index e9591610a..7f6b62a49 100644 --- a/src/main/sensors/battery.c +++ b/src/main/sensors/battery.c @@ -84,12 +84,17 @@ static void updateBatteryVoltage(void) } #ifdef USE_ESC_SENSOR +<<<<<<< 0a89e28c8e6e538d5b283fe9a389c481f24523fb if (feature(FEATURE_ESC_SENSOR) && batteryConfig->batteryMeterType == BATTERY_SENSOR_ESC) { vbatLatest = getEscSensorVbat(); if (debugMode == DEBUG_BATTERY) { debug[0] = -1; } vbat = biquadFilterApply(&vBatFilter, vbatLatest); +======= + if (batteryConfig->batteryMeterType == BATTERY_SENSOR_ESC) { + vbatSample = vbatLatest = getEscSensorVbat(); +>>>>>>> Remove fallback on ADC sensor when ESC sensor fails } else #endif @@ -291,8 +296,6 @@ void updateCurrentMeter(int32_t lastUpdateAt, rxConfig_t *rxConfig, uint16_t dea { amperage = getEscSensorCurrent(); mAhDrawn = getEscSensorConsumption(); - - updateCurrentDrawn(lastUpdateAt); } #endif diff --git a/src/main/sensors/esc_sensor.c b/src/main/sensors/esc_sensor.c index ee0217507..a6b13fdca 100644 --- a/src/main/sensors/esc_sensor.c +++ b/src/main/sensors/esc_sensor.c @@ -269,11 +269,10 @@ void escSensorProcess(timeUs_t currentTimeUs) if (escTriggerLastTimestamp + 10000 < currentTimeMs) { // ESCs did not respond for 10 seconds - // Disable ESC telemetry and fallback to onboard vbat sensor + // Disable ESC telemetry and reset voltage and current to let the use know something is wrong freeEscSensorPort(); escVbat = 0; escCurrent = 0; - escConsumption = 0; } } From ff2f2dae874b18313f0f0b3b4c1e294ee99fe815 Mon Sep 17 00:00:00 2001 From: Bas Delfos Date: Thu, 24 Nov 2016 21:00:46 +0100 Subject: [PATCH 06/14] Fixed Travis build warning --- src/main/sensors/battery.c | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c index 7f6b62a49..721b1b8d4 100644 --- a/src/main/sensors/battery.c +++ b/src/main/sensors/battery.c @@ -292,6 +292,7 @@ void updateCurrentMeter(int32_t lastUpdateAt, rxConfig_t *rxConfig, uint16_t dea break; case CURRENT_SENSOR_ESC: #ifdef USE_ESC_SENSOR +<<<<<<< 2af7ff8434cfe047c841c85aae300154c7b4065e if (feature(FEATURE_ESC_SENSOR)) { amperage = getEscSensorCurrent(); @@ -299,6 +300,12 @@ void updateCurrentMeter(int32_t lastUpdateAt, rxConfig_t *rxConfig, uint16_t dea } #endif +======= + if (batteryConfig->currentMeterType == CURRENT_SENSOR_ESC) { + amperage = getEscSensorCurrent(); + mAhDrawn = getEscSensorConsumption(); + } +>>>>>>> Fixed Travis build warning break; case CURRENT_SENSOR_NONE: amperage = 0; From 2ffe57761e7e59382e393f7603f474922326c123 Mon Sep 17 00:00:00 2001 From: Bas Delfos Date: Sun, 27 Nov 2016 11:16:38 +0100 Subject: [PATCH 07/14] Removed unnecessary enums for CURRENT_SENSOR_ESC and BATTERY_SENSOR_ESC --- src/main/sensors/battery.c | 12 ------------ src/main/sensors/battery.h | 3 ++- 2 files changed, 2 insertions(+), 13 deletions(-) diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c index 721b1b8d4..bf1b13941 100644 --- a/src/main/sensors/battery.c +++ b/src/main/sensors/battery.c @@ -84,17 +84,12 @@ static void updateBatteryVoltage(void) } #ifdef USE_ESC_SENSOR -<<<<<<< 0a89e28c8e6e538d5b283fe9a389c481f24523fb if (feature(FEATURE_ESC_SENSOR) && batteryConfig->batteryMeterType == BATTERY_SENSOR_ESC) { vbatLatest = getEscSensorVbat(); if (debugMode == DEBUG_BATTERY) { debug[0] = -1; } vbat = biquadFilterApply(&vBatFilter, vbatLatest); -======= - if (batteryConfig->batteryMeterType == BATTERY_SENSOR_ESC) { - vbatSample = vbatLatest = getEscSensorVbat(); ->>>>>>> Remove fallback on ADC sensor when ESC sensor fails } else #endif @@ -292,7 +287,6 @@ void updateCurrentMeter(int32_t lastUpdateAt, rxConfig_t *rxConfig, uint16_t dea break; case CURRENT_SENSOR_ESC: #ifdef USE_ESC_SENSOR -<<<<<<< 2af7ff8434cfe047c841c85aae300154c7b4065e if (feature(FEATURE_ESC_SENSOR)) { amperage = getEscSensorCurrent(); @@ -300,12 +294,6 @@ void updateCurrentMeter(int32_t lastUpdateAt, rxConfig_t *rxConfig, uint16_t dea } #endif -======= - if (batteryConfig->currentMeterType == CURRENT_SENSOR_ESC) { - amperage = getEscSensorCurrent(); - mAhDrawn = getEscSensorConsumption(); - } ->>>>>>> Fixed Travis build warning break; case CURRENT_SENSOR_NONE: amperage = 0; diff --git a/src/main/sensors/battery.h b/src/main/sensors/battery.h index 01309a8c8..8f0b1d65f 100644 --- a/src/main/sensors/battery.h +++ b/src/main/sensors/battery.h @@ -31,7 +31,8 @@ typedef enum { CURRENT_SENSOR_NONE = 0, CURRENT_SENSOR_ADC, CURRENT_SENSOR_VIRTUAL, - CURRENT_SENSOR_ESC + CURRENT_SENSOR_ESC, + CURRENT_SENSOR_MAX = CURRENT_SENSOR_ESC } currentSensor_e; typedef enum { From c1d442279f6f0847e97acb5e757e4da285a81611 Mon Sep 17 00:00:00 2001 From: Bas Delfos Date: Sun, 27 Nov 2016 12:27:25 +0100 Subject: [PATCH 08/14] Improved retry timeout check and 10 seconds of no response from ESCs check --- src/main/sensors/esc_sensor.c | 75 ++++++++++++++++++----------------- 1 file changed, 39 insertions(+), 36 deletions(-) diff --git a/src/main/sensors/esc_sensor.c b/src/main/sensors/esc_sensor.c index a6b13fdca..fc11fc89d 100644 --- a/src/main/sensors/esc_sensor.c +++ b/src/main/sensors/esc_sensor.c @@ -89,7 +89,7 @@ static uint8_t tlmFramePosition = 0; static serialPort_t *escSensorPort = NULL; static esc_telemetry_t escSensorData[MAX_SUPPORTED_MOTORS]; static uint32_t escTriggerTimestamp = -1; -static uint32_t escTriggerLastTimestamp = -1; +static uint32_t escLastResponseTimestamp; static uint8_t timeoutRetryCount = 0; static uint8_t totalRetryCount = 0; @@ -208,7 +208,7 @@ void escSensorProcess(timeUs_t currentTimeUs) } // Wait period of time before requesting telemetry (let the system boot first) - if (millis() < ESC_BOOTTIME) { + if (currentTimeMs < ESC_BOOTTIME) { return; } else if (escSensorTriggerState == ESC_SENSOR_TRIGGER_WAIT) { @@ -216,7 +216,7 @@ void escSensorProcess(timeUs_t currentTimeUs) escSensorTriggerState = ESC_SENSOR_TRIGGER_READY; escSensorMotor = 0; escTriggerTimestamp = currentTimeMs; - escTriggerLastTimestamp = escTriggerTimestamp; + escLastResponseTimestamp = escTriggerTimestamp; } else if (escSensorTriggerState == ESC_SENSOR_TRIGGER_READY) { DEBUG_SET(DEBUG_ESC_SENSOR, 0, escSensorMotor+1); @@ -225,49 +225,52 @@ void escSensorProcess(timeUs_t currentTimeUs) motor->requestTelemetry = true; escSensorTriggerState = ESC_SENSOR_TRIGGER_PENDING; } + else if (escSensorTriggerState == ESC_SENSOR_TRIGGER_PENDING) { - if (escTriggerTimestamp + ESC_REQUEST_TIMEOUT < currentTimeMs) { - // ESC did not repond in time, retry - timeoutRetryCount++; - escTriggerTimestamp = currentTimeMs; - escSensorTriggerState = ESC_SENSOR_TRIGGER_READY; + if (escTriggerTimestamp + ESC_REQUEST_TIMEOUT < currentTimeMs) { + // ESC did not repond in time, retry + timeoutRetryCount++; + escTriggerTimestamp = currentTimeMs; + escSensorTriggerState = ESC_SENSOR_TRIGGER_READY; - if (timeoutRetryCount == 4) { - // Not responding after 3 times, skip motor - escSensorData[escSensorMotor].skipped = true; - selectNextMotor(); + if (timeoutRetryCount == 4) { + // Not responding after 3 times, skip motor + escSensorData[escSensorMotor].skipped = true; + selectNextMotor(); + } + + DEBUG_SET(DEBUG_ESC_SENSOR, 1, ++totalRetryCount); } - DEBUG_SET(DEBUG_ESC_SENSOR, 1, ++totalRetryCount); - } + // Get received frame status + uint8_t state = escSensorFrameStatus(); - // Get received frame status - uint8_t state = escSensorFrameStatus(); + if (state == ESC_SENSOR_FRAME_COMPLETE) { + // Wait until all ESCs are processed + if (escSensorMotor == getMotorCount()-1) { + escCurrent = 0; + escConsumption = 0; + escVbat = 0; - if (state == ESC_SENSOR_FRAME_COMPLETE) { - // Wait until all ESCs are processed - if (escSensorMotor == getMotorCount()-1) { - escCurrent = 0; - escConsumption = 0; - escVbat = 0; - - for (int i = 0; i < getMotorCount(); i++) { - if (!escSensorData[i].skipped) { - escVbat = i > 0 ? ((escVbat + escSensorData[i].voltage) / 2) : escSensorData[i].voltage; - escCurrent = escCurrent + escSensorData[i].current; - escConsumption = escConsumption + escSensorData[i].consumption; + for (int i = 0; i < getMotorCount(); i++) { + if (!escSensorData[i].skipped) { + escVbat = i > 0 ? ((escVbat + escSensorData[i].voltage) / 2) : escSensorData[i].voltage; + escCurrent = escCurrent + escSensorData[i].current; + escConsumption = escConsumption + escSensorData[i].consumption; + } } } + + DEBUG_SET(DEBUG_ESC_SENSOR, 2, escVbat); + DEBUG_SET(DEBUG_ESC_SENSOR, 3, escCurrent); + + selectNextMotor(); + escSensorTriggerState = ESC_SENSOR_TRIGGER_READY; + escLastResponseTimestamp = currentTimeMs; } - - DEBUG_SET(DEBUG_ESC_SENSOR, 2, escVbat); - DEBUG_SET(DEBUG_ESC_SENSOR, 3, escCurrent); - - selectNextMotor(); - escSensorTriggerState = ESC_SENSOR_TRIGGER_READY; } - if (escTriggerLastTimestamp + 10000 < currentTimeMs) { + if (escLastResponseTimestamp + 10000 < currentTimeMs) { // ESCs did not respond for 10 seconds // Disable ESC telemetry and reset voltage and current to let the use know something is wrong freeEscSensorPort(); @@ -282,8 +285,8 @@ static void selectNextMotor(void) if (escSensorMotor == getMotorCount()) { escSensorMotor = 0; } + timeoutRetryCount = 0; escTriggerTimestamp = millis(); - escTriggerLastTimestamp = escTriggerTimestamp; } //-- CRC From 8bd49918232fee0c6ca41d2ee3170ebc3c972333 Mon Sep 17 00:00:00 2001 From: Bas Delfos Date: Mon, 5 Dec 2016 22:51:21 +0100 Subject: [PATCH 09/14] Fixed changed requested --- src/main/sensors/battery.c | 20 ++++++++++++++------ 1 file changed, 14 insertions(+), 6 deletions(-) diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c index bf1b13941..f090ae756 100644 --- a/src/main/sensors/battery.c +++ b/src/main/sensors/battery.c @@ -269,9 +269,11 @@ void updateCurrentMeter(int32_t lastUpdateAt, rxConfig_t *rxConfig, uint16_t dea updateCurrentDrawn(lastUpdateAt); + updateConsumptionWarning(); + break; case CURRENT_SENSOR_VIRTUAL: - amperage = (int32_t)batteryConfig->currentMeterOffset; + amperageLatest = (int32_t)batteryConfig->currentMeterOffset; if (ARMING_FLAG(ARMED)) { throttleStatus_e throttleStatus = calculateThrottleStatus(rxConfig, deadband3d_throttle); int throttleOffset = (int32_t)rcCommand[THROTTLE] - 1000; @@ -279,24 +281,30 @@ void updateCurrentMeter(int32_t lastUpdateAt, rxConfig_t *rxConfig, uint16_t dea throttleOffset = 0; } int throttleFactor = throttleOffset + (throttleOffset * throttleOffset / 50); - amperage += throttleFactor * (int32_t)batteryConfig->currentMeterScale / 1000; + amperageLatest += throttleFactor * (int32_t)batteryConfig->currentMeterScale / 1000; } + amperage = amperageLatest; updateCurrentDrawn(lastUpdateAt); + updateConsumptionWarning(); + break; case CURRENT_SENSOR_ESC: #ifdef USE_ESC_SENSOR - if (feature(FEATURE_ESC_SENSOR)) - { - amperage = getEscSensorCurrent(); + if (feature(FEATURE_ESC_SENSOR)) { + amperageLatest = getEscSensorCurrent(); + amperage = amperageLatest; mAhDrawn = getEscSensorConsumption(); + + updateConsumptionWarning(); } - #endif break; + #endif case CURRENT_SENSOR_NONE: amperage = 0; + amperageLatest = 0; break; } From 1eadcf3019dcb7a67582dc21d7170362d6f9b03c Mon Sep 17 00:00:00 2001 From: Bas Delfos Date: Mon, 5 Dec 2016 23:37:02 +0100 Subject: [PATCH 10/14] Added configuration for current and battery meter type through CLI --- src/main/io/serial_cli.c | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 25eaecb8f..9c6be899a 100755 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -415,7 +415,11 @@ static const char * const lookupTableGPSSBASMode[] = { #endif static const char * const lookupTableCurrentSensor[] = { - "NONE", "ADC", "VIRTUAL" + "NONE", "ADC", "VIRTUAL", "ESC" +}; + +static const char * const lookupTableBatterySensor[] = { + "NONE", "ADC", "ESC" }; #ifdef USE_SERVOS @@ -571,6 +575,7 @@ typedef enum { TABLE_BLACKBOX_DEVICE, #endif TABLE_CURRENT_SENSOR, + TABLE_BATTERY_SENSOR, #ifdef USE_SERVOS TABLE_GIMBAL_MODE, #endif @@ -611,6 +616,7 @@ static const lookupTableEntry_t lookupTables[] = { { lookupTableBlackboxDevice, sizeof(lookupTableBlackboxDevice) / sizeof(char *) }, #endif { lookupTableCurrentSensor, sizeof(lookupTableCurrentSensor) / sizeof(char *) }, + { lookupTableBatterySensor, sizeof(lookupTableBatterySensor) / sizeof(char *) }, #ifdef USE_SERVOS { lookupTableGimbalMode, sizeof(lookupTableGimbalMode) / sizeof(char *) }, #endif @@ -799,6 +805,7 @@ const clivalue_t valueTable[] = { { "current_meter_offset", VAR_INT16 | MASTER_VALUE, &batteryConfig()->currentMeterOffset, .config.minmax = { -16000, 16000 } }, { "multiwii_current_meter_output", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &batteryConfig()->multiwiiCurrentMeterOutput, .config.lookup = { TABLE_OFF_ON } }, { "current_meter_type", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &batteryConfig()->currentMeterType, .config.lookup = { TABLE_CURRENT_SENSOR } }, + { "battery_meter_type", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &batteryConfig()->batteryMeterType, .config.lookup = { TABLE_BATTERY_SENSOR } }, { "battery_notpresent_level", VAR_UINT8 | MASTER_VALUE, &batteryConfig()->batterynotpresentlevel, .config.minmax = { 0, 200 } }, { "use_vbat_alerts", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &batteryConfig()->useVBatAlerts, .config.lookup = { TABLE_OFF_ON } }, { "use_consumption_alerts", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &batteryConfig()->useConsumptionAlerts, .config.lookup = { TABLE_OFF_ON } }, From f0eebb5cd04b5880d2e68759823e3be1cf0b7beb Mon Sep 17 00:00:00 2001 From: Bas Delfos Date: Tue, 6 Dec 2016 23:24:17 +0100 Subject: [PATCH 11/14] Added battery meter type to end of MSP_(SET_)VOLTAGE_METER_CONFIG --- src/main/fc/fc_msp.c | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 62d75cddb..48ae8c5dd 100755 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -882,6 +882,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn sbufWriteU8(dst, batteryConfig()->vbatmincellvoltage); sbufWriteU8(dst, batteryConfig()->vbatmaxcellvoltage); sbufWriteU8(dst, batteryConfig()->vbatwarningcellvoltage); + sbufWriteU8(dst, batteryConfig()->batteryMeterType); break; case MSP_CURRENT_METER_CONFIG: @@ -1662,6 +1663,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) batteryConfig()->vbatmincellvoltage = sbufReadU8(src); // vbatlevel_warn1 in MWC2.3 GUI batteryConfig()->vbatmaxcellvoltage = sbufReadU8(src); // vbatlevel_warn2 in MWC2.3 GUI batteryConfig()->vbatwarningcellvoltage = sbufReadU8(src); // vbatlevel when buzzer starts to alert + batteryConfig()->batteryMeterType = sbufReadU8(src); break; case MSP_SET_CURRENT_METER_CONFIG: From e93133860f92d112e37f29a368840fd1fec750df Mon Sep 17 00:00:00 2001 From: Bas Delfos Date: Tue, 6 Dec 2016 23:45:26 +0100 Subject: [PATCH 12/14] Rename FUNCTION_TELEMETRY_ESC to FUNCTION_ESC_SENSOR --- src/main/io/serial.h | 2 +- src/main/sensors/esc_sensor.c | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/io/serial.h b/src/main/io/serial.h index c8d7e3178..02f00d823 100644 --- a/src/main/io/serial.h +++ b/src/main/io/serial.h @@ -37,7 +37,7 @@ typedef enum { FUNCTION_BLACKBOX = (1 << 7), // 128 FUNCTION_TELEMETRY_MAVLINK = (1 << 9), // 512 - FUNCTION_TELEMETRY_ESC = (1 << 10) // 1024 + FUNCTION_ESC_SENSOR = (1 << 10) // 1024 } serialPortFunction_e; typedef enum { diff --git a/src/main/sensors/esc_sensor.c b/src/main/sensors/esc_sensor.c index fc11fc89d..af2c0aa11 100644 --- a/src/main/sensors/esc_sensor.c +++ b/src/main/sensors/esc_sensor.c @@ -128,7 +128,7 @@ int16_t getEscSensorConsumption(void) bool escSensorInit(void) { - serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_TELEMETRY_ESC); + serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_ESC_SENSOR); if (!portConfig) { return false; } @@ -136,7 +136,7 @@ bool escSensorInit(void) portOptions_t options = (SERIAL_NOT_INVERTED); // Initialize serial port - escSensorPort = openSerialPort(portConfig->identifier, FUNCTION_TELEMETRY_ESC, escSensorDataReceive, ESC_SENSOR_BAUDRATE, MODE_RX, options); + escSensorPort = openSerialPort(portConfig->identifier, FUNCTION_ESC_SENSOR, escSensorDataReceive, ESC_SENSOR_BAUDRATE, MODE_RX, options); if (escSensorPort) { escSensorEnabled = true; From 97ff8a154e0f4d4f6e7e5709ceeee0d01486b6a8 Mon Sep 17 00:00:00 2001 From: Bas Delfos Date: Wed, 7 Dec 2016 00:03:30 +0100 Subject: [PATCH 13/14] Bumped MSP api version to 0.1.23 and added length check to MSP_SET_VOLTAGE_METER_TYPE --- src/main/fc/fc_msp.c | 4 +++- src/main/msp/msp_protocol.h | 2 +- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 48ae8c5dd..bbc065145 100755 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -1663,7 +1663,9 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) batteryConfig()->vbatmincellvoltage = sbufReadU8(src); // vbatlevel_warn1 in MWC2.3 GUI batteryConfig()->vbatmaxcellvoltage = sbufReadU8(src); // vbatlevel_warn2 in MWC2.3 GUI batteryConfig()->vbatwarningcellvoltage = sbufReadU8(src); // vbatlevel when buzzer starts to alert - batteryConfig()->batteryMeterType = sbufReadU8(src); + if (dataSize > 4) { + batteryConfig()->batteryMeterType = sbufReadU8(src); + } break; case MSP_SET_CURRENT_METER_CONFIG: diff --git a/src/main/msp/msp_protocol.h b/src/main/msp/msp_protocol.h index 84f0a60a1..925d38c14 100644 --- a/src/main/msp/msp_protocol.h +++ b/src/main/msp/msp_protocol.h @@ -59,7 +59,7 @@ #define MSP_PROTOCOL_VERSION 0 #define API_VERSION_MAJOR 1 // increment when major changes are made -#define API_VERSION_MINOR 22 // increment when any change is made, reset to zero when major changes are released after changing API_VERSION_MAJOR +#define API_VERSION_MINOR 23 // increment when any change is made, reset to zero when major changes are released after changing API_VERSION_MAJOR #define API_VERSION_LENGTH 2 From a59846a45a5673de726c802acfb203d909a57426 Mon Sep 17 00:00:00 2001 From: Bas Delfos Date: Thu, 8 Dec 2016 20:59:24 +0100 Subject: [PATCH 14/14] Removed BATTERY_SENSOR.NONE --- src/main/io/serial_cli.c | 2 +- src/main/sensors/battery.h | 3 +-- 2 files changed, 2 insertions(+), 3 deletions(-) diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 9c6be899a..304918b0b 100755 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -419,7 +419,7 @@ static const char * const lookupTableCurrentSensor[] = { }; static const char * const lookupTableBatterySensor[] = { - "NONE", "ADC", "ESC" + "ADC", "ESC" }; #ifdef USE_SERVOS diff --git a/src/main/sensors/battery.h b/src/main/sensors/battery.h index 8f0b1d65f..cb4de3911 100644 --- a/src/main/sensors/battery.h +++ b/src/main/sensors/battery.h @@ -36,8 +36,7 @@ typedef enum { } currentSensor_e; typedef enum { - BATTERY_SENSOR_NONE = 0, - BATTERY_SENSOR_ADC, + BATTERY_SENSOR_ADC = 0, BATTERY_SENSOR_ESC } batterySensor_e;