diff --git a/.gitignore b/.gitignore index 86b85dc447..67e2f3cbe8 100644 --- a/.gitignore +++ b/.gitignore @@ -13,6 +13,7 @@ obj/ patches/ startup_stm32f10x_md_gcc.s +.idea # script-generated files docs/Manual.pdf diff --git a/Makefile b/Makefile index 3e02f53a4b..281f9a015e 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 \ SPEED_OPTIMISED_SRC = \ common/encoding.c \ diff --git a/src/main/build/debug.h b/src/main/build/debug.h index 6b2e7ca9f9..cd6fc1d780 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 c5fe4ecf60..0fe4f9e616 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_msp.c b/src/main/fc/fc_msp.c index 62d75cddb0..bbc0651459 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,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 + if (dataSize > 4) { + batteryConfig()->batteryMeterType = sbufReadU8(src); + } break; case MSP_SET_CURRENT_METER_CONFIG: diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index f043290f61..cb04a8f23f 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" @@ -109,9 +109,9 @@ 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_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.h b/src/main/io/serial.h index c8d7e31785..02f00d8238 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/io/serial_cli.c b/src/main/io/serial_cli.c index 2f6585a462..57a85a6e0e 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 @@ -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[] = { + "ADC", "ESC" }; #ifdef USE_SERVOS @@ -519,7 +523,7 @@ static const char * const lookupTableDebug[DEBUG_COUNT] = { "VELOCITY", "DFILTER", "ANGLERATE", - "ESC_TELEMETRY", + "ESC_SENSOR", "SCHEDULER", }; @@ -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 } }, diff --git a/src/main/main.c b/src/main/main.c index cf1060a547..d870527469 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/msp/msp_protocol.h b/src/main/msp/msp_protocol.h index 84f0a60a1b..925d38c149 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 diff --git a/src/main/scheduler/scheduler.h b/src/main/scheduler/scheduler.h index 36f5559eff..133943a3f1 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 3a8a59a8a1..f090ae756c 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; } @@ -292,11 +291,11 @@ void updateCurrentMeter(int32_t lastUpdateAt, rxConfig_t *rxConfig, uint16_t dea break; case CURRENT_SENSOR_ESC: - #ifdef USE_ESC_TELEMETRY - if (isEscTelemetryActive()) { - amperageLatest = getEscTelemetryCurrent(); + #ifdef USE_ESC_SENSOR + if (feature(FEATURE_ESC_SENSOR)) { + amperageLatest = getEscSensorCurrent(); amperage = amperageLatest; - mAhDrawn = getEscTelemetryConsumption(); + mAhDrawn = getEscSensorConsumption(); updateConsumptionWarning(); } diff --git a/src/main/sensors/battery.h b/src/main/sensors/battery.h index 01309a8c8c..cb4de39116 100644 --- a/src/main/sensors/battery.h +++ b/src/main/sensors/battery.h @@ -31,12 +31,12 @@ 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 { - BATTERY_SENSOR_NONE = 0, - BATTERY_SENSOR_ADC, + BATTERY_SENSOR_ADC = 0, BATTERY_SENSOR_ESC } batterySensor_e; diff --git a/src/main/sensors/esc_sensor.c b/src/main/sensors/esc_sensor.c new file mode 100644 index 0000000000..af2c0aa11e --- /dev/null +++ b/src/main/sensors/esc_sensor.c @@ -0,0 +1,313 @@ +#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_sensor.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_SENSOR_FRAME_PENDING = 1 << 0, // 1 + ESC_SENSOR_FRAME_COMPLETE = 1 << 1 // 2 +} escTlmFrameState_t; + +typedef enum { + ESC_SENSOR_TRIGGER_WAIT = 0, + ESC_SENSOR_TRIGGER_READY = 1 << 0, // 1 + ESC_SENSOR_TRIGGER_PENDING = 1 << 1, // 2 +} escSensorTriggerState_t; + +#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_SENSOR_BUFFSIZE] = { 0, }; +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 escLastResponseTimestamp; +static uint8_t timeoutRetryCount = 0; +static uint8_t totalRetryCount = 0; + +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 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 isEscSensorActive(void) +{ + return escSensorEnabled; +} + +int16_t getEscSensorVbat(void) +{ + return escVbat / 10; +} + +int16_t getEscSensorCurrent(void) +{ + return escCurrent; +} + +int16_t getEscSensorConsumption(void) +{ + return escConsumption; +} + +bool escSensorInit(void) +{ + serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_ESC_SENSOR); + if (!portConfig) { + return false; + } + + portOptions_t options = (SERIAL_NOT_INVERTED); + + // Initialize serial port + escSensorPort = openSerialPort(portConfig->identifier, FUNCTION_ESC_SENSOR, escSensorDataReceive, ESC_SENSOR_BAUDRATE, MODE_RX, options); + + if (escSensorPort) { + escSensorEnabled = true; + } + + return escSensorPort != NULL; +} + +void freeEscSensorPort(void) +{ + closeSerialPort(escSensorPort); + escSensorPort = NULL; + escSensorEnabled = false; +} + +// Receive ISR callback +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 (escSensorTriggerState == ESC_SENSOR_TRIGGER_WAIT) return; + + tlm[tlmFramePosition] = (uint8_t)c; + + if (tlmFramePosition == ESC_SENSOR_BUFFSIZE - 1) { + tlmFrameDone = true; + tlmFramePosition = 0; + } else { + tlmFramePosition++; + } +} + +uint8_t escSensorFrameStatus(void) +{ + uint8_t frameStatus = ESC_SENSOR_FRAME_PENDING; + uint16_t chksum, tlmsum; + + if (!tlmFrameDone) { + return frameStatus; + } + + tlmFrameDone = false; + + // Get CRC8 checksum + chksum = get_crc8(tlm, ESC_SENSOR_BUFFSIZE - 1); + tlmsum = tlm[ESC_SENSOR_BUFFSIZE - 1]; // last byte contains CRC value + + if (chksum == tlmsum) { + 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_SENSOR_FRAME_COMPLETE; + } + + return frameStatus; +} + +void escSensorProcess(timeUs_t currentTimeUs) +{ + const timeMs_t currentTimeMs = currentTimeUs / 1000; + + if (!escSensorEnabled) { + return; + } + + // Wait period of time before requesting telemetry (let the system boot first) + if (currentTimeMs < ESC_BOOTTIME) { + return; + } + else if (escSensorTriggerState == ESC_SENSOR_TRIGGER_WAIT) { + // Ready for starting requesting telemetry + escSensorTriggerState = ESC_SENSOR_TRIGGER_READY; + escSensorMotor = 0; + escTriggerTimestamp = currentTimeMs; + escLastResponseTimestamp = escTriggerTimestamp; + } + else if (escSensorTriggerState == ESC_SENSOR_TRIGGER_READY) { + DEBUG_SET(DEBUG_ESC_SENSOR, 0, escSensorMotor+1); + + motorDmaOutput_t * const motor = getMotorDmaOutput(escSensorMotor); + 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 (timeoutRetryCount == 4) { + // Not responding after 3 times, skip motor + escSensorData[escSensorMotor].skipped = true; + selectNextMotor(); + } + + DEBUG_SET(DEBUG_ESC_SENSOR, 1, ++totalRetryCount); + } + + // 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; + + 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; + } + } + + 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(); + escVbat = 0; + escCurrent = 0; + } +} + +static void selectNextMotor(void) +{ + escSensorMotor++; + if (escSensorMotor == getMotorCount()) { + escSensorMotor = 0; + } + timeoutRetryCount = 0; + escTriggerTimestamp = millis(); +} + +//-- 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 -#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; - batteryConfig()->currentMeterType = CURRENT_SENSOR_ESC; - 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(timeUs_t currentTimeUs) -{ - const timeMs_t currentTimeMs = currentTimeUs / 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_SENSOR)) && batteryCellCount > 0) { sendVoltage(); sendVoltageAmp(); sendAmperage(); diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c index 496013f143..4d1d19718a 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; }