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

Merge pull request #1525 from basdelfos/esc_telemetry_dshot_rebase 

Implementation of KISS ESC Telemetry protocol (rebase)
This commit is contained in:
Martin Budden 2016-11-17 23:30:03 +01:00 committed by GitHub
parent 6f7496db67 248f7184eb
commit 935547fe50
43 changed files with 493 additions and 50 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
Makefile
View File

@ -589,7 +589,8 @@ HIGHEND_SRC = \
telemetry/hott.c \
telemetry/smartport.c \
telemetry/ltm.c \
telemetry/mavlink.c
telemetry/mavlink.c \
telemetry/esc_telemetry.c \
ifeq ($(TARGET),$(filter $(TARGET),$(F4_TARGETS)))
VCP_SRC = \

7
src/main/blackbox/blackbox.c
View File

@ -808,7 +808,7 @@ void startBlackbox(void)
blackboxHistory[1] = &blackboxHistoryRing[1];
blackboxHistory[2] = &blackboxHistoryRing[2];
vbatReference = vbatLatestADC;
vbatReference = vbatLatest;
//No need to clear the content of blackboxHistoryRing since our first frame will be an intra which overwrites it
@ -1005,8 +1005,8 @@ static void loadMainState(uint32_t currentTime)
blackboxCurrent->motor[i] = motor[i];
}
blackboxCurrent->vbatLatest = vbatLatestADC;
blackboxCurrent->amperageLatest = amperageLatestADC;
blackboxCurrent->vbatLatest = vbatLatest;
blackboxCurrent->amperageLatest = amperageLatest;
#ifdef MAG
for (i = 0; i < XYZ_AXIS_COUNT; i++) {
@ -1625,4 +1625,3 @@ void initBlackbox(void)
}
}
#endif

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

@ -59,5 +59,6 @@ typedef enum {
DEBUG_VELOCITY,
DEBUG_DTERM_FILTER,
DEBUG_ANGLERATE,
DEBUG_ESC_TELEMETRY,
DEBUG_COUNT
} debugType_e;

4
src/main/drivers/pwm_output.h
View File

@ -64,6 +64,7 @@ typedef struct {
const timerHardware_t *timerHardware;
uint16_t value;
uint16_t timerDmaSource;
volatile bool requestTelemetry;
#if defined(STM32F3) || defined(STM32F4) || defined(STM32F7)
uint32_t dmaBuffer[MOTOR_DMA_BUFFER_SIZE];
#else
@ -75,6 +76,8 @@ typedef struct {
#endif
} motorDmaOutput_t;
motorDmaOutput_t *getMotorDmaOutput(uint8_t index);
extern bool pwmMotorsEnabled;
struct timerHardware_s;
@ -111,4 +114,3 @@ pwmOutputPort_t *pwmGetMotors(void);
bool pwmIsSynced(void);
void pwmDisableMotors(void);
void pwmEnableMotors(void);

9
src/main/drivers/pwm_output_stm32f3xx.c
View File

@ -46,6 +46,11 @@ static uint8_t dmaMotorTimerCount = 0;
static motorDmaTimer_t dmaMotorTimers[MAX_DMA_TIMERS];
static motorDmaOutput_t dmaMotors[MAX_SUPPORTED_MOTORS];
motorDmaOutput_t *getMotorDmaOutput(uint8_t index)
{
return &dmaMotors[index];
}
uint8_t getTimerIndex(TIM_TypeDef *timer)
{
for (int i = 0; i < dmaMotorTimerCount; i++) {
@ -66,7 +71,9 @@ void pwmWriteDigital(uint8_t index, uint16_t value)
motorDmaOutput_t * const motor = &dmaMotors[index];
uint16_t packet = (value << 1) | 0; // Here goes telemetry bit (false for now)
uint16_t packet = (value << 1) | (motor->requestTelemetry ? 1 : 0);
motor->requestTelemetry = false; // reset telemetry request to make sure it's triggered only once in a row
// compute checksum
int csum = 0;
int csum_data = packet;

9
src/main/drivers/pwm_output_stm32f4xx.c
View File

@ -45,6 +45,11 @@ static uint8_t dmaMotorTimerCount = 0;
static motorDmaTimer_t dmaMotorTimers[MAX_DMA_TIMERS];
static motorDmaOutput_t dmaMotors[MAX_SUPPORTED_MOTORS];
motorDmaOutput_t *getMotorDmaOutput(uint8_t index)
{
return &dmaMotors[index];
}
uint8_t getTimerIndex(TIM_TypeDef *timer)
{
for (int i = 0; i < dmaMotorTimerCount; i++) {
@ -64,7 +69,9 @@ void pwmWriteDigital(uint8_t index, uint16_t value)
motorDmaOutput_t * const motor = &dmaMotors[index];
uint16_t packet = (value << 1) | 0; // Here goes telemetry bit (false for now)
uint16_t packet = (value << 1) | (motor->requestTelemetry ? 1 : 0);
motor->requestTelemetry = false; // reset telemetry request to make sure it's triggered only once in a row
// compute checksum
int csum = 0;
int csum_data = packet;

9
src/main/drivers/pwm_output_stm32f7xx.c
View File

@ -44,6 +44,11 @@ static uint8_t dmaMotorTimerCount = 0;
static motorDmaTimer_t dmaMotorTimers[MAX_DMA_TIMERS];
static motorDmaOutput_t dmaMotors[MAX_SUPPORTED_MOTORS];
motorDmaOutput_t *getMotorDmaOutput(uint8_t index)
{
return &dmaMotors[index];
}
uint8_t getTimerIndex(TIM_TypeDef *timer)
{
for (int i = 0; i < dmaMotorTimerCount; i++) {
@ -64,7 +69,9 @@ void pwmWriteDigital(uint8_t index, uint16_t value)
motorDmaOutput_t * const motor = &dmaMotors[index];
uint16_t packet = (value << 1) | 0; // Here goes telemetry bit (false for now)
uint16_t packet = (value << 1) | (motor->requestTelemetry ? 1 : 0);
motor->requestTelemetry = false; // reset telemetry request to make sure it's triggered only once in a row
// compute checksum
int csum = 0;
int csum_data = packet;

1
src/main/fc/config.c
View File

@ -424,6 +424,7 @@ void resetBatteryConfig(batteryConfig_t *batteryConfig)
batteryConfig->vbatmincellvoltage = 33;
batteryConfig->vbatwarningcellvoltage = 35;
batteryConfig->vbathysteresis = 1;
batteryConfig->batteryMeterType = BATTERY_SENSOR_ADC;
batteryConfig->currentMeterOffset = 0;
batteryConfig->currentMeterScale = 400; // for Allegro ACS758LCB-100U (40mV/A)
batteryConfig->batteryCapacity = 0;

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

@ -55,6 +55,7 @@ typedef enum {
FEATURE_VTX = 1 << 24,
FEATURE_RX_SPI = 1 << 25,
FEATURE_SOFTSPI = 1 << 26,
FEATURE_ESC_TELEMETRY = 1 << 27,
} features_e;
void beeperOffSet(uint32_t mask);

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

@ -66,6 +66,7 @@
#include "scheduler/scheduler.h"
#include "telemetry/telemetry.h"
#include "telemetry/esc_telemetry.h"
#include "config/feature.h"
#include "config/config_profile.h"
@ -102,7 +103,7 @@ static void taskUpdateBattery(uint32_t currentTime)
{
#ifdef USE_ADC
static uint32_t vbatLastServiced = 0;
if (feature(FEATURE_VBAT)) {
if (feature(FEATURE_VBAT) || feature(FEATURE_ESC_TELEMETRY)) {
if (cmp32(currentTime, vbatLastServiced) >= VBATINTERVAL) {
vbatLastServiced = currentTime;
updateBattery();
@ -111,7 +112,7 @@ static void taskUpdateBattery(uint32_t currentTime)
#endif
static uint32_t ibatLastServiced = 0;
if (feature(FEATURE_CURRENT_METER)) {
if (feature(FEATURE_CURRENT_METER) || feature(FEATURE_ESC_TELEMETRY)) {
const int32_t ibatTimeSinceLastServiced = cmp32(currentTime, ibatLastServiced);
if (ibatTimeSinceLastServiced >= IBATINTERVAL) {
@ -194,6 +195,15 @@ static void taskTelemetry(uint32_t currentTime)
}
#endif
#ifdef USE_ESC_TELEMETRY
static void taskEscTelemetry(uint32_t currentTime)
{
if (feature(FEATURE_ESC_TELEMETRY)) {
escTelemetryProcess(currentTime);
}
}
#endif
void fcTasksInit(void)
{
schedulerInit();
@ -254,6 +264,9 @@ void fcTasksInit(void)
#ifdef USE_BST
setTaskEnabled(TASK_BST_MASTER_PROCESS, true);
#endif
#ifdef USE_ESC_TELEMETRY
setTaskEnabled(TASK_ESC_TELEMETRY, feature(FEATURE_ESC_TELEMETRY));
#endif
#ifdef CMS
#ifdef USE_MSP_DISPLAYPORT
setTaskEnabled(TASK_CMS, true);
@ -421,6 +434,15 @@ cfTask_t cfTasks[TASK_COUNT] = {
},
#endif
#ifdef USE_ESC_TELEMETRY
[TASK_ESC_TELEMETRY] = {
.taskName = "ESC_TELEMETRY",
.taskFunc = taskEscTelemetry,
.desiredPeriod = 1000000 / 100, // 100 Hz
.staticPriority = TASK_PRIORITY_LOW,
},
#endif
#ifdef CMS
[TASK_CMS] = {
.taskName = "CMS",

4
src/main/flight/mixer.c
View File

@ -240,6 +240,10 @@ static motorMixer_t *customMixers;
static uint16_t disarmMotorOutput, minMotorOutputNormal, maxMotorOutputNormal, deadbandMotor3dHigh, deadbandMotor3dLow;
static float rcCommandThrottleRange;
uint8_t getMotorCount() {
return motorCount;
}
bool isMotorProtocolDshot(void) {
#ifdef USE_DSHOT
if (motorConfig->motorPwmProtocol == PWM_TYPE_DSHOT150 || motorConfig->motorPwmProtocol == PWM_TYPE_DSHOT300 || motorConfig->motorPwmProtocol == PWM_TYPE_DSHOT600)

2
src/main/flight/mixer.h
View File

@ -115,6 +115,8 @@ extern int16_t motor_disarmed[MAX_SUPPORTED_MOTORS];
struct motorConfig_s;
struct rxConfig_s;
uint8_t getMotorCount();
void mixerUseConfigs(
flight3DConfig_t *flight3DConfigToUse,
struct motorConfig_s *motorConfigToUse,

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

@ -37,6 +37,7 @@ typedef enum {
FUNCTION_BLACKBOX = (1 << 7), // 128
FUNCTION_PASSTHROUGH = (1 << 8), // 256
FUNCTION_TELEMETRY_MAVLINK = (1 << 9), // 512
FUNCTION_TELEMETRY_ESC = (1 << 10), // 1024
} serialPortFunction_e;
typedef enum {

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

@ -231,7 +231,7 @@ static const char * const featureNames[] = {
"SONAR", "TELEMETRY", "CURRENT_METER", "3D", "RX_PARALLEL_PWM",
"RX_MSP", "RSSI_ADC", "LED_STRIP", "DISPLAY", "OSD",
"BLACKBOX", "CHANNEL_FORWARDING", "TRANSPONDER", "AIRMODE",
"SDCARD", "VTX", "RX_SPI", "SOFTSPI", NULL
"SDCARD", "VTX", "RX_SPI", "SOFTSPI", "ESC_TELEMETRY", NULL
};
// sync this with rxFailsafeChannelMode_e
@ -513,6 +513,7 @@ static const char * const lookupTableDebug[DEBUG_COUNT] = {
"VELOCITY",
"DFILTER",
"ANGLERATE",
"ESC_TELEMETRY",
};
#ifdef OSD
@ -3600,14 +3601,14 @@ static void cliTasks(char *cmdline)
subTaskFrequency = (int)(1000000.0f / ((float)cycleTime));
taskFrequency = subTaskFrequency / masterConfig.pid_process_denom;
if (masterConfig.pid_process_denom > 1) {
cliPrintf("%02d - (%12s) ", taskId, taskInfo.taskName);
cliPrintf("%02d - (%13s) ", taskId, taskInfo.taskName);
} else {
taskFrequency = subTaskFrequency;
cliPrintf("%02d - (%8s/%3s) ", taskId, taskInfo.subTaskName, taskInfo.taskName);
cliPrintf("%02d - (%9s/%3s) ", taskId, taskInfo.subTaskName, taskInfo.taskName);
}
} else {
taskFrequency = (int)(1000000.0f / ((float)taskInfo.latestDeltaTime));
cliPrintf("%02d - (%12s) ", taskId, taskInfo.taskName);
cliPrintf("%02d - (%13s) ", taskId, taskInfo.taskName);
}
const int maxLoad = (taskInfo.maxExecutionTime * taskFrequency + 5000) / 1000;
const int averageLoad = (taskInfo.averageExecutionTime * taskFrequency + 5000) / 1000;
@ -3619,11 +3620,11 @@ static void cliTasks(char *cmdline)
taskFrequency, taskInfo.maxExecutionTime, taskInfo.averageExecutionTime,
maxLoad/10, maxLoad%10, averageLoad/10, averageLoad%10, taskInfo.totalExecutionTime / 1000);
if (taskId == TASK_GYROPID && masterConfig.pid_process_denom > 1) {
cliPrintf(" - (%12s) %6d\r\n", taskInfo.subTaskName, subTaskFrequency);
cliPrintf(" - (%13s) %6d\r\n", taskInfo.subTaskName, subTaskFrequency);
}
}
}
cliPrintf("Total (excluding SERIAL) %22d.%1d%% %4d.%1d%%\r\n", maxLoadSum/10, maxLoadSum%10, averageLoadSum/10, averageLoadSum%10);
cliPrintf("Total (excluding SERIAL) %23d.%1d%% %4d.%1d%%\r\n", maxLoadSum/10, maxLoadSum%10, averageLoadSum/10, averageLoadSum%10);
}
#endif

7
src/main/main.c
View File

@ -103,6 +103,7 @@
#include "sensors/initialisation.h"
#include "telemetry/telemetry.h"
#include "telemetry/esc_telemetry.h"
#include "flight/pid.h"
#include "flight/imu.h"
@ -490,6 +491,12 @@ void init(void)
}
#endif
#ifdef USE_ESC_TELEMETRY
if (feature(FEATURE_ESC_TELEMETRY)) {
escTelemetryInit();
}
#endif
#ifdef USB_CABLE_DETECTION
usbCableDetectInit();
#endif

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

@ -85,6 +85,9 @@ typedef enum {
#ifdef USE_BST
TASK_BST_MASTER_PROCESS,
#endif
#ifdef USE_ESC_TELEMETRY
TASK_ESC_TELEMETRY,
#endif
#ifdef CMS
TASK_CMS,
#endif

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

@ -35,11 +35,15 @@
#include "sensors/battery.h"
#include "telemetry/esc_telemetry.h"
#include "fc/rc_controls.h"
#include "io/beeper.h"
#include "rx/rx.h"
#include "common/utils.h"
#define VBATT_LPF_FREQ 0.4f
// Battery monitoring stuff
@ -48,8 +52,8 @@ uint16_t batteryWarningVoltage;
uint16_t batteryCriticalVoltage;
uint16_t vbat = 0; // battery voltage in 0.1V steps (filtered)
uint16_t vbatLatestADC = 0; // most recent unsmoothed raw reading from vbat ADC
uint16_t amperageLatestADC = 0; // most recent raw reading from current ADC
uint16_t vbatLatest = 0; // most recent unsmoothed value
uint16_t amperageLatest = 0; // most recent value
int32_t amperage = 0; // amperage read by current sensor in centiampere (1/100th A)
int32_t mAhDrawn = 0; // milliampere hours drawn from the battery since start
@ -69,7 +73,17 @@ static void updateBatteryVoltage(void)
static bool vbatFilterIsInitialised;
// store the battery voltage with some other recent battery voltage readings
uint16_t vbatSample = vbatLatestADC = adcGetChannel(ADC_BATTERY);
uint16_t vbatSample;
#ifdef USE_ESC_TELEMETRY
if (batteryConfig->batteryMeterType == BATTERY_SENSOR_ESC && isEscTelemetryActive()) {
vbatSample = vbatLatest = getEscTelemetryVbat();
}
else
#endif
{
vbatSample = vbatLatest = batteryAdcToVoltage(adcGetChannel(ADC_BATTERY));
}
if (debugMode == DEBUG_BATTERY) debug[0] = vbatSample;
@ -77,8 +91,7 @@ static void updateBatteryVoltage(void)
biquadFilterInitLPF(&vbatFilter, VBATT_LPF_FREQ, 50000); //50HZ Update
vbatFilterIsInitialised = true;
}
vbatSample = biquadFilterApply(&vbatFilter, vbatSample);
vbat = batteryAdcToVoltage(vbatSample);
vbat = biquadFilterApply(&vbatFilter, vbatSample);
if (debugMode == DEBUG_BATTERY) debug[1] = vbat;
}
@ -87,12 +100,12 @@ static void updateBatteryVoltage(void)
void updateBattery(void)
{
uint16_t vbatPreviousADC = vbatLatestADC;
uint16_t vbatPrevious = vbatLatest;
updateBatteryVoltage();
uint16_t vbatMeasured = batteryAdcToVoltage(vbatLatestADC);
uint16_t vbatMeasured = vbatLatest;
/* battery has just been connected*/
if (batteryState == BATTERY_NOT_PRESENT && (ARMING_FLAG(ARMED) || (vbat > batteryConfig->batterynotpresentlevel && ABS(vbatMeasured - batteryAdcToVoltage(vbatPreviousADC)) <= VBAT_STABLE_MAX_DELTA))) {
if (batteryState == BATTERY_NOT_PRESENT && (ARMING_FLAG(ARMED) || (vbat > batteryConfig->batterynotpresentlevel && ABS(vbatMeasured - vbatPrevious) <= VBAT_STABLE_MAX_DELTA))) {
/* Actual battery state is calculated below, this is really BATTERY_PRESENT */
batteryState = BATTERY_OK;
@ -105,13 +118,16 @@ void updateBattery(void)
batteryWarningVoltage = batteryCellCount * batteryConfig->vbatwarningcellvoltage;
batteryCriticalVoltage = batteryCellCount * batteryConfig->vbatmincellvoltage;
/* battery has been disconnected - can take a while for filter cap to disharge so we use a threshold of batteryConfig->batterynotpresentlevel */
} else if (batteryState != BATTERY_NOT_PRESENT && !ARMING_FLAG(ARMED) && vbat <= batteryConfig->batterynotpresentlevel && ABS(vbatMeasured - batteryAdcToVoltage(vbatPreviousADC)) <= VBAT_STABLE_MAX_DELTA) {
} else if (batteryState != BATTERY_NOT_PRESENT && !ARMING_FLAG(ARMED) && vbat <= batteryConfig->batterynotpresentlevel && ABS(vbatMeasured - vbatPrevious) <= VBAT_STABLE_MAX_DELTA) {
batteryState = BATTERY_NOT_PRESENT;
batteryCellCount = 0;
batteryWarningVoltage = 0;
batteryCriticalVoltage = 0;
}
if (debugMode == DEBUG_BATTERY) debug[2] = batteryState;
if (debugMode == DEBUG_BATTERY) debug[3] = batteryCellCount;
switch(batteryState)
{
case BATTERY_OK:
@ -177,16 +193,22 @@ static int32_t currentSensorToCentiamps(uint16_t src)
void updateCurrentMeter(int32_t lastUpdateAt, rxConfig_t *rxConfig, uint16_t deadband3d_throttle)
{
static int32_t amperageRaw = 0;
#ifdef USE_ESC_TELEMETRY
UNUSED(lastUpdateAt);
#else
static int64_t mAhdrawnRaw = 0;
#endif
static int32_t amperageRaw = 0;
int32_t throttleOffset = (int32_t)rcCommand[THROTTLE] - 1000;
int32_t throttleFactor = 0;
switch(batteryConfig->currentMeterType) {
case CURRENT_SENSOR_ADC:
amperageRaw -= amperageRaw / 8;
amperageRaw += (amperageLatestADC = adcGetChannel(ADC_CURRENT));
amperage = currentSensorToCentiamps(amperageRaw / 8);
amperageRaw += adcGetChannel(ADC_CURRENT);
amperage = amperageLatest = currentSensorToCentiamps(amperageRaw / 8);
break;
case CURRENT_SENSOR_VIRTUAL:
amperage = (int32_t)batteryConfig->currentMeterOffset;
@ -201,10 +223,20 @@ void updateCurrentMeter(int32_t lastUpdateAt, rxConfig_t *rxConfig, uint16_t dea
case CURRENT_SENSOR_NONE:
amperage = 0;
break;
case CURRENT_SENSOR_ESC:
#ifdef USE_ESC_TELEMETRY
if (batteryConfig->currentMeterType == CURRENT_SENSOR_ESC && isEscTelemetryActive()) {
amperage = getEscTelemetryCurrent();
mAhDrawn = getEscTelemetryConsumption();
}
#endif
break;
}
#ifndef USE_ESC_TELEMETRY
mAhdrawnRaw += (amperage * lastUpdateAt) / 1000;
mAhDrawn = mAhdrawnRaw / (3600 * 100);
#endif
}
float calculateVbatPidCompensation(void) {

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

@ -31,9 +31,17 @@ typedef enum {
CURRENT_SENSOR_NONE = 0,
CURRENT_SENSOR_ADC,
CURRENT_SENSOR_VIRTUAL,
CURRENT_SENSOR_MAX = CURRENT_SENSOR_VIRTUAL
CURRENT_SENSOR_ESC,
CURRENT_SENSOR_MAX = CURRENT_SENSOR_ESC
} currentSensor_e;
typedef enum {
BATTERY_SENSOR_NONE = 0,
BATTERY_SENSOR_ADC,
BATTERY_SENSOR_ESC,
BATTERY_SENSOR_MAX = BATTERY_SENSOR_ESC
} batterySensor_e;
typedef struct batteryConfig_s {
uint8_t vbatscale; // adjust this to match battery voltage to reported value
uint8_t vbatresdivval; // resistor divider R2 (default NAZE 10(K))
@ -42,10 +50,11 @@ typedef struct batteryConfig_s {
uint8_t vbatmincellvoltage; // minimum voltage per cell, this triggers battery critical alarm, in 0.1V units, default is 33 (3.3V)
uint8_t vbatwarningcellvoltage; // warning voltage per cell, this triggers battery warning alarm, in 0.1V units, default is 35 (3.5V)
uint8_t vbathysteresis; // hysteresis for alarm, default 1 = 0.1V
batterySensor_e batteryMeterType; // type of battery meter uses, either ADC or ESC
int16_t currentMeterScale; // scale the current sensor output voltage to milliamps. Value in 1/10th mV/A
uint16_t currentMeterOffset; // offset of the current sensor in millivolt steps
currentSensor_e currentMeterType; // type of current meter used, either ADC or virtual
currentSensor_e currentMeterType; // type of current meter used, either ADC, Virtual or ESC
// FIXME this doesn't belong in here since it's a concern of MSP, not of the battery code.
uint8_t multiwiiCurrentMeterOutput; // if set to 1 output the amperage in milliamp steps instead of 0.01A steps via msp
@ -62,10 +71,10 @@ typedef enum {
extern uint16_t vbat;
extern uint16_t vbatRaw;
extern uint16_t vbatLatestADC;
extern uint16_t vbatLatest;
extern uint8_t batteryCellCount;
extern uint16_t batteryWarningVoltage;
extern uint16_t amperageLatestADC;
extern uint16_t amperageLatest;
extern int32_t amperage;
extern int32_t mAhDrawn;

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

@ -24,6 +24,7 @@
#define USBD_PRODUCT_STRING "AnyFCF7"
#define USE_DSHOT
#define USE_ESC_TELEMETRY
#define LED0 PB7
#define LED1 PB6

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

@ -45,6 +45,7 @@
#define USE_EXTI
#define USE_DSHOT
#define USE_ESC_TELEMETRY
#define REMAP_TIM16_DMA
#define REMAP_TIM17_DMA

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

@ -145,6 +145,7 @@
#define VBAT_ADC_PIN PC3
#define USE_DSHOT
#define USE_ESC_TELEMETRY
#define LED_STRIP
#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT
@ -166,4 +167,3 @@
#define USABLE_TIMER_CHANNEL_COUNT 7
#define USED_TIMERS ( TIM_N(2) | TIM_N(3) | TIM_N(5) | TIM_N(8) | TIM_N(9))

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

@ -126,6 +126,7 @@
#define LED_STRIP
#define USE_DSHOT
#define USE_ESC_TELEMETRY
#define DEFAULT_RX_FEATURE FEATURE_RX_PPM
@ -146,4 +147,3 @@
// only 6 outputs available on hardware
#define USABLE_TIMER_CHANNEL_COUNT 10
#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(15))

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

@ -162,6 +162,7 @@
#define USE_SERIAL_4WAY_BLHELI_INTERFACE
#define USE_DSHOT
#define USE_ESC_TELEMETRY
#define REMAP_TIM17_DMA
#define TARGET_IO_PORTA 0xffff
@ -171,4 +172,3 @@
#define USABLE_TIMER_CHANNEL_COUNT 8
#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(8) | TIM_N(16) | TIM_N(17))

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

@ -176,6 +176,7 @@
#define USE_SERIAL_4WAY_BLHELI_INTERFACE
#define USE_DSHOT
#define USE_ESC_TELEMETRY
#define TARGET_IO_PORTA 0xffff
#define TARGET_IO_PORTB 0xffff
@ -184,4 +185,3 @@
#define USABLE_TIMER_CHANNEL_COUNT 6
#define USED_TIMERS ( TIM_N(2) | TIM_N(3) | TIM_N(5) | TIM_N(8) )

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

@ -24,6 +24,7 @@
#define USBD_PRODUCT_STRING "FuryF7"
#define USE_DSHOT
#define USE_ESC_TELEMETRY
#define LED0 PB5
#define LED1 PB4

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

@ -47,6 +47,7 @@
#define USE_FLASH_M25P16
#define USE_DSHOT
#define USE_ESC_TELEMETRY
#define REMAP_TIM17_DMA
#define USE_VCP
@ -105,4 +106,3 @@
#define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(3)|BIT(4))
#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(16) |TIM_N(17))

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

@ -24,6 +24,10 @@
#define SBUS_PORT_OPTIONS (SERIAL_STOPBITS_2 | SERIAL_PARITY_EVEN | SERIAL_INVERTED | SERIAL_BIDIR)
#define USE_DSHOT
#define USE_ESC_TELEMETRY
#define USE_ESCSERIAL
#define ESCSERIAL_TIMER_TX_HARDWARE 6
#define LED0 PB1

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

@ -47,6 +47,7 @@
#define ENSURE_MPU_DATA_READY_IS_LOW
#define USE_DSHOT
#define USE_ESC_TELEMETRY
#define USE_SPI
#define USE_SPI_DEVICE_1
@ -173,4 +174,3 @@
#define USABLE_TIMER_CHANNEL_COUNT 12
#endif
#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(8) | TIM_N(15))

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

@ -103,6 +103,7 @@
#define LED_STRIP
#define USE_DSHOT
#define USE_ESC_TELEMETRY
#define SPEKTRUM_BIND
// USART2, PB4
@ -120,4 +121,3 @@
#define USABLE_TIMER_CHANNEL_COUNT 10
#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(15) | TIM_N(17))

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

@ -106,6 +106,7 @@
#define RSSI_ADC_PIN PB2
#define USE_DSHOT
#define USE_ESC_TELEMETRY
#define REMAP_TIM17_DMA
// UART1 TX uses DMA1_Channel4, which is also used by dshot on motor 4

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

@ -138,6 +138,7 @@
#define SDCARD_SPI_FULL_SPEED_CLOCK_DIVIDER 2
#define USE_DSHOT
#define USE_ESC_TELEMETRY
// DSHOT output 4 uses DMA1_Channel5, so don't use it for the SDCARD until we find an alternative
#ifndef USE_DSHOT

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

@ -123,6 +123,7 @@
//#define RSSI_ADC_PIN PA0
#define USE_DSHOT
#define USE_ESC_TELEMETRY
#define LED_STRIP

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

@ -38,6 +38,7 @@
#endif
#define USE_DSHOT
#define USE_ESC_TELEMETRY
#define LED0 PB5
// Disable LED1, conflicts with AirbotF4/Flip32F4/Revolt beeper

1
src/main/target/SOULF4/target.h
View File

@ -105,6 +105,7 @@
#define LED_STRIP
#define USE_DSHOT
#define USE_ESC_TELEMETRY
#define SPEKTRUM_BIND
// USART3,

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

@ -36,6 +36,7 @@
#define INVERTER_USART USART6
#define USE_DSHOT
#define USE_ESC_TELEMETRY
// MPU9250 interrupt
#define USE_EXTI
@ -132,4 +133,3 @@
#define USABLE_TIMER_CHANNEL_COUNT 11
#define USED_TIMERS ( TIM_N(2) | TIM_N(3) | TIM_N(5) | TIM_N(12) | TIM_N(8) | TIM_N(9))

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

@ -116,6 +116,7 @@
#define RSSI_ADC_PIN PB2
#define USE_DSHOT
#define USE_ESC_TELEMETRY
#define REMAP_TIM17_DMA
// UART1 TX uses DMA1_Channel4, which is also used by dshot on motor 4

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

@ -36,6 +36,7 @@
#define ENSURE_MAG_DATA_READY_IS_HIGH
#define USE_DSHOT
#define USE_ESC_TELEMETRY
#define GYRO
#define USE_GYRO_SPI_MPU6500
@ -156,4 +157,3 @@
#define USABLE_TIMER_CHANNEL_COUNT 12 // PPM, 8 PWM, UART3 RX/TX, LED Strip
#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(8) | TIM_N(15))

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

@ -170,6 +170,7 @@
#define EXTERNAL1_ADC_PIN PC3
#define USE_DSHOT
#define USE_ESC_TELEMETRY
#define LED_STRIP
@ -192,4 +193,3 @@
#define USABLE_TIMER_CHANNEL_COUNT 14
#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(8) | TIM_N(16) | TIM_N(17))

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

@ -102,6 +102,8 @@
#define RSSI_ADC_PIN PB2
#define USE_DSHOT
#define USE_ESC_TELEMETRY
#define REMAP_TIM17_DMA
// UART1 TX uses DMA1_Channel4, which is also used by dshot on motor 4
#if defined(USE_UART1_TX_DMA) && defined(USE_DSHOT)
@ -129,4 +131,3 @@
#define USABLE_TIMER_CHANNEL_COUNT 17
#define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(15) | TIM_N(16) | TIM_N(17) )

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

@ -0,0 +1,312 @@
#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;
masterConfig.batteryConfig.currentMeterType = CURRENT_SENSOR_ESC;
masterConfig.batteryConfig.batteryMeterType = BATTERY_SENSOR_ESC;
}
return escTelemetryPort != NULL;
}
void freeEscTelemetryPort(void)
{
closeSerialPort(escTelemetryPort);
escTelemetryPort = NULL;
escTelemetryEnabled = false;
}
// Receive ISR callback
static void escTelemetryDataReceive(uint16_t c)
{
// KISS ESC sends some data during startup, ignore this for now (maybe future use)
// startup data could be firmware version and serialnumber
if (escTelemetryTriggerState == ESC_TLM_TRIGGER_WAIT) return;
tlm[tlmFramePosition] = (uint8_t)c;
if (tlmFramePosition == ESC_TLM_BUFFSIZE - 1) {
tlmFrameDone = true;
tlmFramePosition = 0;
} else {
tlmFramePosition++;
}
}
uint8_t escTelemetryFrameStatus(void)
{
uint8_t frameStatus = ESC_TLM_FRAME_PENDING;
uint16_t chksum, tlmsum;
if (!tlmFrameDone) {
return frameStatus;
}
tlmFrameDone = false;
// Get CRC8 checksum
chksum = get_crc8(tlm, ESC_TLM_BUFFSIZE - 1);
tlmsum = tlm[ESC_TLM_BUFFSIZE - 1]; // last byte contains CRC value
if (chksum == tlmsum) {
escTelemetryData[escTelemetryMotor].skipped = false;
escTelemetryData[escTelemetryMotor].temperature = tlm[0];
escTelemetryData[escTelemetryMotor].voltage = tlm[1] << 8 | tlm[2];
escTelemetryData[escTelemetryMotor].current = tlm[3] << 8 | tlm[4];
escTelemetryData[escTelemetryMotor].consumption = tlm[5] << 8 | tlm[6];
escTelemetryData[escTelemetryMotor].rpm = tlm[7] << 8 | tlm[8];
frameStatus = ESC_TLM_FRAME_COMPLETE;
}
return frameStatus;
}
void escTelemetryProcess(uint32_t currentTime)
{
uint32_t currentTimeMs = currentTime / 1000;
if (!escTelemetryEnabled) {
return;
}
// Wait period of time before requesting telemetry (let the system boot first)
if (millis() < ESC_BOOTTIME) {
return;
}
else if (escTelemetryTriggerState == ESC_TLM_TRIGGER_WAIT) {
// Ready for starting requesting telemetry
escTelemetryTriggerState = ESC_TLM_TRIGGER_READY;
escTelemetryMotor = 0;
escTriggerTimestamp = currentTimeMs;
escTriggerLastTimestamp = escTriggerTimestamp;
}
else if (escTelemetryTriggerState == ESC_TLM_TRIGGER_READY) {
if (debugMode == DEBUG_ESC_TELEMETRY) debug[0] = escTelemetryMotor+1;
motorDmaOutput_t * const motor = getMotorDmaOutput(escTelemetryMotor);
motor->requestTelemetry = true;
escTelemetryTriggerState = ESC_TLM_TRIGGER_PENDING;
}
if (escTriggerTimestamp + ESC_REQUEST_TIMEOUT < currentTimeMs) {
// ESC did not repond in time, retry
timeoutRetryCount++;
escTriggerTimestamp = currentTimeMs;
escTelemetryTriggerState = ESC_TLM_TRIGGER_READY;
if (timeoutRetryCount == 4) {
// Not responding after 3 times, skip motor
escTelemetryData[escTelemetryMotor].skipped = true;
selectNextMotor();
}
if (debugMode == DEBUG_ESC_TELEMETRY) debug[1]++;
}
// Get received frame status
uint8_t state = escTelemetryFrameStatus();
if (state == ESC_TLM_FRAME_COMPLETE) {
// Wait until all ESCs are processed
if (escTelemetryMotor == getMotorCount()-1) {
escCurrent = 0;
escConsumption = 0;
escVbat = 0;
for (int i = 0; i < getMotorCount(); i++) {
if (!escTelemetryData[i].skipped) {
escVbat = i > 0 ? ((escVbat + escTelemetryData[i].voltage) / 2) : escTelemetryData[i].voltage;
escCurrent = escCurrent + escTelemetryData[i].current;
escConsumption = escConsumption + escTelemetryData[i].consumption;
}
}
}
if (debugMode == DEBUG_ESC_TELEMETRY) debug[2] = escVbat;
if (debugMode == DEBUG_ESC_TELEMETRY) debug[3] = escCurrent;
selectNextMotor();
escTelemetryTriggerState = ESC_TLM_TRIGGER_READY;
}
if (escTriggerLastTimestamp + 10000 < currentTimeMs) {
// ESCs did not respond for 10 seconds
// Disable ESC telemetry and fallback to onboard vbat sensor
freeEscTelemetryPort();
escVbat = 0;
escCurrent = 0;
escConsumption = 0;
}
}
static void selectNextMotor(void)
{
escTelemetryMotor++;
if (escTelemetryMotor == getMotorCount()) {
escTelemetryMotor = 0;
}
escTriggerTimestamp = millis();
escTriggerLastTimestamp = escTriggerTimestamp;
}
//-- CRC
static uint8_t update_crc8(uint8_t crc, uint8_t crc_seed)
{
uint8_t crc_u = crc;
crc_u ^= crc_seed;
for (int i=0; i<8; i++) {
crc_u = ( crc_u & 0x80 ) ? 0x7 ^ ( crc_u << 1 ) : ( crc_u << 1 );
}
return (crc_u);
}
static uint8_t get_crc8(uint8_t *Buf, uint8_t BufLen)
{
uint8_t crc = 0;
for(int i=0; i<BufLen; i++) crc = update_crc8(Buf[i], crc);
return (crc);
}
#endif

10
src/main/telemetry/esc_telemetry.h Normal file
View File

@ -0,0 +1,10 @@
#pragma once
uint8_t escTelemetryFrameStatus(void);
bool escTelemetryInit(void);
bool isEscTelemetryActive(void);
int16_t getEscTelemetryVbat(void);
int16_t getEscTelemetryCurrent(void);
int16_t getEscTelemetryConsumption(void);
void escTelemetryProcess(uint32_t currentTime);

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) && batteryCellCount > 0) {
if ((feature(FEATURE_VBAT) || feature(FEATURE_ESC_TELEMETRY)) && 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)) {
if (feature(FEATURE_CURRENT_METER) || feature(FEATURE_ESC_TELEMETRY)) {
smartPortSendPackage(id, amperage / 10); // given in 10mA steps, unknown requested unit
smartPortHasRequest = 0;
}
@ -659,7 +659,7 @@ void handleSmartPortTelemetry(void)
}
break;
case FSSP_DATAID_FUEL :
if (feature(FEATURE_CURRENT_METER)) {
if (feature(FEATURE_CURRENT_METER) || feature(FEATURE_ESC_TELEMETRY)) {
smartPortSendPackage(id, mAhDrawn); // given in mAh, unknown requested unit
smartPortHasRequest = 0;
}