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

Merge pull request #3474 from mikeller/add_esc_version_info 

Added display of (KISS) ESC version info for 'dshotprog <ESC index> 6'.
This commit is contained in:
Michael Keller 2017-07-11 21:16:56 +12:00 committed by GitHub
parent 7366901b2e 613e5c3b2a
commit de6b4ff62a
3 changed files with 163 additions and 37 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

128
src/main/fc/cli.c
View File

@ -120,6 +120,7 @@ extern uint8_t __config_end;
#include "sensors/battery.h"
#include "sensors/boardalignment.h"
#include "sensors/compass.h"
#include "sensors/esc_sensor.h"
#include "sensors/gyro.h"
#include "sensors/sensors.h"
@ -877,12 +878,12 @@ static void cliSerialPassthrough(char *cmdline)
tok = strtok_r(NULL, " ", &saveptr);
}
tfp_printf("Port %d ", id);
cliPrintf("Port %d ", id);
serialPort_t *passThroughPort;
serialPortUsage_t *passThroughPortUsage = findSerialPortUsageByIdentifier(id);
if (!passThroughPortUsage || passThroughPortUsage->serialPort == NULL) {
if (!baud) {
tfp_printf("closed, specify baud.\r\n");
cliPrint("closed, specify baud.\r\n");
return;
}
if (!mode)
@ -892,17 +893,17 @@ static void cliSerialPassthrough(char *cmdline)
baud, mode,
SERIAL_NOT_INVERTED);
if (!passThroughPort) {
tfp_printf("could not be opened.\r\n");
cliPrint("could not be opened.\r\n");
return;
}
tfp_printf("opened, baud = %d.\r\n", baud);
cliPrintf("opened, baud = %d.\r\n", baud);
} else {
passThroughPort = passThroughPortUsage->serialPort;
// If the user supplied a mode, override the port's mode, otherwise
// leave the mode unchanged. serialPassthrough() handles one-way ports.
tfp_printf("already open.\r\n");
cliPrint("already open.\r\n");
if (mode && passThroughPort->mode != mode) {
tfp_printf("mode changed from %d to %d.\r\n",
cliPrintf("mode changed from %d to %d.\r\n",
passThroughPort->mode, mode);
serialSetMode(passThroughPort, mode);
}
@ -913,7 +914,7 @@ static void cliSerialPassthrough(char *cmdline)
}
}
tfp_printf("forwarding, power cycle to exit.\r\n");
cliPrint("Forwarding, power cycle to exit.\r\n");
serialPassthrough(cliPort, passThroughPort, NULL, NULL);
}
@ -2212,6 +2213,98 @@ static int parseOutputIndex(char *pch, bool allowAllEscs) {
}
#ifdef USE_DSHOT
#define ESC_INFO_V1_EXPECTED_FRAME_SIZE 15
#define ESC_INFO_V2_EXPECTED_FRAME_SIZE 21
#define ESC_INFO_VERSION_POSITION 12
void printEscInfo(const uint8_t *escInfoBytes, uint8_t bytesRead)
{
bool escInfoReceived = false;
if (bytesRead > ESC_INFO_VERSION_POSITION) {
uint8_t escInfoVersion = 0;
uint8_t frameLength = 0;
if (escInfoBytes[ESC_INFO_VERSION_POSITION] == 255) {
escInfoVersion = 2;
frameLength = ESC_INFO_V2_EXPECTED_FRAME_SIZE;
} else {
escInfoVersion = 1;
frameLength = ESC_INFO_V1_EXPECTED_FRAME_SIZE;
}
if (((escInfoVersion == 1) && (bytesRead == ESC_INFO_V1_EXPECTED_FRAME_SIZE))
|| ((escInfoVersion == 2) && (bytesRead == ESC_INFO_V2_EXPECTED_FRAME_SIZE))) {
escInfoReceived = true;
if (calculateCrc8(escInfoBytes, frameLength - 1) == escInfoBytes[frameLength - 1]) {
uint8_t firmwareVersion;
char firmwareSubVersion;
uint8_t escType;
switch (escInfoVersion) {
case 1:
firmwareVersion = escInfoBytes[12];
firmwareSubVersion = (char)((escInfoBytes[13] & 0x1f) + 97);
escType = (escInfoBytes[13] & 0xe0) >> 5;
break;
case 2:
firmwareVersion = escInfoBytes[13];
firmwareSubVersion = (char)escInfoBytes[14];
escType = escInfoBytes[15];
break;
}
cliPrint("ESC: ");
switch (escType) {
case 1:
cliPrintLine("KISS8A");
break;
case 2:
cliPrintLine("KISS16A");
break;
case 3:
cliPrintLine("KISS24A");
break;
case 5:
cliPrintLine("KISS Ultralite");
break;
default:
cliPrintLine("unknown");
break;
}
cliPrint("MCU: 0x");
for (int i = 0; i < 12; i++) {
if (i && (i % 3 == 0)) {
cliPrint("-");
}
cliPrintf("%02x", escInfoBytes[i]);
}
cliPrintLinefeed();
cliPrintLinef("Firmware: %d.%02d%c", firmwareVersion / 100, firmwareVersion % 100, firmwareSubVersion);
if (escInfoVersion == 2) {
cliPrintLinef("Rotation: %s", escInfoBytes[16] ? "reversed" : "normal");
cliPrintLinef("3D: %s", escInfoBytes[17] ? "on" : "off");
}
} else {
cliPrint("Checksum error.");
}
}
}
if (!escInfoReceived) {
cliPrint("No info.");
}
}
static void cliDshotProg(char *cmdline)
{
if (isEmpty(cmdline) || motorConfig()->dev.motorPwmProtocol < PWM_TYPE_DSHOT150) {
@ -2242,17 +2335,32 @@ static void cliDshotProg(char *cmdline)
for (unsigned i = 0; i < getMotorCount(); i++) {
pwmWriteDshotCommand(i, command);
}
cliPrintf("Command %d written.\r\n", command);
} else {
uint8_t escInfoBuffer[ESC_INFO_V2_EXPECTED_FRAME_SIZE];
if (command == DSHOT_CMD_ESC_INFO) {
delay(10); // Wait for potential ESC telemetry transmission to finish
startEscDataRead(escInfoBuffer, ESC_INFO_V2_EXPECTED_FRAME_SIZE);
}
pwmWriteDshotCommand(escIndex, command);
if (command == DSHOT_CMD_ESC_INFO) {
delay(10);
printEscInfo(escInfoBuffer, getNumberEscBytesRead());
} else {
cliPrintf("Command %d written.\r\n", command);
}
}
if (command <= 5) {
delay(10); // wait for sound output to finish
}
tfp_printf("Command %d written.\r\n", command);
} else {
tfp_printf("Invalid command, range 1 to %d.\r\n", DSHOT_MIN_THROTTLE - 1);
cliPrintf("Invalid command, range 1 to %d.\r\n", DSHOT_MIN_THROTTLE - 1);
}
break;

68
src/main/sensors/esc_sensor.c
View File

@ -97,13 +97,15 @@ typedef enum {
} 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 volatile bool tlmFramePending = false;
static uint8_t tlm[ESC_SENSOR_BUFFSIZE] = { 0, };
static uint8_t tlmFramePosition = 0;
#define TELEMETRY_FRAME_SIZE 10
static uint8_t telemetryBuffer[TELEMETRY_FRAME_SIZE] = { 0, };
static volatile uint8_t *buffer;
static volatile uint8_t bufferSize = 0;
static volatile uint8_t bufferPosition = 0;
static serialPort_t *escSensorPort = NULL;
@ -119,6 +121,23 @@ static bool combinedDataNeedsUpdate = true;
static uint16_t totalTimeoutCount = 0;
static uint16_t totalCrcErrorCount = 0;
void startEscDataRead(uint8_t *frameBuffer, uint8_t frameLength)
{
buffer = frameBuffer;
bufferPosition = 0;
bufferSize = frameLength;
}
uint8_t getNumberEscBytesRead(void)
{
return bufferPosition;
}
static bool isFrameComplete(void)
{
return bufferPosition == bufferSize;
}
bool isEscSensorActive(void)
{
return escSensorPort != NULL;
@ -166,19 +185,11 @@ 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 (!tlmFramePending) {
if (isFrameComplete()) {
return;
}
tlm[tlmFramePosition] = (uint8_t)c;
if (tlmFramePosition == ESC_SENSOR_BUFFSIZE - 1) {
tlmFramePosition = 0;
tlmFramePending = false;
} else {
tlmFramePosition++;
}
buffer[bufferPosition++] = (uint8_t)c;
}
bool escSensorInit(void)
@ -200,7 +211,7 @@ bool escSensorInit(void)
return escSensorPort != NULL;
}
static uint8_t update_crc8(uint8_t crc, uint8_t crc_seed)
static uint8_t updateCrc8(uint8_t crc, uint8_t crc_seed)
{
uint8_t crc_u = crc;
crc_u ^= crc_seed;
@ -212,30 +223,33 @@ static uint8_t update_crc8(uint8_t crc, uint8_t crc_seed)
return (crc_u);
}
static uint8_t get_crc8(uint8_t *Buf, uint8_t BufLen)
uint8_t calculateCrc8(const uint8_t *Buf, const uint8_t BufLen)
{
uint8_t crc = 0;
for (int i=0; i<BufLen; i++) crc = update_crc8(Buf[i], crc);
return (crc);
for (int i = 0; i < BufLen; i++) {
crc = updateCrc8(Buf[i], crc);
}
return crc;
}
static uint8_t decodeEscFrame(void)
{
if (tlmFramePending) {
if (!isFrameComplete()) {
return ESC_SENSOR_FRAME_PENDING;
}
// Get CRC8 checksum
uint16_t chksum = get_crc8(tlm, ESC_SENSOR_BUFFSIZE - 1);
uint16_t tlmsum = tlm[ESC_SENSOR_BUFFSIZE - 1]; // last byte contains CRC value
uint16_t chksum = calculateCrc8(telemetryBuffer, TELEMETRY_FRAME_SIZE - 1);
uint16_t tlmsum = telemetryBuffer[TELEMETRY_FRAME_SIZE - 1]; // last byte contains CRC value
uint8_t frameStatus;
if (chksum == tlmsum) {
escSensorData[escSensorMotor].dataAge = 0;
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];
escSensorData[escSensorMotor].temperature = telemetryBuffer[0];
escSensorData[escSensorMotor].voltage = telemetryBuffer[1] << 8 | telemetryBuffer[2];
escSensorData[escSensorMotor].current = telemetryBuffer[3] << 8 | telemetryBuffer[4];
escSensorData[escSensorMotor].consumption = telemetryBuffer[5] << 8 | telemetryBuffer[6];
escSensorData[escSensorMotor].rpm = telemetryBuffer[7] << 8 | telemetryBuffer[8];
combinedDataNeedsUpdate = true;
@ -286,7 +300,7 @@ void escSensorProcess(timeUs_t currentTimeUs)
case ESC_SENSOR_TRIGGER_READY:
escTriggerTimestamp = currentTimeMs;
tlmFramePending = true;
startEscDataRead(telemetryBuffer, TELEMETRY_FRAME_SIZE);
motorDmaOutput_t * const motor = getMotorDmaOutput(escSensorMotor);
motor->requestTelemetry = true;
escSensorTriggerState = ESC_SENSOR_TRIGGER_PENDING;

4
src/main/sensors/esc_sensor.h
View File

@ -45,3 +45,7 @@ void escSensorProcess(timeUs_t currentTime);
escSensorData_t *getEscSensorData(uint8_t motorNumber);
void startEscDataRead(uint8_t *frameBuffer, uint8_t frameLength);
uint8_t getNumberEscBytesRead(void);
uint8_t calculateCrc8(const uint8_t *Buf, const uint8_t BufLen);