Implementation of KISS ESC Telemetry protocol
This commit is contained in:
parent
14370426af
commit
16178a0662
3
Makefile
3
Makefile
|
@ -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 = \
|
||||
|
|
|
@ -59,5 +59,6 @@ typedef enum {
|
|||
DEBUG_VELOCITY,
|
||||
DEBUG_DTERM_FILTER,
|
||||
DEBUG_ANGLERATE,
|
||||
DEBUG_ESC_TELEMETRY,
|
||||
DEBUG_COUNT
|
||||
} debugType_e;
|
||||
|
|
|
@ -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);
|
||||
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -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"
|
||||
|
@ -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",
|
||||
|
|
|
@ -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 {
|
||||
|
|
|
@ -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
|
||||
|
||||
|
|
|
@ -104,6 +104,7 @@
|
|||
#include "sensors/initialisation.h"
|
||||
|
||||
#include "telemetry/telemetry.h"
|
||||
#include "telemetry/esc_telemetry.h"
|
||||
|
||||
#include "flight/pid.h"
|
||||
#include "flight/imu.h"
|
||||
|
@ -491,6 +492,12 @@ void init(void)
|
|||
}
|
||||
#endif
|
||||
|
||||
#ifdef USE_ESC_TELEMETRY
|
||||
if (feature(FEATURE_ESC_TELEMETRY)) {
|
||||
escTelemetryInit();
|
||||
}
|
||||
#endif
|
||||
|
||||
#ifdef USB_CABLE_DETECTION
|
||||
usbCableDetectInit();
|
||||
#endif
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -35,6 +35,8 @@
|
|||
|
||||
#include "sensors/battery.h"
|
||||
|
||||
#include "telemetry/esc_telemetry.h"
|
||||
|
||||
#include "fc/rc_controls.h"
|
||||
#include "io/beeper.h"
|
||||
|
||||
|
@ -65,6 +67,10 @@ static uint16_t batteryAdcToVoltage(uint16_t src)
|
|||
|
||||
static void updateBatteryVoltage(void)
|
||||
{
|
||||
if (batteryConfig->batteryMeterType == BATTERY_SENSOR_ESC) {
|
||||
vbat = getEscTelemetryVbat();
|
||||
}
|
||||
else {
|
||||
static biquadFilter_t vbatFilter;
|
||||
static bool vbatFilterIsInitialised;
|
||||
|
||||
|
@ -79,6 +85,7 @@ static void updateBatteryVoltage(void)
|
|||
}
|
||||
vbatSample = biquadFilterApply(&vbatFilter, vbatSample);
|
||||
vbat = batteryAdcToVoltage(vbatSample);
|
||||
}
|
||||
|
||||
if (debugMode == DEBUG_BATTERY) debug[1] = vbat;
|
||||
}
|
||||
|
@ -201,11 +208,19 @@ void updateCurrentMeter(int32_t lastUpdateAt, rxConfig_t *rxConfig, uint16_t dea
|
|||
case CURRENT_SENSOR_NONE:
|
||||
amperage = 0;
|
||||
break;
|
||||
case CURRENT_SENSOR_ESC:
|
||||
amperage = getEscTelemetryCurrent();
|
||||
break;
|
||||
}
|
||||
|
||||
if (batteryConfig->currentMeterType == CURRENT_SENSOR_ESC) {
|
||||
mAhDrawn = getEscTelemetryConsumption();
|
||||
}
|
||||
else {
|
||||
mAhdrawnRaw += (amperage * lastUpdateAt) / 1000;
|
||||
mAhDrawn = mAhdrawnRaw / (3600 * 100);
|
||||
}
|
||||
}
|
||||
|
||||
float calculateVbatPidCompensation(void) {
|
||||
float batteryScaler = 1.0f;
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
||||
|
|
|
@ -0,0 +1,298 @@
|
|||
#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
|
||||
*/
|
||||
|
||||
typedef struct {
|
||||
bool skipped;
|
||||
uint8_t temperature;
|
||||
uint16_t voltage;
|
||||
uint16_t current;
|
||||
uint16_t consumption;
|
||||
uint16_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 1000 // 1 seconds
|
||||
|
||||
static bool tlmFrameDone = false;
|
||||
static bool firstCycleComplete = false;
|
||||
static uint8_t tlm[ESC_TLM_BUFFSIZE] = { 0, };
|
||||
static uint8_t tlmFramePosition = 0;
|
||||
static serialPort_t *escTelemetryPort = NULL;
|
||||
static esc_telemetry_t escTelemetryData[4];
|
||||
static uint32_t escTriggerTimestamp = -1;
|
||||
|
||||
static uint8_t escTelemetryMotor = 0; // motor index 0 - 3
|
||||
static bool escTelemetryEnabled = false;
|
||||
static escTlmTriggerState_t escTelemetryTriggerState = ESC_TLM_TRIGGER_WAIT;
|
||||
|
||||
static uint16_t escVbat = 0;
|
||||
static uint16_t escCurrent = 0;
|
||||
static uint16_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;
|
||||
}
|
||||
|
||||
uint16_t getEscTelemetryVbat(void)
|
||||
{
|
||||
return escVbat / 10;
|
||||
}
|
||||
|
||||
uint16_t getEscTelemetryCurrent(void)
|
||||
{
|
||||
return escCurrent;
|
||||
}
|
||||
|
||||
uint16_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)
|
||||
{
|
||||
UNUSED(currentTime);
|
||||
|
||||
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 = millis();
|
||||
}
|
||||
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 < millis())
|
||||
{
|
||||
// ESC did not repond in time, skip to next motor
|
||||
escTelemetryData[escTelemetryMotor].skipped = true;
|
||||
selectNextMotor();
|
||||
escTelemetryTriggerState = ESC_TLM_TRIGGER_READY;
|
||||
|
||||
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 (firstCycleComplete)
|
||||
{
|
||||
uint8_t i;
|
||||
escCurrent = 0;
|
||||
escConsumption = 0;
|
||||
for (i = 0; i < 4; i++) // Motor count for Dshot limited to 4
|
||||
{
|
||||
if (!escTelemetryData[i].skipped)
|
||||
{
|
||||
escVbat = escVbat == 0 ? escTelemetryData[i].voltage : (escVbat + escTelemetryData[i].voltage) / 2;
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
||||
static void selectNextMotor(void)
|
||||
{
|
||||
escTelemetryMotor++;
|
||||
if (escTelemetryMotor >= 4) { // Motor count for Dshot limited to 4
|
||||
escTelemetryMotor = 0;
|
||||
firstCycleComplete = true;
|
||||
}
|
||||
escTriggerTimestamp = millis();
|
||||
}
|
||||
|
||||
//-- CRC
|
||||
|
||||
static uint8_t update_crc8(uint8_t crc, uint8_t crc_seed)
|
||||
{
|
||||
uint8_t crc_u, i;
|
||||
|
||||
crc_u = crc;
|
||||
crc_u ^= crc_seed;
|
||||
|
||||
for ( 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, i;
|
||||
for( i=0; i<BufLen; i++) crc = update_crc8(Buf[i], crc);
|
||||
return (crc);
|
||||
}
|
|
@ -0,0 +1,10 @@
|
|||
#pragma once
|
||||
|
||||
uint8_t escTelemetryFrameStatus(void);
|
||||
bool escTelemetryInit(void);
|
||||
bool isEscTelemetryActive(void);
|
||||
uint16_t getEscTelemetryVbat(void);
|
||||
uint16_t getEscTelemetryCurrent(void);
|
||||
uint16_t getEscTelemetryConsumption(void);
|
||||
|
||||
void escTelemetryProcess(uint32_t currentTime);
|
|
@ -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();
|
||||
|
|
|
@ -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;
|
||||
}
|
||||
|
|
Loading…
Reference in New Issue