From ca8359c489d3afa9a6dfb8d156741f9aa7b4a3f7 Mon Sep 17 00:00:00 2001 From: Michael Date: Tue, 4 Apr 2023 08:46:38 +0200 Subject: [PATCH] Additional serial protocol: Graupner HoTT SUMD (#2137) * initial commit * editorial changes * removed unnecessary extern definitions and include * forgotten items * don't occupy RX if SUMD (as in SBUS) setting SUMD Failsafe bit is not necessary as it is handled by !frameAvailable * CRC: changed from using crc table to polynomial calculation plus some editorial changes * follwing @pkendall64's review: - removed not necessary changes - moved defines to .cpp file * following @Jye's review changed crc calculation to use existing ExpressLRS class --- src/html/index.html | 1 + src/html/scan.js | 4 ++ src/include/common.h | 3 +- src/lib/CONFIG/config.h | 4 +- src/lib/LUA/rx_devLUA.cpp | 2 +- src/lib/OPTIONS/options.cpp | 2 + src/src/rx-serial/SerialSUMD.cpp | 99 ++++++++++++++++++++++++++++++++++++++++ src/src/rx-serial/SerialSUMD.h | 22 +++++++++ src/src/rx_main.cpp | 13 +++++- 9 files changed, 145 insertions(+), 5 deletions(-) create mode 100644 src/src/rx-serial/SerialSUMD.cpp create mode 100644 src/src/rx-serial/SerialSUMD.h diff --git a/src/html/index.html b/src/html/index.html index 8142571c..a6b90f3c 100644 --- a/src/html/index.html +++ b/src/html/index.html @@ -200,6 +200,7 @@ + diff --git a/src/html/scan.js b/src/html/scan.js index d5b8e5b6..b693bc0b 100644 --- a/src/html/scan.js +++ b/src/html/scan.js @@ -230,6 +230,10 @@ function updateConfig(data) { _('rcvr-uart-baud').disabled = false; _('rcvr-uart-baud').value = '420000'; } + if (_('serial-protocol').value == 4) { + _('rcvr-uart-baud').disabled = true; + _('rcvr-uart-baud').value = '115200'; + } else { _('rcvr-uart-baud').disabled = true; _('rcvr-uart-baud').value = '100000'; diff --git a/src/include/common.h b/src/include/common.h index ece40fb5..e9ed8d87 100644 --- a/src/include/common.h +++ b/src/include/common.h @@ -184,7 +184,8 @@ enum eSerialProtocol : uint8_t PROTOCOL_CRSF, PROTOCOL_INVERTED_CRSF, PROTOCOL_SBUS, - PROTOCOL_INVERTED_SBUS + PROTOCOL_INVERTED_SBUS, + PROTOCOL_SUMD }; #ifndef UNIT_TEST diff --git a/src/lib/CONFIG/config.h b/src/lib/CONFIG/config.h index d31563ea..fc446496 100644 --- a/src/lib/CONFIG/config.h +++ b/src/lib/CONFIG/config.h @@ -174,8 +174,8 @@ typedef struct { forceTlmOff:1, rateInitialIdx:4; // Rate to start rateCycling at on boot uint8_t modelId; - uint8_t serialProtocol:2, - unused:6; + uint8_t serialProtocol:3, + unused:5; rx_config_pwm_t pwmChannels[PWM_MAX_CHANNELS]; } rx_config_t; diff --git a/src/lib/LUA/rx_devLUA.cpp b/src/lib/LUA/rx_devLUA.cpp index 34cc5825..9d1ef838 100644 --- a/src/lib/LUA/rx_devLUA.cpp +++ b/src/lib/LUA/rx_devLUA.cpp @@ -18,7 +18,7 @@ static const char *rxModes = "50Hz;60Hz;100Hz;160Hz;333Hz;400Hz;10kHzDuty;On/Off static struct luaItem_selection luaSerialProtocol = { {"Protocol", CRSF_TEXT_SELECTION}, 0, // value - "CRSF;Inverted CRSF;SBUS;Inverted SBUS", + "CRSF;Inverted CRSF;SBUS;Inverted SBUS;SUMD", STR_EMPTYSPACE }; diff --git a/src/lib/OPTIONS/options.cpp b/src/lib/OPTIONS/options.cpp index bf8e3c87..c8038db5 100644 --- a/src/lib/OPTIONS/options.cpp +++ b/src/lib/OPTIONS/options.cpp @@ -80,6 +80,8 @@ __attribute__ ((used)) const firmware_options_t firmwareOptions = { .uart_baud = USE_AIRPORT_AT_BAUD, #elif defined(USE_SBUS_PROTOCOL) .uart_baud = 100000, +#elif defined(USE_SUMD_PROTOCOL) + .uart_baud = 115200, #elif defined(RCVR_UART_BAUD) .uart_baud = RCVR_UART_BAUD, #else diff --git a/src/src/rx-serial/SerialSUMD.cpp b/src/src/rx-serial/SerialSUMD.cpp new file mode 100644 index 00000000..d12ec39e --- /dev/null +++ b/src/src/rx-serial/SerialSUMD.cpp @@ -0,0 +1,99 @@ +#include "SerialSUMD.h" +#include "CRSF.h" +#include "device.h" + +#define SUMD_HEADER_SIZE 3 // 3 Bytes header +#define SUMD_DATA_SIZE_16CH (16*2) // 2 Bytes per channel +#define SUMD_CRC_SIZE 2 // 16 bit CRC +#define SUMD_FRAME_16CH_LEN (SUMD_HEADER_SIZE+SUMD_DATA_SIZE_16CH+SUMD_CRC_SIZE) + +void SerialSUMD::setLinkQualityStats(uint16_t lq, uint16_t rssi) +{ + linkQuality = lq; + rssiDBM = rssi; +} + +uint32_t SerialSUMD::sendRCFrameToFC(bool frameAvailable, uint32_t *channelData) +{ + if (!frameAvailable) { + return DURATION_IMMEDIATELY; + } + + uint8_t outBuffer[SUMD_FRAME_16CH_LEN]; + + outBuffer[0] = 0xA8; //Graupner + outBuffer[1] = 0x01; //SUMD + outBuffer[2] = 0x10; //16CH + + uint16_t us = (CRSF_to_US(ChannelData[0]) << 3); + outBuffer[3] = us >> 8; + outBuffer[4] = us & 0x00ff; + us = (CRSF_to_US(ChannelData[1]) << 3); + outBuffer[5] = us >> 8; + outBuffer[6] = us & 0x00ff; + us = (CRSF_to_US(ChannelData[2]) << 3); + outBuffer[7] = us >> 8; + outBuffer[8] = us & 0x00ff; + us = (CRSF_to_US(ChannelData[3]) << 3); + outBuffer[9] = us >> 8; + outBuffer[10] = us & 0x00ff; + us = (CRSF_to_US(ChannelData[7]) << 3); //channel 8 mapped to 5 to move arm channel away from the aileron function + outBuffer[11] = us >> 8; + outBuffer[12] = us & 0x00ff; + us = (CRSF_to_US(ChannelData[5]) << 3); + outBuffer[13] = us >> 8; + outBuffer[14] = us & 0x00ff; + us = (CRSF_to_US(ChannelData[6]) << 3); + outBuffer[15] = us >> 8; + outBuffer[16] = us & 0x00ff; + us = (CRSF_to_US(ChannelData[4]) << 3); //channel 5 mapped to 8 + outBuffer[17] = us >> 8; + outBuffer[18] = us & 0x00ff; + us = (CRSF_to_US(ChannelData[8]) << 3); + outBuffer[19] = us >> 8; + outBuffer[20] = us & 0x00ff; + us = (CRSF_to_US(ChannelData[9]) << 3); + outBuffer[21] = us >> 8; + outBuffer[22] = us & 0x00ff; + us = (CRSF_to_US(ChannelData[10]) << 3); + outBuffer[23] = us >> 8; + outBuffer[24] = us & 0x00ff; + us = (CRSF_to_US(ChannelData[11]) << 3); + outBuffer[25] = us >> 8; + outBuffer[26] = us & 0x00ff; + us = (CRSF_to_US(ChannelData[12]) << 3); + outBuffer[27] = us >> 8; + outBuffer[28] = us & 0x00ff; + us = (CRSF_to_US(ChannelData[13]) << 3); + outBuffer[29] = us >> 8; + outBuffer[30] = us & 0x00ff; + us = (CRSF_to_US(ChannelData[14]) << 3); + outBuffer[31] = us >> 8; + outBuffer[32] = us & 0x00ff; + us = (CRSF_to_US(ChannelData[15]) << 3); + outBuffer[33] = us >> 8; + outBuffer[34] = us & 0x00ff; + + uint16_t crc = crc2Byte.calc(outBuffer, (SUMD_HEADER_SIZE + SUMD_DATA_SIZE_16CH), 0); + outBuffer[35] = (uint8_t)(crc >> 8); + outBuffer[36] = (uint8_t)(crc & 0x00ff); + + _outputPort->write(outBuffer, sizeof(outBuffer)); + + return DURATION_IMMEDIATELY; +} + +void SerialSUMD::sendLinkStatisticsToFC() +{ + // unsupported +} + +void SerialSUMD::sendMSPFrameToFC(uint8_t* data) +{ + (void)data; + // unsupported +} + +void processByte(uint8_t byte) { + // unsupported +} \ No newline at end of file diff --git a/src/src/rx-serial/SerialSUMD.h b/src/src/rx-serial/SerialSUMD.h new file mode 100644 index 00000000..20e542be --- /dev/null +++ b/src/src/rx-serial/SerialSUMD.h @@ -0,0 +1,22 @@ +#include "SerialIO.h" +#include "crc.h" + +class SerialSUMD : public SerialIO { +public: + explicit SerialSUMD(Stream &out, Stream &in) : SerialIO(&out, &in) { crc2Byte.init(16, 0x1021); } + + virtual ~SerialSUMD() {} + + Crc2Byte crc2Byte; + + void setLinkQualityStats(uint16_t lq, uint16_t rssi) override; + uint32_t sendRCFrameToFC(bool frameAvailable, uint32_t *channelData) override; + void sendMSPFrameToFC(uint8_t* data) override; + void sendLinkStatisticsToFC() override; + +private: + uint16_t linkQuality = 0; + uint16_t rssiDBM = 0; + + void processByte(uint8_t byte) override {}; +}; diff --git a/src/src/rx_main.cpp b/src/src/rx_main.cpp index ef23b87d..5318f7a9 100644 --- a/src/src/rx_main.cpp +++ b/src/src/rx_main.cpp @@ -18,6 +18,7 @@ #include "rx-serial/SerialNOOP.h" #include "rx-serial/SerialCRSF.h" #include "rx-serial/SerialSBUS.h" +#include "rx-serial/SerialSUMD.h" #include "rx-serial/SerialAirPort.h" #include "rx-serial/devSerialIO.h" @@ -1138,6 +1139,7 @@ void MspReceiveComplete() static void setupSerial() { bool sbusSerialOutput = false; + bool sumdSerialOutput = false; if (OPT_CRSF_RCVR_NO_SERIAL) { @@ -1157,6 +1159,11 @@ static void setupSerial() sbusSerialOutput = true; serialBaud = 100000; } + else if (config.GetSerialProtocol() == PROTOCOL_SUMD) + { + sumdSerialOutput = true; + serialBaud = 115200; + } bool invert = config.GetSerialProtocol() == PROTOCOL_SBUS || config.GetSerialProtocol() == PROTOCOL_INVERTED_CRSF; #ifdef PLATFORM_STM32 @@ -1212,7 +1219,7 @@ static void setupSerial() #if defined(PLATFORM_ESP8266) SerialConfig config = sbusSerialOutput ? SERIAL_8E2 : SERIAL_8N1; - SerialMode mode = sbusSerialOutput ? SERIAL_TX_ONLY : SERIAL_FULL; + SerialMode mode = (sbusSerialOutput || sumdSerialOutput) ? SERIAL_TX_ONLY : SERIAL_FULL; Serial.begin(serialBaud, config, mode, -1, invert); #elif defined(PLATFORM_ESP32) uint32_t config = sbusSerialOutput ? SERIAL_8E2 : SERIAL_8N1; @@ -1227,6 +1234,10 @@ static void setupSerial() { serialIO = new SerialSBUS(SERIAL_PROTOCOL_TX, SERIAL_PROTOCOL_RX); } + else if (sumdSerialOutput) + { + serialIO = new SerialSUMD(SERIAL_PROTOCOL_TX, SERIAL_PROTOCOL_RX); + } else { serialIO = new SerialCRSF(SERIAL_PROTOCOL_TX, SERIAL_PROTOCOL_RX); -- 2.11.4.GIT