1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-19 06:15:16 +03:00
betaflight/src/main/sensors/esc_sensor.c

313 lines
8.1 KiB
C

#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