rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #1646 from basdelfos/esc_telemetry_as_sensor 

ESC telemetry as sensor
This commit is contained in:
J Blackman 2016-12-11 18:26:41 +11:00 committed by GitHub
parent 2c6da57b38 a59846a45a
commit ccfb19edfe
39 changed files with 412 additions and 393 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
.gitignore vendored
View File

@ -13,6 +13,7 @@
obj/
patches/
startup_stm32f10x_md_gcc.s
.idea
# script-generated files
docs/Manual.pdf

2
Makefile
View File

@ -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 \

2
src/main/build/debug.h
View File

@ -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

2
src/main/fc/config.h
View File

@ -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);

4
src/main/fc/fc_msp.c
View File

@ -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:

31
src/main/fc/fc_tasks.c
View File

@ -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

2
src/main/io/serial.h
View File

@ -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 {

13
src/main/io/serial_cli.c
View File

@ -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 } },

8
src/main/main.c
View File

@ -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

2
src/main/msp/msp_protocol.h
View File

@ -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

4
src/main/scheduler/scheduler.h
View File

@ -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,

17
src/main/sensors/battery.c
View File

@ -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();
}

6
src/main/sensors/battery.h
View File

@ -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;

313
src/main/sensors/esc_sensor.c Normal file
View File

@ -0,0 +1,313 @@
#include <stdbool.h>
#include <stdint.h>
#include <stdlib.h>
#include <platform.h>
#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<BufLen; i++) crc = update_crc8(Buf[i], crc);
return (crc);
}
#endif

10
src/main/sensors/esc_sensor.h Normal file
View File

@ -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);

2
src/main/target/ANYFCF7/target.h
View File

@ -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

3
src/main/target/BETAFLIGHTF3/target.h
View File

@ -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

3
src/main/target/BLUEJAYF4/target.h
View File

@ -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

3
src/main/target/DOGE/target.h
View File

@ -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

3
src/main/target/FURYF3/target.h
View File

@ -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

3
src/main/target/FURYF4/target.h
View File

@ -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

2
src/main/target/FURYF7/target.h
View File

@ -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

3
src/main/target/IMPULSERCF3/target.h
View File

@ -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

3
src/main/target/KISSFC/target.h
View File

@ -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

3
src/main/target/LUX_RACE/target.h
View File

@ -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

3
src/main/target/MOTOLAB/target.h
View File

@ -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

3
src/main/target/MULTIFLITEPICO/target.h
View File

@ -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

3
src/main/target/OMNIBUS/target.h
View File

@ -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

3
src/main/target/OMNIBUSF4/target.h
View File

@ -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

3
src/main/target/REVO/target.h
View File

@ -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

3
src/main/target/SPARKY2/target.h
View File

@ -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

3
src/main/target/SPRACINGF3/target.h
View File

@ -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

3
src/main/target/SPRACINGF3EVO/target.h
View File

@ -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

3
src/main/target/STM32F3DISCOVERY/target.h
View File

@ -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

3
src/main/target/X_RACERSPI/target.h
View File

@ -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

312
src/main/telemetry/esc_telemetry.c
View File

@ -1,312 +0,0 @@
#include <stdbool.h>
#include <stdint.h>
#include <stdlib.h>
#include <platform.h>
#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<BufLen; i++) crc = update_crc8(Buf[i], crc);
return (crc);
}
#endif

12
src/main/telemetry/esc_telemetry.h
View File

@ -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);

2
src/main/telemetry/frsky.c
View File

@ -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();

4
src/main/telemetry/smartport.c
View File

@ -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;
}