From 856b7ab266365a649bda6097a30e2a5bc4856274 Mon Sep 17 00:00:00 2001 From: Witty-Wizard Date: Mon, 1 Jul 2024 14:20:24 +0530 Subject: [PATCH] [Tested]: Fport Rc decoding supported --- src/SerialIO.h | 1 - src/crsf/crsf.cpp | 29 +++++++++++++++++--- src/crsf/crsf.h | 2 +- src/fport/fport.cpp | 30 ++++++++++++++++----- src/fport/fport.h | 30 ++++----------------- src/fport/fport_protocol.h | 55 ++++++++++++++++++++++++++++++++++++++ src/ibus/ibus.h | 25 +---------------- src/ibus/ibus_protocol.h | 45 +++++++++++++++++++++++++++++++ src/sbus/sbus.h | 32 +--------------------- src/sbus/sbus_protocol.h | 51 +++++++++++++++++++++++++++++++++++ 10 files changed, 208 insertions(+), 92 deletions(-) create mode 100644 src/fport/fport_protocol.h create mode 100644 src/ibus/ibus_protocol.h create mode 100644 src/sbus/sbus_protocol.h diff --git a/src/SerialIO.h b/src/SerialIO.h index 41ff9af..f497db5 100644 --- a/src/SerialIO.h +++ b/src/SerialIO.h @@ -6,7 +6,6 @@ #ifndef SerialIO_H #define SerialIO_H #include - #define PACKED __attribute__((packed)) typedef struct rc_channels_s { diff --git a/src/crsf/crsf.cpp b/src/crsf/crsf.cpp index 4e367d5..9a56657 100644 --- a/src/crsf/crsf.cpp +++ b/src/crsf/crsf.cpp @@ -1,4 +1,5 @@ #include "crsf.h" +#include crsf::crsf(Stream *rxPort, int rxPin, int txPin, bool inverted) : SerialIO(rxPort, rxPin, txPin, inverted){}; @@ -19,12 +20,32 @@ void crsf::begin() { } void crsf::processIncoming() { + uint8_t size = CRSF_MAX_PACKET_SIZE; while (_rxPort->available()) { _rxData[CRSF_MAX_PACKET_SIZE - 1] = _rxPort->read(); - if ((_rxData[0] == CRSF_ADDRESS_CRSF_TRANSMITTER || - _rxData[0] == CRSF_ADDRESS_CRSF_TRANSMITTER) && - (crc8(&_rxData[2], _rxData[1]) == 0)) { - memcpy(&_channelData, &_rxData[3], sizeof(_channelData)); + if (crc8(&_rxData[CRSF_MAX_PACKET_SIZE - size], + _rxData[CRSF_MAX_PACKET_SIZE - size - 1]) == 0) { + if ((_rxData[CRSF_MAX_PACKET_SIZE - size - 2] == + CRSF_ADDRESS_FLIGHT_CONTROLLER) || + (_rxData[CRSF_MAX_PACKET_SIZE - size - 2] == + CRSF_ADDRESS_CRSF_TRANSMITTER)) { + if (_rxData[CRSF_MAX_PACKET_SIZE - size] == + CRSF_FRAMETYPE_RC_CHANNELS_PACKED) { + memcpy(&_channelData, &_rxData[CRSF_MAX_PACKET_SIZE - size + 1], + sizeof(_channelData)); + } + } + } + + if (_rxData[CRSF_MAX_PACKET_SIZE - 1] == CRSF_ADDRESS_CRSF_TRANSMITTER || + _rxData[CRSF_MAX_PACKET_SIZE - 1] == CRSF_ADDRESS_FLIGHT_CONTROLLER) { + leftShift(_rxData, sizeof(_rxData)); + } else if (_rxData[CRSF_MAX_PACKET_SIZE - 2] == + CRSF_ADDRESS_CRSF_TRANSMITTER || + _rxData[CRSF_MAX_PACKET_SIZE - 2] == + CRSF_ADDRESS_FLIGHT_CONTROLLER) { + size = _rxData[CRSF_MAX_PACKET_SIZE - 1]; + leftShift(_rxData, sizeof(_rxData)); } else { leftShift(_rxData, sizeof(_rxData)); } diff --git a/src/crsf/crsf.h b/src/crsf/crsf.h index 3a0bce8..285fc4b 100644 --- a/src/crsf/crsf.h +++ b/src/crsf/crsf.h @@ -19,7 +19,7 @@ class crsf : public SerialIO { private: crsf_channels_t _channelData; - uint8_t _rxData[CRSF_MAX_PACKET_SIZE]; + uint8_t _rxData[CRSF_MAX_PACKET_SIZE]={0}; bool _headerDetected; // Flag indicating whether a header has been detected in // the incoming data. uint8_t _rxIndex; // Index for the receive_buffer. diff --git a/src/fport/fport.cpp b/src/fport/fport.cpp index 911333b..1962afa 100644 --- a/src/fport/fport.cpp +++ b/src/fport/fport.cpp @@ -5,6 +5,7 @@ */ #include "fport.h" +#include fport::fport(Stream *rxPort, int rxPin, int txPin, bool inverted) : SerialIO(rxPort, rxPin, txPin, inverted) {} @@ -25,14 +26,31 @@ void fport::begin() { } void fport::processIncoming() { + uint8_t size = FPORT_MAX_PACKET_SIZE; while (_rxPort->available()) { _rxData[FPORT_MAX_PACKET_SIZE - 1] = _rxPort->read(); + if (_rxData[FPORT_MAX_PACKET_SIZE - size - 4] == FPORT_END_BYTES && + _rxData[FPORT_MAX_PACKET_SIZE - 1] == FPORT_END_BYTES) { + if (_rxData[FPORT_MAX_PACKET_SIZE - size - 2] == + FPORT_FRAMETYPE_RC_CHANNELS_PACKED) { + memcpy(&_channelData, &_rxData[FPORT_MAX_PACKET_SIZE - size - 1], + sizeof(_channelData)); + } + } + + if (_rxData[FPORT_MAX_PACKET_SIZE - 1] == FPORT_END_BYTES) { + leftShift(_rxData, sizeof(_rxData)); + } else if (_rxData[FPORT_MAX_PACKET_SIZE - 2] == 0x7E) { + size = _rxData[FPORT_MAX_PACKET_SIZE - 1]; + leftShift(_rxData, sizeof(_rxData)); + } else { + leftShift(_rxData, sizeof(_rxData)); + } } - while (!(_rxData[0] == 0x7E && _rxData[FPORT_MAX_PACKET_SIZE - 1] == 0x7E)) { - leftShift(_rxData, sizeof(_rxData)); - } - memcpy(&_channelData, &_rxData[4], sizeof(_channelData)); - memset(_rxData, 0, sizeof(_rxData)); } -void fport::getChannel(rc_channels_t *channelData) {} \ No newline at end of file +void fport::getChannel(rc_channels_t *channelData) { + memcpy(channelData, &_channelData, sizeof(rc_channels_t)); +} + +void fport::crc() {} \ No newline at end of file diff --git a/src/fport/fport.h b/src/fport/fport.h index 9af3f44..57f3f70 100644 --- a/src/fport/fport.h +++ b/src/fport/fport.h @@ -9,39 +9,19 @@ #define FPORT_H #include "../SerialIO.h" - -#define FPORT_BAUDRATE 115200 ///< F.Port baudrate -#define FPORT_MAX_PACKET_SIZE 29 ///< F.Port maximum packet length - -typedef struct fport_channels_s { - unsigned channel1 : 11; - unsigned channel2 : 11; - unsigned channel3 : 11; - unsigned channel4 : 11; - unsigned channel5 : 11; - unsigned channel6 : 11; - unsigned channel7 : 11; - unsigned channel8 : 11; - unsigned channel9 : 11; - unsigned channel10 : 11; - unsigned channel11 : 11; - unsigned channel12 : 11; - unsigned channel13 : 11; - unsigned channel14 : 11; - unsigned channel15 : 11; - unsigned channel16 : 11; -} PACKED fport_channels_t; +#include "fport_protocol.h" class fport : public SerialIO { private: - uint8_t _rxData[FPORT_MAX_PACKET_SIZE]; + uint8_t _rxData[FPORT_MAX_PACKET_SIZE] = {0}; fport_channels_t _channelData; public: -explicit fport(Stream *rxPort, int rxPin = -1, int txPin = -1, - bool inverted = true); + explicit fport(Stream *rxPort, int rxPin = -1, int txPin = -1, + bool inverted = true); void begin() override; void processIncoming() override; void getChannel(rc_channels_t *channelData) override; + void crc(); }; #endif \ No newline at end of file diff --git a/src/fport/fport_protocol.h b/src/fport/fport_protocol.h new file mode 100644 index 0000000..55b0fa3 --- /dev/null +++ b/src/fport/fport_protocol.h @@ -0,0 +1,55 @@ +/*! + * @file fport_protocol.h + * @brief Header file for the F.Port protocol implementation. + */ +#pragma once + +#ifndef FPORT_PROTOCOL_H +#define FPORT_PROTOCOL_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include + +#define PACKED __attribute__((packed)) +#define FPORT_BAUDRATE 115200 ///< F.Port baudrate +#define FPORT_MAX_PACKET_SIZE 29 ///< F.Port maximum packet length +#define FPORT_END_BYTES 0x7E + +typedef enum { + FPORT_FRAMETYPE_RC_CHANNELS_PACKED = 0x00, + FPORT_FRAMETYPE_DOWNLINK = 0x01, + FPORT_FRAMETYPE_UPLINK = 0x81, +} fport_frame_type_e; + +typedef struct fport_channels_s { + unsigned channel1 : 11; + unsigned channel2 : 11; + unsigned channel3 : 11; + unsigned channel4 : 11; + unsigned channel5 : 11; + unsigned channel6 : 11; + unsigned channel7 : 11; + unsigned channel8 : 11; + unsigned channel9 : 11; + unsigned channel10 : 11; + unsigned channel11 : 11; + unsigned channel12 : 11; + unsigned channel13 : 11; + unsigned channel14 : 11; + unsigned channel15 : 11; + unsigned channel16 : 11; + unsigned dummy : 4; + unsigned failsafe : 1; + unsigned framelost : 1; + unsigned channel17 : 1; + unsigned channel18 : 1; +} PACKED fport_channels_t; + +#ifdef __cplusplus +} +#endif + +#endif \ No newline at end of file diff --git a/src/ibus/ibus.h b/src/ibus/ibus.h index ae647fb..2e001ef 100644 --- a/src/ibus/ibus.h +++ b/src/ibus/ibus.h @@ -8,30 +8,7 @@ #define IBUS_H #include "../SerialIO.h" // Include header file for the serial IO class - -#define IBUS_MAX_PACKET_SIZE 32 ///< Maximum packet size for the IBUS protocol -#define IBUS_BAUDRATE 115200 ///< Baud rate for IBUS communication -#define IBUS_HEADER1 0x20 -#define IBUS_HEADER2 0x40 - -typedef struct ibus_channels_s { - unsigned header : 16; - unsigned channel1 : 16; - unsigned channel2 : 16; - unsigned channel3 : 16; - unsigned channel4 : 16; - unsigned channel5 : 16; - unsigned channel6 : 16; - unsigned channel7 : 16; - unsigned channel8 : 16; - unsigned channel9 : 16; - unsigned channel10 : 16; - unsigned channel11 : 16; - unsigned channel12 : 16; - unsigned channel13 : 16; - unsigned channel14 : 16; - unsigned checksum : 16; -} PACKED ibus_channels_t; +#include "ibus_protocol.h" /** * @brief A class for handling IBUS protocol communication. diff --git a/src/ibus/ibus_protocol.h b/src/ibus/ibus_protocol.h new file mode 100644 index 0000000..e88103e --- /dev/null +++ b/src/ibus/ibus_protocol.h @@ -0,0 +1,45 @@ +/*! + * @file ibus_protocol.h + * @brief Header file for the iBus protocol implementation. + */ +#pragma once + +#ifndef IBUS_PROTOCOL_H +#define IBUS_PROTOCOL_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include + +#define PACKED __attribute__((packed)) +#define IBUS_MAX_PACKET_SIZE 32 ///< Maximum packet size for the IBUS protocol +#define IBUS_BAUDRATE 115200 ///< Baud rate for IBUS communication +#define IBUS_HEADER1 0x20 +#define IBUS_HEADER2 0x40 + +typedef struct ibus_channels_s { + unsigned header : 16; + unsigned channel1 : 16; + unsigned channel2 : 16; + unsigned channel3 : 16; + unsigned channel4 : 16; + unsigned channel5 : 16; + unsigned channel6 : 16; + unsigned channel7 : 16; + unsigned channel8 : 16; + unsigned channel9 : 16; + unsigned channel10 : 16; + unsigned channel11 : 16; + unsigned channel12 : 16; + unsigned channel13 : 16; + unsigned channel14 : 16; + unsigned checksum : 16; +} PACKED ibus_channels_t; + +#ifdef __cplusplus +} +#endif + +#endif \ No newline at end of file diff --git a/src/sbus/sbus.h b/src/sbus/sbus.h index 5ceba1b..82f24b5 100644 --- a/src/sbus/sbus.h +++ b/src/sbus/sbus.h @@ -8,37 +8,7 @@ #define SBUS_H #include "../SerialIO.h" // Include header file for the serial IO class - -#define HEADER_SBUS 0x0F ///< SBus Header Byte -#define FOOTER_SBUS 0x00 ///< SBus Footer Byte -#define SBUS_BAUDRATE 100000 ///< SBus baudrate -#define SBUS_MAX_PACKET_SIZE 25 ///< SBus packet length - -typedef struct sbus_channels_s { - unsigned header : 8; - unsigned channel1 : 11; - unsigned channel2 : 11; - unsigned channel3 : 11; - unsigned channel4 : 11; - unsigned channel5 : 11; - unsigned channel6 : 11; - unsigned channel7 : 11; - unsigned channel8 : 11; - unsigned channel9 : 11; - unsigned channel10 : 11; - unsigned channel11 : 11; - unsigned channel12 : 11; - unsigned channel13 : 11; - unsigned channel14 : 11; - unsigned channel15 : 11; - unsigned channel16 : 11; - unsigned dummy : 4; - unsigned failsafe : 1; - unsigned framelost : 1; - unsigned channel17 : 1; - unsigned channel18 : 1; - unsigned footer : 8; -} PACKED sbus_channels_t; +#include "sbus_protocol.h" /** * @brief A class for handling SBUS protocol communication. diff --git a/src/sbus/sbus_protocol.h b/src/sbus/sbus_protocol.h new file mode 100644 index 0000000..7ce573c --- /dev/null +++ b/src/sbus/sbus_protocol.h @@ -0,0 +1,51 @@ +/*! + * @file sbus_protocol.h + * @brief Header file for the SBus protocol implementation. + */ +#pragma once + +#ifndef SBUS_PROTOCOL_H +#define SBUS_PROTOCOL_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include + +#define PACKED __attribute__((packed)) +#define HEADER_SBUS 0x0F ///< SBus Header Byte +#define FOOTER_SBUS 0x00 ///< SBus Footer Byte +#define SBUS_BAUDRATE 100000 ///< SBus baudrate +#define SBUS_MAX_PACKET_SIZE 25 ///< SBus packet length + +typedef struct sbus_channels_s { + unsigned header : 8; + unsigned channel1 : 11; + unsigned channel2 : 11; + unsigned channel3 : 11; + unsigned channel4 : 11; + unsigned channel5 : 11; + unsigned channel6 : 11; + unsigned channel7 : 11; + unsigned channel8 : 11; + unsigned channel9 : 11; + unsigned channel10 : 11; + unsigned channel11 : 11; + unsigned channel12 : 11; + unsigned channel13 : 11; + unsigned channel14 : 11; + unsigned channel15 : 11; + unsigned channel16 : 11; + unsigned dummy : 4; + unsigned failsafe : 1; + unsigned framelost : 1; + unsigned channel17 : 1; + unsigned channel18 : 1; + unsigned footer : 8; +} PACKED sbus_channels_t; +#ifdef __cplusplus +} +#endif + +#endif \ No newline at end of file