mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-20 23:05:19 +03:00
Moved ESC telemetry to sensors
This commit is contained in:
Bas Delfos
parent
aa1ae88ddf
commit
b53cbbefc3
33 changed files with 154 additions and 158 deletions
310
src/main/sensors/esc_sensor.c
Normal file
310
src/main/sensors/esc_sensor.c
Normal file
|
|
@ -0,0 +1,310 @@
|
|||
#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 escTriggerLastTimestamp = -1;
|
||||
static uint8_t timeoutRetryCount = 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_TELEMETRY_ESC);
|
||||
if (!portConfig) {
|
||||
return false;
|
||||
}
|
||||
|
||||
portOptions_t options = (SERIAL_NOT_INVERTED);
|
||||
|
||||
// Initialize serial port
|
||||
escSensorPort = openSerialPort(portConfig->identifier, FUNCTION_TELEMETRY_ESC, 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 (millis() < ESC_BOOTTIME) {
|
||||
return;
|
||||
}
|
||||
else if (escSensorTriggerState == ESC_SENSOR_TRIGGER_WAIT) {
|
||||
// Ready for starting requesting telemetry
|
||||
escSensorTriggerState = ESC_SENSOR_TRIGGER_READY;
|
||||
escSensorMotor = 0;
|
||||
escTriggerTimestamp = currentTimeMs;
|
||||
escTriggerLastTimestamp = escTriggerTimestamp;
|
||||
}
|
||||
else if (escSensorTriggerState == ESC_SENSOR_TRIGGER_READY) {
|
||||
if (debugMode == DEBUG_ESC_SENSOR) debug[0] = escSensorMotor+1;
|
||||
|
||||
motorDmaOutput_t * const motor = getMotorDmaOutput(escSensorMotor);
|
||||
motor->requestTelemetry = true;
|
||||
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();
|
||||
}
|
||||
|
||||
if (debugMode == DEBUG_ESC_SENSOR) debug[1]++;
|
||||
}
|
||||
|
||||
// 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;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (debugMode == DEBUG_ESC_SENSOR) debug[2] = escVbat;
|
||||
if (debugMode == DEBUG_ESC_SENSOR) debug[3] = escCurrent;
|
||||
|
||||
selectNextMotor();
|
||||
escSensorTriggerState = ESC_SENSOR_TRIGGER_READY;
|
||||
}
|
||||
|
||||
if (escTriggerLastTimestamp + 10000 < currentTimeMs) {
|
||||
// ESCs did not respond for 10 seconds
|
||||
// Disable ESC telemetry and fallback to onboard vbat sensor
|
||||
freeEscSensorPort();
|
||||
escVbat = 0;
|
||||
escCurrent = 0;
|
||||
escConsumption = 0;
|
||||
}
|
||||
}
|
||||
|
||||
static void selectNextMotor(void)
|
||||
{
|
||||
escSensorMotor++;
|
||||
if (escSensorMotor == getMotorCount()) {
|
||||
escSensorMotor = 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
|
||||
Loading…
Add table
Add a link
Reference in a new issue