├── keywords.txt ├── README.md ├── .clang-format ├── example └── PickChennel │ ├── PickChennel.ino │ └── PickChennel.cpp ├── CRSF.code-workspace ├── library.properties ├── .gitignore ├── src ├── crc8.cpp ├── crc8.h ├── CrsfSerial.h ├── crsf_protocol.h └── CrsfSerial.cpp └── LICENSE /keywords.txt: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # CRSF -------------------------------------------------------------------------------- /.clang-format: -------------------------------------------------------------------------------- 1 | Language: Cpp 2 | BasedOnStyle: Google 3 | AlignConsecutiveMacros: true 4 | AlignEscapedNewlines: Right -------------------------------------------------------------------------------- /example/PickChennel/PickChennel.ino: -------------------------------------------------------------------------------- 1 | /** 2 | * 3 | * Using this sketch, you can see how your remote works. 4 | * 5 | * Connect the receiver to Serial3 and turn on the transmitter and receiver. 6 | * 7 | * If you use another chip with one hardware Serial, you can use a software one 8 | * 9 | */ -------------------------------------------------------------------------------- /CRSF.code-workspace: -------------------------------------------------------------------------------- 1 | /** 2 | * @file CRSF 3 | * @author Sam4uk 4 | * 5 | * @copyright Copyright (c) 2024 6 | * 7 | */ 8 | { 9 | "folders": [ 10 | { 11 | "path": "." 12 | } 13 | ], 14 | "settings": { 15 | "files.autoSave": "onFocusChange", 16 | "workbench.colorTheme": "Elegant Red", 17 | "workbench.iconTheme": "vscode-great-icons" 18 | } 19 | } -------------------------------------------------------------------------------- /library.properties: -------------------------------------------------------------------------------- 1 | name=CRSF 2 | version=0.1.0 3 | author=Sam4uk 4 | maintainer=Sam4uk 5 | sentence=Бібліотека для радіо керованих моделей протоколом CRSF 6 | paragraph=Бібліотека знаходиться на стадії розробки та чорнової публікації 7 | category=Communication 8 | url=https://sam4uk.github.io/libraries/crsf/ 9 | architectures=* 10 | includes=CrsfSerial.h 11 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Prerequisites 2 | *.d 3 | 4 | # Compiled Object files 5 | *.slo 6 | *.lo 7 | *.o 8 | *.obj 9 | 10 | # Precompiled Headers 11 | *.gch 12 | *.pch 13 | 14 | # Compiled Dynamic libraries 15 | *.so 16 | *.dylib 17 | *.dll 18 | 19 | # Fortran module files 20 | *.mod 21 | *.smod 22 | 23 | # Compiled Static libraries 24 | *.lai 25 | *.la 26 | *.a 27 | *.lib 28 | 29 | # Executables 30 | *.exe 31 | *.out 32 | *.app 33 | -------------------------------------------------------------------------------- /src/crc8.cpp: -------------------------------------------------------------------------------- 1 | #include "crc8.h" 2 | 3 | Crc8::Crc8(uint8_t poly) 4 | { 5 | init(poly); 6 | } 7 | 8 | void Crc8::init(uint8_t poly) 9 | { 10 | for (int idx=0; idx<256; ++idx) 11 | { 12 | uint8_t crc = idx; 13 | for (int shift=0; shift<8; ++shift) 14 | { 15 | crc = (crc << 1) ^ ((crc & 0x80) ? poly : 0); 16 | } 17 | _lut[idx] = crc & 0xff; 18 | } 19 | } 20 | 21 | uint8_t Crc8::calc(uint8_t *data, uint8_t len) 22 | { 23 | uint8_t crc = 0; 24 | while (len--) 25 | { 26 | crc = _lut[crc ^ *data++]; 27 | } 28 | return crc; 29 | } 30 | -------------------------------------------------------------------------------- /src/crc8.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file crc8.h 3 | * @author Sam4uk 4 | * @brief 5 | * @version 0.1 6 | * @date 2023-12-09 7 | * 8 | * @copyright Copyright (c) 2023 9 | * 10 | */ 11 | #ifndef CRC8_H 12 | #define CRC8_H 13 | 14 | 15 | #include 16 | 17 | class Crc8 18 | { 19 | public: 20 | /** 21 | * @brief Construct a new Crc 8 object 22 | * 23 | * @param poly 24 | */ 25 | Crc8(uint8_t poly); 26 | /** 27 | * @brief 28 | * 29 | * @param data 30 | * @param len 31 | * @return uint8_t 32 | */ 33 | uint8_t calc(uint8_t *data, uint8_t len); 34 | 35 | protected: 36 | uint8_t _lut[256]; 37 | void init(uint8_t poly); 38 | }; 39 | #endif -------------------------------------------------------------------------------- /example/PickChennel/PickChennel.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file PickChennel.ino 3 | * @author Sam4uk 4 | * @version 0.1 5 | * @date 2024-01-02 6 | * 7 | * @copyright Copyright (c) 2024 8 | * 9 | */ 10 | #include 11 | #if defined(__AVR_ATmega2560__) 12 | CrsfSerial crsf(Serial3, CRSF_BAUDRATE); 13 | #else 14 | #error NOT MEGA2560 15 | #endif 16 | 17 | /** 18 | * @brief The function that will be called upon receiving data via control 19 | * channels 20 | */ 21 | void packetChannels(); 22 | 23 | /** 24 | * @brief Executed once on power-up or reboot 25 | */ 26 | void setup() { 27 | Serial.begin(115200); 28 | crsf.begin(); 29 | crsf.onPacketChannels = &packetChannels; 30 | } 31 | 32 | /** 33 | * @brief Executes cyclically while the power is on 34 | */ 35 | void loop() { crsf.loop(); } 36 | 37 | void packetChannels() { 38 | for (auto ch{1}; ch <= CRSF_NUM_CHANNELS; ++ch) { 39 | Serial.print(crsf.getChannel(ch)); 40 | if (ch != CRSF_NUM_CHANNELS) Serial.print(", "); 41 | } 42 | Serial.println(); 43 | } -------------------------------------------------------------------------------- /src/CrsfSerial.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file CrsfSerial.h 3 | * @author Sam4uk 4 | * @brief 5 | * @version 0.1 6 | * @date 2023-12-09 7 | * 8 | * @copyright Copyright (c) 2023 9 | * 10 | */ 11 | #ifndef CRSF_SERIAL_H 12 | #define CRSF_SERIAL_H 13 | #ifdef ARDUINO 14 | #include 15 | #endif 16 | #include 17 | 18 | #include "crsf_protocol.h" 19 | 20 | /// @brief 21 | enum eFailsafeAction { 22 | fsaNoPulses, ///< 23 | fsaHold ///< 24 | }; 25 | 26 | class CrsfSerial { 27 | public: 28 | // Packet timeout where buffer is flushed if no data is received in this time 29 | static const unsigned int CRSF_PACKET_TIMEOUT_MS = 100; 30 | static const unsigned int CRSF_FAILSAFE_STAGE1_MS = 300; 31 | 32 | /** 33 | * @brief Construct a new Crsf Serial object 34 | * 35 | * @param port 36 | * @param baud 37 | */ 38 | CrsfSerial(HardwareSerial &port, uint32_t baud = CRSF_BAUDRATE); 39 | /** 40 | * @brief 41 | * 42 | * @param baud 43 | */ 44 | void begin(uint32_t baud = 0); 45 | /** 46 | * @brief Call from main loop to update 47 | * 48 | */ 49 | void loop(); 50 | /** 51 | * @brief 52 | * 53 | * @param b 54 | */ 55 | void write(uint8_t b); 56 | /** 57 | * @brief 58 | * 59 | * @param buf 60 | * @param len 61 | */ 62 | void write(const uint8_t *buf, size_t len); 63 | /** 64 | * @brief 65 | * 66 | * @param addr 67 | * @param type 68 | * @param payload 69 | * @param len 70 | */ 71 | void queuePacket(uint8_t addr, uint8_t type, const void *payload, 72 | uint8_t len); 73 | 74 | /** 75 | * @brief Get the Baud object 76 | * 77 | * @return uint32_t 78 | */ 79 | uint32_t getBaud() const; 80 | /** 81 | * @brief Return current channel value (1-based) in us 82 | * 83 | * @param ch 84 | * @return int 85 | */ 86 | int getChannel(unsigned int ch) const; 87 | /** 88 | * @brief Get the Link Statistics object 89 | * 90 | * @return const crsfLinkStatistics_t* 91 | */ 92 | const crsfLinkStatistics_t *getLinkStatistics() const; 93 | /** 94 | * @brief Get the Gps Sensor object 95 | * 96 | * @return const crsf_sensor_gps_t* 97 | */ 98 | const crsf_sensor_gps_t *getGpsSensor() const; 99 | /** 100 | * @brief 101 | * 102 | * @return true 103 | * @return false 104 | */ 105 | bool isLinkUp() const; 106 | /** 107 | * @brief Get the Passthrough Mode object 108 | * 109 | * @return true 110 | * @return false 111 | */ 112 | bool getPassthroughMode() const; 113 | /** 114 | * @brief Enter passthrough mode (serial sent directly to shiftybyte), 115 | * optionally changing the baud rate used during passthrough mode 116 | * @param val 117 | * True to start passthrough mode, false to resume processing CRSF 118 | * @param passthroughBaud 119 | * New baud rate for passthrough mode, or 0 to not change baud 120 | * Not used if disabling passthough 121 | */ 122 | void setPassthroughMode(bool val, uint32_t passthroughBaud = 0); 123 | 124 | // Event Handlers 125 | /** 126 | * @brief 127 | * 128 | */ 129 | void (*onLinkUp)(); 130 | /** 131 | * @brief 132 | * 133 | */ 134 | void (*onLinkDown)(); 135 | // OobData is any byte which is not CRSF, including passthrough 136 | /** 137 | * @brief 138 | * 139 | */ 140 | void (*onOobData)(uint8_t b); 141 | // CRSF Packet Callbacks 142 | /** 143 | * @brief 144 | * 145 | */ 146 | void (*onPacketChannels)(); 147 | /** 148 | * @brief 149 | * 150 | */ 151 | void (*onPacketLinkStatistics)(crsfLinkStatistics_t *ls); 152 | /** 153 | * @brief 154 | * 155 | */ 156 | void (*onPacketGps)(crsf_sensor_gps_t *gpsSensor); 157 | 158 | private: 159 | HardwareSerial &_port; 160 | uint8_t _rxBuf[CRSF_MAX_PACKET_SIZE]; 161 | uint8_t _rxBufPos; 162 | Crc8 _crc; 163 | crsfLinkStatistics_t _linkStatistics; 164 | crsf_sensor_gps_t _gpsSensor; 165 | uint32_t _baud; 166 | uint32_t _lastReceive; 167 | uint32_t _lastChannelsPacket; 168 | bool _linkIsUp; 169 | uint32_t _passthroughBaud; 170 | int _channels[CRSF_NUM_CHANNELS]; 171 | 172 | void handleSerialIn(); 173 | void handleByteReceived(); 174 | void shiftRxBuffer(uint8_t cnt); 175 | void processPacketIn(uint8_t len); 176 | void checkPacketTimeout(); 177 | void checkLinkDown(); 178 | 179 | // Packet Handlers 180 | void packetChannelsPacked(const crsf_header_t *p); 181 | void packetLinkStatistics(const crsf_header_t *p); 182 | void packetGps(const crsf_header_t *p); 183 | }; 184 | 185 | #endif -------------------------------------------------------------------------------- /src/crsf_protocol.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file crsf_protocol.h 3 | * @author Sam4uk 4 | * @brief 5 | * @version 0.1 6 | * @date 2023-12-08 7 | * 8 | * @copyright Copyright (c) 2023 9 | * 10 | */ 11 | #ifndef CRSF_PROTOCOL_H 12 | #define CRSF_PROTOCOL_H 13 | #include 14 | 15 | #if !defined(PACKED) 16 | #define PACKED __attribute__((packed)) 17 | #endif 18 | 19 | /// @brief Boudrate 20 | #define CRSF_BAUDRATE (420000) 21 | 22 | /// @brief Channels count 23 | #define CRSF_NUM_CHANNELS (16) 24 | 25 | /// 987us - actual CRSF min is 0 with E.Limits on 26 | #define CRSF_CHANNEL_VALUE_MIN (172) 27 | #define CRSF_CHANNEL_VALUE_1000 (191) 28 | #define CRSF_CHANNEL_VALUE_MID (992) 29 | #define CRSF_CHANNEL_VALUE_2000 (1792) 30 | // 2012us - actual CRSF max is 1984 with E.Limits on 31 | #define CRSF_CHANNEL_VALUE_MAX (1811) 32 | #define CRSF_CHANNEL_VALUE_SPAN \ 33 | (CRSF_CHANNEL_VALUE_MAX - CRSF_CHANNEL_VALUE_MIN) 34 | #define CRSF_ELIMIT_US_MIN (891) // microseconds for CRSF=0 (E.Limits=ON) 35 | #define CRSF_ELIMIT_US_MAX (2119) // microseconds for CRSF=1984 36 | #define CRSF_MAX_PACKET_SIZE \ 37 | (64) // max declared len is 62+DEST+LEN on top of that = 64 38 | #define CRSF_MAX_PAYLOAD_LEN \ 39 | (CRSF_MAX_PACKET_SIZE - \ 40 | 4) // Max size of payload in [dest] [len] [type] [payload] [crc8] 41 | 42 | // Clashes with CRSF_ADDRESS_FLIGHT_CONTROLLER 43 | #define CRSF_SYNC_BYTE (0XC8) 44 | 45 | enum { 46 | CRSF_FRAME_LENGTH_ADDRESS = 1, // length of ADDRESS field 47 | CRSF_FRAME_LENGTH_FRAMELENGTH = 1, // length of FRAMELENGTH field 48 | CRSF_FRAME_LENGTH_TYPE = 1, // length of TYPE field 49 | CRSF_FRAME_LENGTH_CRC = 1, // length of CRC field 50 | CRSF_FRAME_LENGTH_TYPE_CRC = 2, // length of TYPE and CRC fields combined 51 | CRSF_FRAME_LENGTH_EXT_TYPE_CRC = 52 | 4, // length of Extended Dest/Origin, TYPE and CRC fields combined 53 | CRSF_FRAME_LENGTH_NON_PAYLOAD = 54 | 4, // combined length of all fields except payload 55 | }; 56 | 57 | enum { 58 | CRSF_FRAME_GPS_PAYLOAD_SIZE = 15, 59 | CRSF_FRAME_BATTERY_SENSOR_PAYLOAD_SIZE = 8, 60 | CRSF_FRAME_LINK_STATISTICS_PAYLOAD_SIZE = 10, 61 | CRSF_FRAME_RC_CHANNELS_PAYLOAD_SIZE = 62 | 22, // 11 bits per channel * 16 channels = 22 bytes. 63 | CRSF_FRAME_ATTITUDE_PAYLOAD_SIZE = 6, 64 | }; 65 | 66 | typedef enum { 67 | CRSF_FRAMETYPE_GPS = 0x02, 68 | CRSF_FRAMETYPE_BATTERY_SENSOR = 0x08, 69 | CRSF_FRAMETYPE_LINK_STATISTICS = 0x14, 70 | CRSF_FRAMETYPE_OPENTX_SYNC = 0x10, 71 | CRSF_FRAMETYPE_RADIO_ID = 0x3A, 72 | CRSF_FRAMETYPE_RC_CHANNELS_PACKED = 0x16, 73 | CRSF_FRAMETYPE_ATTITUDE = 0x1E, 74 | CRSF_FRAMETYPE_FLIGHT_MODE = 0x21, 75 | // Extended Header Frames, range: 0x28 to 0x96 76 | CRSF_FRAMETYPE_DEVICE_PING = 0x28, 77 | CRSF_FRAMETYPE_DEVICE_INFO = 0x29, 78 | CRSF_FRAMETYPE_PARAMETER_SETTINGS_ENTRY = 0x2B, 79 | CRSF_FRAMETYPE_PARAMETER_READ = 0x2C, 80 | CRSF_FRAMETYPE_PARAMETER_WRITE = 0x2D, 81 | CRSF_FRAMETYPE_COMMAND = 0x32, 82 | // MSP commands 83 | CRSF_FRAMETYPE_MSP_REQ = 84 | 0x7A, // response request using msp sequence as command 85 | CRSF_FRAMETYPE_MSP_RESP = 0x7B, // reply with 58 byte chunked binary 86 | CRSF_FRAMETYPE_MSP_WRITE = 0x7C, // write with 8 byte chunked binary (OpenTX 87 | // outbound telemetry buffer limit) 88 | } crsf_frame_type_e; 89 | 90 | typedef enum { 91 | CRSF_ADDRESS_BROADCAST = 0x00, 92 | CRSF_ADDRESS_USB = 0x10, 93 | CRSF_ADDRESS_TBS_CORE_PNP_PRO = 0x80, 94 | CRSF_ADDRESS_RESERVED1 = 0x8A, 95 | CRSF_ADDRESS_CURRENT_SENSOR = 0xC0, 96 | CRSF_ADDRESS_GPS = 0xC2, 97 | CRSF_ADDRESS_TBS_BLACKBOX = 0xC4, 98 | CRSF_ADDRESS_FLIGHT_CONTROLLER = 0xC8, 99 | CRSF_ADDRESS_RESERVED2 = 0xCA, 100 | CRSF_ADDRESS_RACE_TAG = 0xCC, 101 | CRSF_ADDRESS_RADIO_TRANSMITTER = 0xEA, 102 | CRSF_ADDRESS_CRSF_RECEIVER = 0xEC, 103 | CRSF_ADDRESS_CRSF_TRANSMITTER = 0xEE, 104 | } crsf_addr_e; 105 | 106 | typedef struct crsf_header_s { 107 | uint8_t device_addr; // from crsf_addr_e 108 | uint8_t frame_size; // counts size after this byte, so it must be the payload 109 | // size + 2 (type and crc) 110 | uint8_t type; // from crsf_frame_type_e 111 | uint8_t data[0]; 112 | } PACKED crsf_header_t; 113 | 114 | typedef struct crsf_channels_s { 115 | unsigned ch0 : 11; 116 | unsigned ch1 : 11; 117 | unsigned ch2 : 11; 118 | unsigned ch3 : 11; 119 | unsigned ch4 : 11; 120 | unsigned ch5 : 11; 121 | unsigned ch6 : 11; 122 | unsigned ch7 : 11; 123 | unsigned ch8 : 11; 124 | unsigned ch9 : 11; 125 | unsigned ch10 : 11; 126 | unsigned ch11 : 11; 127 | unsigned ch12 : 11; 128 | unsigned ch13 : 11; 129 | unsigned ch14 : 11; 130 | unsigned ch15 : 11; 131 | } PACKED crsf_channels_t; 132 | 133 | typedef struct crsfPayloadLinkstatistics_s { 134 | uint8_t uplink_RSSI_1; 135 | uint8_t uplink_RSSI_2; 136 | uint8_t uplink_Link_quality; 137 | int8_t uplink_SNR; 138 | uint8_t active_antenna; 139 | uint8_t rf_Mode; 140 | uint8_t uplink_TX_Power; 141 | uint8_t downlink_RSSI; 142 | uint8_t downlink_Link_quality; 143 | int8_t downlink_SNR; 144 | } crsfLinkStatistics_t; 145 | 146 | typedef struct crsf_sensor_battery_s { 147 | uint32_t voltage : 16; // V * 10 big endian 148 | uint32_t current : 16; // A * 10 big endian 149 | uint32_t capacity : 24; // mah big endian 150 | uint32_t remaining : 8; // % 151 | } PACKED crsf_sensor_battery_t; 152 | 153 | typedef struct crsf_sensor_gps_s { 154 | int32_t latitude; // degree / 10,000,000 big endian 155 | int32_t longitude; // degree / 10,000,000 big endian 156 | uint16_t groundspeed; // km/h / 10 big endian 157 | uint16_t heading; // GPS heading, degree/100 big endian 158 | uint16_t altitude; // meters, +1000m big endian 159 | uint8_t satellites; // satellites 160 | } PACKED crsf_sensor_gps_t; 161 | 162 | #if !defined(__linux__) 163 | static inline uint16_t htobe16(uint16_t val) { 164 | #if (__BYTE_ORDER__ == __ORDER_BIG_ENDIAN__) 165 | return val; 166 | #else 167 | return __builtin_bswap16(val); 168 | #endif 169 | } 170 | 171 | static inline uint16_t be16toh(uint16_t val) { 172 | #if (__BYTE_ORDER__ == __ORDER_BIG_ENDIAN__) 173 | return val; 174 | #else 175 | return __builtin_bswap16(val); 176 | #endif 177 | } 178 | 179 | static inline uint32_t htobe32(uint32_t val) { 180 | #if (__BYTE_ORDER__ == __ORDER_BIG_ENDIAN__) 181 | return val; 182 | #else 183 | return __builtin_bswap32(val); 184 | #endif 185 | } 186 | 187 | static inline uint32_t be32toh(uint32_t val) { 188 | #if (__BYTE_ORDER__ == __ORDER_BIG_ENDIAN__) 189 | return val; 190 | #else 191 | return __builtin_bswap32(val); 192 | #endif 193 | } 194 | #endif 195 | 196 | #endif -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Creative Commons Legal Code 2 | 3 | CC0 1.0 Universal 4 | 5 | CREATIVE COMMONS CORPORATION IS NOT A LAW FIRM AND DOES NOT PROVIDE 6 | LEGAL SERVICES. DISTRIBUTION OF THIS DOCUMENT DOES NOT CREATE AN 7 | ATTORNEY-CLIENT RELATIONSHIP. CREATIVE COMMONS PROVIDES THIS 8 | INFORMATION ON AN "AS-IS" BASIS. CREATIVE COMMONS MAKES NO WARRANTIES 9 | REGARDING THE USE OF THIS DOCUMENT OR THE INFORMATION OR WORKS 10 | PROVIDED HEREUNDER, AND DISCLAIMS LIABILITY FOR DAMAGES RESULTING FROM 11 | THE USE OF THIS DOCUMENT OR THE INFORMATION OR WORKS PROVIDED 12 | HEREUNDER. 13 | 14 | Statement of Purpose 15 | 16 | The laws of most jurisdictions throughout the world automatically confer 17 | exclusive Copyright and Related Rights (defined below) upon the creator 18 | and subsequent owner(s) (each and all, an "owner") of an original work of 19 | authorship and/or a database (each, a "Work"). 20 | 21 | Certain owners wish to permanently relinquish those rights to a Work for 22 | the purpose of contributing to a commons of creative, cultural and 23 | scientific works ("Commons") that the public can reliably and without fear 24 | of later claims of infringement build upon, modify, incorporate in other 25 | works, reuse and redistribute as freely as possible in any form whatsoever 26 | and for any purposes, including without limitation commercial purposes. 27 | These owners may contribute to the Commons to promote the ideal of a free 28 | culture and the further production of creative, cultural and scientific 29 | works, or to gain reputation or greater distribution for their Work in 30 | part through the use and efforts of others. 31 | 32 | For these and/or other purposes and motivations, and without any 33 | expectation of additional consideration or compensation, the person 34 | associating CC0 with a Work (the "Affirmer"), to the extent that he or she 35 | is an owner of Copyright and Related Rights in the Work, voluntarily 36 | elects to apply CC0 to the Work and publicly distribute the Work under its 37 | terms, with knowledge of his or her Copyright and Related Rights in the 38 | Work and the meaning and intended legal effect of CC0 on those rights. 39 | 40 | 1. Copyright and Related Rights. A Work made available under CC0 may be 41 | protected by copyright and related or neighboring rights ("Copyright and 42 | Related Rights"). Copyright and Related Rights include, but are not 43 | limited to, the following: 44 | 45 | i. the right to reproduce, adapt, distribute, perform, display, 46 | communicate, and translate a Work; 47 | ii. moral rights retained by the original author(s) and/or performer(s); 48 | iii. publicity and privacy rights pertaining to a person's image or 49 | likeness depicted in a Work; 50 | iv. rights protecting against unfair competition in regards to a Work, 51 | subject to the limitations in paragraph 4(a), below; 52 | v. rights protecting the extraction, dissemination, use and reuse of data 53 | in a Work; 54 | vi. database rights (such as those arising under Directive 96/9/EC of the 55 | European Parliament and of the Council of 11 March 1996 on the legal 56 | protection of databases, and under any national implementation 57 | thereof, including any amended or successor version of such 58 | directive); and 59 | vii. other similar, equivalent or corresponding rights throughout the 60 | world based on applicable law or treaty, and any national 61 | implementations thereof. 62 | 63 | 2. Waiver. To the greatest extent permitted by, but not in contravention 64 | of, applicable law, Affirmer hereby overtly, fully, permanently, 65 | irrevocably and unconditionally waives, abandons, and surrenders all of 66 | Affirmer's Copyright and Related Rights and associated claims and causes 67 | of action, whether now known or unknown (including existing as well as 68 | future claims and causes of action), in the Work (i) in all territories 69 | worldwide, (ii) for the maximum duration provided by applicable law or 70 | treaty (including future time extensions), (iii) in any current or future 71 | medium and for any number of copies, and (iv) for any purpose whatsoever, 72 | including without limitation commercial, advertising or promotional 73 | purposes (the "Waiver"). Affirmer makes the Waiver for the benefit of each 74 | member of the public at large and to the detriment of Affirmer's heirs and 75 | successors, fully intending that such Waiver shall not be subject to 76 | revocation, rescission, cancellation, termination, or any other legal or 77 | equitable action to disrupt the quiet enjoyment of the Work by the public 78 | as contemplated by Affirmer's express Statement of Purpose. 79 | 80 | 3. Public License Fallback. Should any part of the Waiver for any reason 81 | be judged legally invalid or ineffective under applicable law, then the 82 | Waiver shall be preserved to the maximum extent permitted taking into 83 | account Affirmer's express Statement of Purpose. In addition, to the 84 | extent the Waiver is so judged Affirmer hereby grants to each affected 85 | person a royalty-free, non transferable, non sublicensable, non exclusive, 86 | irrevocable and unconditional license to exercise Affirmer's Copyright and 87 | Related Rights in the Work (i) in all territories worldwide, (ii) for the 88 | maximum duration provided by applicable law or treaty (including future 89 | time extensions), (iii) in any current or future medium and for any number 90 | of copies, and (iv) for any purpose whatsoever, including without 91 | limitation commercial, advertising or promotional purposes (the 92 | "License"). The License shall be deemed effective as of the date CC0 was 93 | applied by Affirmer to the Work. Should any part of the License for any 94 | reason be judged legally invalid or ineffective under applicable law, such 95 | partial invalidity or ineffectiveness shall not invalidate the remainder 96 | of the License, and in such case Affirmer hereby affirms that he or she 97 | will not (i) exercise any of his or her remaining Copyright and Related 98 | Rights in the Work or (ii) assert any associated claims and causes of 99 | action with respect to the Work, in either case contrary to Affirmer's 100 | express Statement of Purpose. 101 | 102 | 4. Limitations and Disclaimers. 103 | 104 | a. No trademark or patent rights held by Affirmer are waived, abandoned, 105 | surrendered, licensed or otherwise affected by this document. 106 | b. Affirmer offers the Work as-is and makes no representations or 107 | warranties of any kind concerning the Work, express, implied, 108 | statutory or otherwise, including without limitation warranties of 109 | title, merchantability, fitness for a particular purpose, non 110 | infringement, or the absence of latent or other defects, accuracy, or 111 | the present or absence of errors, whether or not discoverable, all to 112 | the greatest extent permissible under applicable law. 113 | c. Affirmer disclaims responsibility for clearing rights of other persons 114 | that may apply to the Work or any use thereof, including without 115 | limitation any person's Copyright and Related Rights in the Work. 116 | Further, Affirmer disclaims responsibility for obtaining any necessary 117 | consents, permissions or other rights required for any use of the 118 | Work. 119 | d. Affirmer understands and acknowledges that Creative Commons is not a 120 | party to this document and has no duty or obligation with respect to 121 | this CC0 or use of the Work. 122 | -------------------------------------------------------------------------------- /src/CrsfSerial.cpp: -------------------------------------------------------------------------------- 1 | #include "CrsfSerial.h" 2 | 3 | // static void hexdump(void *p, size_t len) 4 | // { 5 | // char *data = (char *)p; 6 | // while (len > 0) 7 | // { 8 | // uint8_t linepos = 0; 9 | // char* linestart = data; 10 | // // Binary part 11 | // while (len > 0 && linepos < 16) 12 | // { 13 | // if (*data < 0x0f) 14 | // Serial.write('0'); 15 | // Serial.print(*data, HEX); 16 | // Serial.write(' '); 17 | // ++data; 18 | // ++linepos; 19 | // --len; 20 | // } 21 | 22 | // // Spacer to align last line 23 | // for (uint8_t i = linepos; i < 16; ++i) 24 | // Serial.print(" "); 25 | 26 | // // ASCII part 27 | // for (uint8_t i = 0; i < linepos; ++i) 28 | // Serial.write((linestart[i] < ' ') ? '.' : linestart[i]); 29 | // Serial.println(); 30 | // } 31 | // } 32 | 33 | CrsfSerial::CrsfSerial(HardwareSerial &port, uint32_t baud) 34 | : _port(port), 35 | _crc(0xd5), 36 | _baud(baud), 37 | _lastReceive(0), 38 | _lastChannelsPacket(0), 39 | _linkIsUp(false), 40 | _passthroughBaud(0) {} 41 | 42 | void CrsfSerial::begin(uint32_t baud) { 43 | if (baud != 0) 44 | _port.begin(baud); 45 | else 46 | _port.begin(_baud); 47 | } 48 | 49 | // Call from main loop to update 50 | void CrsfSerial::loop() { handleSerialIn(); } 51 | 52 | void CrsfSerial::write(uint8_t b) { _port.write(b); } 53 | 54 | void CrsfSerial::write(const uint8_t *buf, size_t len) { 55 | _port.write(buf, len); 56 | } 57 | 58 | void CrsfSerial::queuePacket(uint8_t addr, uint8_t type, const void *payload, 59 | uint8_t len) { 60 | if (getPassthroughMode()) return; 61 | if (len > CRSF_MAX_PAYLOAD_LEN) return; 62 | 63 | uint8_t buf[CRSF_MAX_PACKET_SIZE]; 64 | buf[0] = addr; 65 | buf[1] = len + 2; // type + payload + crc 66 | buf[2] = type; 67 | memcpy(&buf[3], payload, len); 68 | buf[len + 3] = _crc.calc(&buf[2], len + 1); 69 | 70 | write(buf, len + 4); 71 | } 72 | 73 | uint32_t CrsfSerial::getBaud() const { return _baud; }; 74 | 75 | int CrsfSerial::getChannel(unsigned int ch) const { return _channels[ch - 1]; } 76 | 77 | const crsfLinkStatistics_t *CrsfSerial::getLinkStatistics() const { 78 | return &_linkStatistics; 79 | } 80 | 81 | const crsf_sensor_gps_t *CrsfSerial::getGpsSensor() const { 82 | return &_gpsSensor; 83 | } 84 | 85 | bool CrsfSerial::isLinkUp() const { return _linkIsUp; } 86 | 87 | bool CrsfSerial::getPassthroughMode() const { return _passthroughBaud != 0; } 88 | 89 | void CrsfSerial::setPassthroughMode(bool val, uint32_t passthroughBaud) { 90 | if (val) { 91 | // If not requesting any baud change 92 | if (passthroughBaud == 0) { 93 | // Enter passthrough mode if not yet 94 | if (_passthroughBaud == 0) _passthroughBaud = _baud; 95 | return; 96 | } 97 | 98 | _passthroughBaud = passthroughBaud; 99 | } else { 100 | // Not in passthrough, can't leave it any harder than we already are 101 | if (_passthroughBaud == 0) return; 102 | 103 | // Leaving passthrough, but going back to same baud, just disable 104 | if (_passthroughBaud == _baud) { 105 | _passthroughBaud = 0; 106 | return; 107 | } 108 | 109 | _passthroughBaud = 0; 110 | } 111 | 112 | // Can only get here if baud is changing, close and reopen the port 113 | _port.end(); // assumes flush() 114 | begin(_passthroughBaud); 115 | } 116 | 117 | void CrsfSerial::handleSerialIn() { 118 | while (_port.available()) { 119 | uint8_t b = _port.read(); 120 | _lastReceive = millis(); 121 | 122 | if (getPassthroughMode()) { 123 | if (onOobData) onOobData(b); 124 | continue; 125 | } 126 | 127 | _rxBuf[_rxBufPos++] = b; 128 | handleByteReceived(); 129 | 130 | if (_rxBufPos == (sizeof(_rxBuf) / sizeof(_rxBuf[0]))) { 131 | // Packet buffer filled and no valid packet found, dump the whole thing 132 | _rxBufPos = 0; 133 | } 134 | } 135 | 136 | checkPacketTimeout(); 137 | checkLinkDown(); 138 | } 139 | 140 | void CrsfSerial::handleByteReceived() { 141 | bool reprocess; 142 | do { 143 | reprocess = false; 144 | if (_rxBufPos > 1) { 145 | uint8_t len = _rxBuf[1]; 146 | // Sanity check the declared length isn't outside Type + 147 | // X{1,CRSF_MAX_PAYLOAD_LEN} + CRC assumes there never will be a CRSF 148 | // message that just has a type and no data (X) 149 | if (len < 3 || len > (CRSF_MAX_PAYLOAD_LEN + 2)) { 150 | shiftRxBuffer(1); 151 | reprocess = true; 152 | } 153 | 154 | else if (_rxBufPos >= (len + 2)) { 155 | uint8_t inCrc = _rxBuf[2 + len - 1]; 156 | uint8_t crc = _crc.calc(&_rxBuf[2], len - 1); 157 | if (crc == inCrc) { 158 | processPacketIn(len); 159 | shiftRxBuffer(len + 2); 160 | reprocess = true; 161 | } else { 162 | shiftRxBuffer(1); 163 | reprocess = true; 164 | } 165 | } // if complete packet 166 | } // if pos > 1 167 | } while (reprocess); 168 | } 169 | 170 | void CrsfSerial::checkPacketTimeout() { 171 | // If we haven't received data in a long time, flush the buffer a byte at a 172 | // time (to trigger shiftyByte) 173 | if (_rxBufPos > 0 && millis() - _lastReceive > CRSF_PACKET_TIMEOUT_MS) 174 | while (_rxBufPos) shiftRxBuffer(1); 175 | } 176 | 177 | void CrsfSerial::checkLinkDown() { 178 | if (_linkIsUp && millis() - _lastChannelsPacket > CRSF_FAILSAFE_STAGE1_MS) { 179 | if (onLinkDown) onLinkDown(); 180 | _linkIsUp = false; 181 | } 182 | } 183 | 184 | void CrsfSerial::processPacketIn(uint8_t len) { 185 | const crsf_header_t *hdr = (crsf_header_t *)_rxBuf; 186 | if (hdr->device_addr == CRSF_ADDRESS_FLIGHT_CONTROLLER) { 187 | switch (hdr->type) { 188 | case CRSF_FRAMETYPE_GPS: 189 | packetGps(hdr); 190 | break; 191 | case CRSF_FRAMETYPE_RC_CHANNELS_PACKED: 192 | packetChannelsPacked(hdr); 193 | break; 194 | case CRSF_FRAMETYPE_LINK_STATISTICS: 195 | packetLinkStatistics(hdr); 196 | break; 197 | } 198 | } // CRSF_ADDRESS_FLIGHT_CONTROLLER 199 | } 200 | 201 | // Shift the bytes in the RxBuf down by cnt bytes 202 | void CrsfSerial::shiftRxBuffer(uint8_t cnt) { 203 | // If removing the whole thing, just set pos to 0 204 | if (cnt >= _rxBufPos) { 205 | _rxBufPos = 0; 206 | return; 207 | } 208 | 209 | if (cnt == 1 && onOobData) onOobData(_rxBuf[0]); 210 | 211 | // Otherwise do the slow shift down 212 | uint8_t *src = &_rxBuf[cnt]; 213 | uint8_t *dst = &_rxBuf[0]; 214 | _rxBufPos -= cnt; 215 | uint8_t left = _rxBufPos; 216 | while (left--) *dst++ = *src++; 217 | } 218 | 219 | void CrsfSerial::packetChannelsPacked(const crsf_header_t *p) { 220 | crsf_channels_t *ch = (crsf_channels_t *)&p->data; 221 | _channels[0] = ch->ch0; 222 | _channels[1] = ch->ch1; 223 | _channels[2] = ch->ch2; 224 | _channels[3] = ch->ch3; 225 | _channels[4] = ch->ch4; 226 | _channels[5] = ch->ch5; 227 | _channels[6] = ch->ch6; 228 | _channels[7] = ch->ch7; 229 | _channels[8] = ch->ch8; 230 | _channels[9] = ch->ch9; 231 | _channels[10] = ch->ch10; 232 | _channels[11] = ch->ch11; 233 | _channels[12] = ch->ch12; 234 | _channels[13] = ch->ch13; 235 | _channels[14] = ch->ch14; 236 | _channels[15] = ch->ch15; 237 | 238 | for (unsigned int i = 0; i < CRSF_NUM_CHANNELS; ++i) 239 | _channels[i] = map(_channels[i], CRSF_CHANNEL_VALUE_1000, 240 | CRSF_CHANNEL_VALUE_2000, 1000, 2000); 241 | 242 | if (!_linkIsUp && onLinkUp) onLinkUp(); 243 | _linkIsUp = true; 244 | _lastChannelsPacket = millis(); 245 | 246 | if (onPacketChannels) onPacketChannels(); 247 | } 248 | 249 | void CrsfSerial::packetLinkStatistics(const crsf_header_t *p) { 250 | const crsfLinkStatistics_t *link = (crsfLinkStatistics_t *)p->data; 251 | memcpy(&_linkStatistics, link, sizeof(_linkStatistics)); 252 | 253 | if (onPacketLinkStatistics) onPacketLinkStatistics(&_linkStatistics); 254 | } 255 | 256 | void CrsfSerial::packetGps(const crsf_header_t *p) { 257 | const crsf_sensor_gps_t *gps = (crsf_sensor_gps_t *)p->data; 258 | _gpsSensor.latitude = be32toh(gps->latitude); 259 | _gpsSensor.longitude = be32toh(gps->longitude); 260 | _gpsSensor.groundspeed = be16toh(gps->groundspeed); 261 | _gpsSensor.heading = be16toh(gps->heading); 262 | _gpsSensor.altitude = be16toh(gps->altitude); 263 | _gpsSensor.satellites = gps->satellites; 264 | 265 | if (onPacketGps) onPacketGps(&_gpsSensor); 266 | } 267 | 268 | --------------------------------------------------------------------------------