├── .gitignore ├── README.md ├── .vscode └── extensions.json ├── platformio.ini ├── test └── README ├── lib ├── README ├── crsf │ ├── crsf.h │ └── crsf.cpp └── PPMReader │ ├── PPMReader.h │ └── PPMReader.cpp ├── include └── README └── src └── main.cpp /.gitignore: -------------------------------------------------------------------------------- 1 | .pio 2 | .vscode/.browse.c_cpp.db* 3 | .vscode/c_cpp_properties.json 4 | .vscode/launch.json 5 | .vscode/ipch 6 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # PPM2CRSF 2 | 3 | ExpressLRS now has native PPM support! This is left around only as an example of how to do this. 4 | 5 | ~~Convert PPM signals to a CRSF module input, allowing older handsets or surface handsets to be used with ExpressLRS~~ 6 | -------------------------------------------------------------------------------- /.vscode/extensions.json: -------------------------------------------------------------------------------- 1 | { 2 | // See http://go.microsoft.com/fwlink/?LinkId=827846 3 | // for the documentation about the extensions.json format 4 | "recommendations": [ 5 | "platformio.platformio-ide" 6 | ], 7 | "unwantedRecommendations": [ 8 | "ms-vscode.cpptools-extension-pack" 9 | ] 10 | } 11 | -------------------------------------------------------------------------------- /platformio.ini: -------------------------------------------------------------------------------- 1 | ; PlatformIO Project Configuration File 2 | ; 3 | ; Build options: build flags, source filter 4 | ; Upload options: custom upload port, speed and extra flags 5 | ; Library options: dependencies, extra library storages 6 | ; Advanced options: extra scripting 7 | ; 8 | ; Please visit documentation for the other options and examples 9 | ; https://docs.platformio.org/page/projectconf.html 10 | 11 | [env:esp32doit-devkit-v1] 12 | platform = espressif32 13 | board = esp32doit-devkit-v1 14 | framework = arduino 15 | monitor_speed = 921600 -------------------------------------------------------------------------------- /test/README: -------------------------------------------------------------------------------- 1 | 2 | This directory is intended for PlatformIO Test Runner and project tests. 3 | 4 | Unit Testing is a software testing method by which individual units of 5 | source code, sets of one or more MCU program modules together with associated 6 | control data, usage procedures, and operating procedures, are tested to 7 | determine whether they are fit for use. Unit testing finds problems early 8 | in the development cycle. 9 | 10 | More information about PlatformIO Unit Testing: 11 | - https://docs.platformio.org/en/latest/advanced/unit-testing/index.html 12 | -------------------------------------------------------------------------------- /lib/README: -------------------------------------------------------------------------------- 1 | 2 | This directory is intended for project specific (private) libraries. 3 | PlatformIO will compile them to static libraries and link into executable file. 4 | 5 | The source code of each library should be placed in a an own separate directory 6 | ("lib/your_library_name/[here are source files]"). 7 | 8 | For example, see a structure of the following two libraries `Foo` and `Bar`: 9 | 10 | |--lib 11 | | | 12 | | |--Bar 13 | | | |--docs 14 | | | |--examples 15 | | | |--src 16 | | | |- Bar.c 17 | | | |- Bar.h 18 | | | |- library.json (optional, custom build options, etc) https://docs.platformio.org/page/librarymanager/config.html 19 | | | 20 | | |--Foo 21 | | | |- Foo.c 22 | | | |- Foo.h 23 | | | 24 | | |- README --> THIS FILE 25 | | 26 | |- platformio.ini 27 | |--src 28 | |- main.c 29 | 30 | and a contents of `src/main.c`: 31 | ``` 32 | #include 33 | #include 34 | 35 | int main (void) 36 | { 37 | ... 38 | } 39 | 40 | ``` 41 | 42 | PlatformIO Library Dependency Finder will find automatically dependent 43 | libraries scanning project source files. 44 | 45 | More information about PlatformIO Library Dependency Finder 46 | - https://docs.platformio.org/page/librarymanager/ldf.html 47 | -------------------------------------------------------------------------------- /include/README: -------------------------------------------------------------------------------- 1 | 2 | This directory is intended for project header files. 3 | 4 | A header file is a file containing C declarations and macro definitions 5 | to be shared between several project source files. You request the use of a 6 | header file in your project source file (C, C++, etc) located in `src` folder 7 | by including it, with the C preprocessing directive `#include'. 8 | 9 | ```src/main.c 10 | 11 | #include "header.h" 12 | 13 | int main (void) 14 | { 15 | ... 16 | } 17 | ``` 18 | 19 | Including a header file produces the same results as copying the header file 20 | into each source file that needs it. Such copying would be time-consuming 21 | and error-prone. With a header file, the related declarations appear 22 | in only one place. If they need to be changed, they can be changed in one 23 | place, and programs that include the header file will automatically use the 24 | new version when next recompiled. The header file eliminates the labor of 25 | finding and changing all the copies as well as the risk that a failure to 26 | find one copy will result in inconsistencies within a program. 27 | 28 | In C, the usual convention is to give header files names that end with `.h'. 29 | It is most portable to use only letters, digits, dashes, and underscores in 30 | header file names, and at most one dot. 31 | 32 | Read more about using header files in official GCC documentation: 33 | 34 | * Include Syntax 35 | * Include Operation 36 | * Once-Only Headers 37 | * Computed Includes 38 | 39 | https://gcc.gnu.org/onlinedocs/cpp/Header-Files.html 40 | -------------------------------------------------------------------------------- /lib/crsf/crsf.h: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | // Address definitions 4 | #define CRSF_ADDR_MODULE 0xEE // Transmitter module which will OTA our data 5 | 6 | // Type definitions 7 | #define CRSF_TYPE_CHANNELS 0x16 // An RC channels packet 8 | #define CRSF_TYPE_CHANNELS_PAYLOAD_LENGTH 22 9 | 10 | // internal crsf variables 11 | #define CRSF_OVERHEAD_LENGTH 4 // destination, length, type, crc8 12 | #define CRSF_CHANNEL_MIN 172 13 | #define CRSF_CHANNEL_MID 991 14 | #define CRSF_CHANNEL_MAX 1811 15 | #define CRSF_TIME_NEEDED_PER_FRAME_US 1100 // 700 ms + 400 ms for potential ad-hoc request 16 | #define CRSF_TIME_BETWEEN_FRAMES_US (1e6 / 50) // 50Hz 17 | #define CRSF_PAYLOAD_OFFSET offsetof(crsfFrameDef_t, type) 18 | #define CRSF_MAX_CHANNEL_COUNT 16 19 | #define CRSF_FRAME_SIZE_MAX 64 20 | #define CRSF_SERIAL_BAUDRATE 400000 21 | 22 | // crc implementation from CRSF protocol document rev7 23 | static uint8_t crsf_crc8tab[256] = { 24 | 0x00, 0xD5, 0x7F, 0xAA, 0xFE, 0x2B, 0x81, 0x54, 0x29, 0xFC, 0x56, 0x83, 0xD7, 0x02, 0xA8, 0x7D, 25 | 0x52, 0x87, 0x2D, 0xF8, 0xAC, 0x79, 0xD3, 0x06, 0x7B, 0xAE, 0x04, 0xD1, 0x85, 0x50, 0xFA, 0x2F, 26 | 0xA4, 0x71, 0xDB, 0x0E, 0x5A, 0x8F, 0x25, 0xF0, 0x8D, 0x58, 0xF2, 0x27, 0x73, 0xA6, 0x0C, 0xD9, 27 | 0xF6, 0x23, 0x89, 0x5C, 0x08, 0xDD, 0x77, 0xA2, 0xDF, 0x0A, 0xA0, 0x75, 0x21, 0xF4, 0x5E, 0x8B, 28 | 0x9D, 0x48, 0xE2, 0x37, 0x63, 0xB6, 0x1C, 0xC9, 0xB4, 0x61, 0xCB, 0x1E, 0x4A, 0x9F, 0x35, 0xE0, 29 | 0xCF, 0x1A, 0xB0, 0x65, 0x31, 0xE4, 0x4E, 0x9B, 0xE6, 0x33, 0x99, 0x4C, 0x18, 0xCD, 0x67, 0xB2, 30 | 0x39, 0xEC, 0x46, 0x93, 0xC7, 0x12, 0xB8, 0x6D, 0x10, 0xC5, 0x6F, 0xBA, 0xEE, 0x3B, 0x91, 0x44, 31 | 0x6B, 0xBE, 0x14, 0xC1, 0x95, 0x40, 0xEA, 0x3F, 0x42, 0x97, 0x3D, 0xE8, 0xBC, 0x69, 0xC3, 0x16, 32 | 0xEF, 0x3A, 0x90, 0x45, 0x11, 0xC4, 0x6E, 0xBB, 0xC6, 0x13, 0xB9, 0x6C, 0x38, 0xED, 0x47, 0x92, 33 | 0xBD, 0x68, 0xC2, 0x17, 0x43, 0x96, 0x3C, 0xE9, 0x94, 0x41, 0xEB, 0x3E, 0x6A, 0xBF, 0x15, 0xC0, 34 | 0x4B, 0x9E, 0x34, 0xE1, 0xB5, 0x60, 0xCA, 0x1F, 0x62, 0xB7, 0x1D, 0xC8, 0x9C, 0x49, 0xE3, 0x36, 35 | 0x19, 0xCC, 0x66, 0xB3, 0xE7, 0x32, 0x98, 0x4D, 0x30, 0xE5, 0x4F, 0x9A, 0xCE, 0x1B, 0xB1, 0x64, 36 | 0x72, 0xA7, 0x0D, 0xD8, 0x8C, 0x59, 0xF3, 0x26, 0x5B, 0x8E, 0x24, 0xF1, 0xA5, 0x70, 0xDA, 0x0F, 37 | 0x20, 0xF5, 0x5F, 0x8A, 0xDE, 0x0B, 0xA1, 0x74, 0x09, 0xDC, 0x76, 0xA3, 0xF7, 0x22, 0x88, 0x5D, 38 | 0xD6, 0x03, 0xA9, 0x7C, 0x28, 0xFD, 0x57, 0x82, 0xFF, 0x2A, 0x80, 0x55, 0x01, 0xD4, 0x7E, 0xAB, 39 | 0x84, 0x51, 0xFB, 0x2E, 0x7A, 0xAF, 0x05, 0xD0, 0xAD, 0x78, 0xD2, 0x07, 0x53, 0x86, 0x2C, 0xF9}; 40 | 41 | uint8_t crsf_crc8(const uint8_t *ptr, uint8_t len); 42 | 43 | void crsfPrepareChannelsPacket(uint8_t packet[], int channels[]); -------------------------------------------------------------------------------- /lib/PPMReader/PPMReader.h: -------------------------------------------------------------------------------- 1 | /* 2 | Modified 2023 MUSTARDTIGER 3 | Copyright 2016 Aapo Nikkilä 4 | 5 | This file is part of PPM Reader. 6 | 7 | PPM Reader is free software: you can redistribute it and/or modify 8 | it under the terms of the GNU General Public License as published by 9 | the Free Software Foundation, either version 3 of the License, or 10 | (at your option) any later version. 11 | 12 | PPM Reader is distributed in the hope that it will be useful, 13 | but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | GNU General Public License for more details. 16 | 17 | You should have received a copy of the GNU General Public License 18 | along with PPM Reader. If not, see . 19 | */ 20 | 21 | #ifndef PPM_READER 22 | #define PPM_READER 23 | 24 | #include 25 | 26 | class PPMReader { 27 | 28 | public: 29 | 30 | // The range of a channel's possible values 31 | unsigned long minChannelValue = 988; 32 | unsigned long maxChannelValue = 2012; 33 | 34 | /* The maximum error (in either direction) in channel value 35 | * with which the channel value is still considered valid */ 36 | unsigned long channelValueMaxError = 10; 37 | 38 | /* The minimum value (time) after which the signal frame is considered to 39 | * be finished and we can start to expect a new signal frame. */ 40 | unsigned long blankTime = 2200; 41 | 42 | // Callback function which will be called when the PPM frame is completed and all channels are ready 43 | void (*onFrameComplete)() = nullptr; 44 | 45 | // Set when the interrupt fires 46 | volatile unsigned long interruptMicros = 0; 47 | 48 | private: 49 | 50 | // The amount of channels to be expected from the PPM signal. 51 | byte channelCount = 0; 52 | 53 | // Arrays for keeping track of channel values 54 | volatile unsigned long *rawValues = NULL; 55 | volatile unsigned long *validValues = NULL; 56 | 57 | // A counter variable for determining which channel is being read next 58 | volatile byte pulseCounter = 0; 59 | 60 | // A time variable to remember when the last pulse was read 61 | volatile unsigned long microsAtLastPulse = 0; 62 | 63 | 64 | 65 | public: 66 | 67 | PPMReader(byte channelCount); 68 | ~PPMReader(); 69 | 70 | /* Returns the latest raw (not necessarily valid) value for the 71 | * channel (starting from 1). */ 72 | unsigned long rawChannelValue(byte channel); 73 | 74 | /* Returns the latest received value that was considered valid for the channel (starting from 1). 75 | * Returns defaultValue if the given channel hasn't received any valid values yet. */ 76 | unsigned long latestValidChannelValue(byte channel, unsigned long defaultValue); 77 | 78 | // An interrupt service routine for handling the interrupts activated by PPM pulses 79 | void handleInterrupt(); 80 | 81 | private: 82 | 83 | 84 | }; 85 | 86 | #endif 87 | -------------------------------------------------------------------------------- /lib/PPMReader/PPMReader.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | Modified 2023 MUSTARDTIGER 3 | Copyright 2016 Aapo Nikkilä 4 | 5 | This file is part of PPM Reader. 6 | 7 | PPM Reader is free software: you can redistribute it and/or modify 8 | it under the terms of the GNU General Public License as published by 9 | the Free Software Foundation, either version 3 of the License, or 10 | (at your option) any later version. 11 | 12 | PPM Reader is distributed in the hope that it will be useful, 13 | but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | GNU General Public License for more details. 16 | 17 | You should have received a copy of the GNU General Public License 18 | along with PPM Reader. If not, see . 19 | */ 20 | 21 | #include "PPMReader.h" 22 | 23 | PPMReader::PPMReader(byte channelCount) { 24 | // Check for valid parameters 25 | if (channelCount > 0) { 26 | // Setup an array for storing channel values 27 | this->channelCount = channelCount; 28 | rawValues = new unsigned long[channelCount]; 29 | validValues = new unsigned long[channelCount]; 30 | for (int i = 0; i < channelCount; ++i) { 31 | rawValues[i] = 0; 32 | validValues[i] = 0; 33 | } 34 | } 35 | } 36 | 37 | PPMReader::~PPMReader() { 38 | delete [] rawValues; 39 | delete [] validValues; 40 | } 41 | 42 | void PPMReader::handleInterrupt() { 43 | // Remember the current micros() and calculate the time since the last pulseReceived() 44 | unsigned long previousMicros = microsAtLastPulse; 45 | microsAtLastPulse = interruptMicros; 46 | unsigned long time = microsAtLastPulse - previousMicros; 47 | 48 | if (time > blankTime) { 49 | /* If the time between pulses was long enough to be considered an end 50 | * of a signal frame, prepare to read channel values from the next pulses */ 51 | if (pulseCounter == channelCount && this->onFrameComplete != nullptr) { 52 | this->onFrameComplete(); 53 | } 54 | pulseCounter = 0; 55 | } 56 | else { 57 | // Store times between pulses as channel values 58 | //Serial.printf("%d(%d) = %d\n", pulseCounter, channelCount, time); 59 | if (pulseCounter < channelCount) { 60 | rawValues[pulseCounter] = time; 61 | 62 | if (time >= minChannelValue - channelValueMaxError && time <= maxChannelValue + channelValueMaxError) { 63 | validValues[pulseCounter] = constrain(time, minChannelValue, maxChannelValue); 64 | } 65 | } 66 | ++pulseCounter; 67 | } 68 | } 69 | 70 | unsigned long PPMReader::rawChannelValue(byte channel) { 71 | // Check for channel's validity and return the latest raw channel value or 0 72 | unsigned long value = 0; 73 | if (channel >= 1 && channel <= channelCount) { 74 | value = rawValues[channel-1]; 75 | } 76 | return value; 77 | } 78 | 79 | unsigned long PPMReader::latestValidChannelValue(byte channel, unsigned long defaultValue) { 80 | // Check for channel's validity and return the latest valid channel value or defaultValue. 81 | unsigned long value = defaultValue; 82 | if (channel >= 1 && channel <= channelCount) { 83 | value = validValues[channel-1]; 84 | } 85 | return value; 86 | } 87 | -------------------------------------------------------------------------------- /lib/crsf/crsf.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "crsf.h" 3 | 4 | uint8_t crsf_crc8(const uint8_t *ptr, uint8_t len) 5 | { 6 | uint8_t crc = 0; 7 | for (uint8_t i = 0; i < len; i++) 8 | { 9 | crc = crsf_crc8tab[crc ^ *ptr++]; 10 | } 11 | return crc; 12 | } 13 | 14 | void crsfPreparePacket(uint8_t* packet, const uint8_t destination, const uint8_t type, const uint8_t payload_length, const uint8_t payload[]) 15 | { 16 | // Calculate length + type field + CRC8 17 | uint8_t length = payload_length + 2; 18 | // Write header 19 | packet[0] = destination; 20 | packet[1] = length; 21 | packet[2] = type; 22 | // Write payload 23 | for (uint8_t i = 0; i < payload_length; i++) { 24 | packet[i + 3] = payload[i]; 25 | } 26 | // Insert CRC 27 | packet[length + 1] = crsf_crc8(&packet[2], payload_length + 1); 28 | } 29 | 30 | void crsfPrepareChannelsPacket(uint8_t* packet, int channels[]) 31 | { 32 | int mapped_channels[CRSF_MAX_CHANNEL_COUNT]; 33 | for (uint8_t i = 0; i < CRSF_MAX_CHANNEL_COUNT; i++) { 34 | mapped_channels[i] = map(channels[i],1000,2000,CRSF_CHANNEL_MIN,CRSF_CHANNEL_MAX); 35 | } 36 | uint8_t payload[CRSF_TYPE_CHANNELS_PAYLOAD_LENGTH]; 37 | payload[0] = (uint8_t)(mapped_channels[0] & 0x07FF); 38 | payload[1] = (uint8_t)((mapped_channels[0] & 0x07FF) >> 8 | (mapped_channels[1] & 0x07FF) << 3); 39 | payload[2] = (uint8_t)((mapped_channels[1] & 0x07FF) >> 5 | (mapped_channels[2] & 0x07FF) << 6); 40 | payload[3] = (uint8_t)((mapped_channels[2] & 0x07FF) >> 2); 41 | payload[4] = (uint8_t)((mapped_channels[2] & 0x07FF) >> 10 | (mapped_channels[3] & 0x07FF) << 1); 42 | payload[5] = (uint8_t)((mapped_channels[3] & 0x07FF) >> 7 | (mapped_channels[4] & 0x07FF) << 4); 43 | payload[6] = (uint8_t)((mapped_channels[4] & 0x07FF) >> 4 | (mapped_channels[5] & 0x07FF) << 7); 44 | payload[7] = (uint8_t)((mapped_channels[5] & 0x07FF) >> 1); 45 | payload[8] = (uint8_t)((mapped_channels[5] & 0x07FF) >> 9 | (mapped_channels[6] & 0x07FF) << 2); 46 | payload[9] = (uint8_t)((mapped_channels[6] & 0x07FF) >> 6 | (mapped_channels[7] & 0x07FF) << 5); 47 | payload[10] = (uint8_t)((mapped_channels[7] & 0x07FF) >> 3); 48 | payload[11] = (uint8_t)((mapped_channels[8] & 0x07FF)); 49 | payload[12] = (uint8_t)((mapped_channels[8] & 0x07FF) >> 8 | (mapped_channels[9] & 0x07FF) << 3); 50 | payload[13] = (uint8_t)((mapped_channels[9] & 0x07FF) >> 5 | (mapped_channels[10] & 0x07FF) << 6); 51 | payload[14] = (uint8_t)((mapped_channels[10] & 0x07FF) >> 2); 52 | payload[15] = (uint8_t)((mapped_channels[10] & 0x07FF) >> 10 | (mapped_channels[11] & 0x07FF) << 1); 53 | payload[16] = (uint8_t)((mapped_channels[11] & 0x07FF) >> 7 | (mapped_channels[12] & 0x07FF) << 4); 54 | payload[17] = (uint8_t)((mapped_channels[12] & 0x07FF) >> 4 | (mapped_channels[13] & 0x07FF) << 7); 55 | payload[18] = (uint8_t)((mapped_channels[13] & 0x07FF) >> 1); 56 | payload[19] = (uint8_t)((mapped_channels[13] & 0x07FF) >> 9 | (mapped_channels[14] & 0x07FF) << 2); 57 | payload[20] = (uint8_t)((mapped_channels[14] & 0x07FF) >> 6 | (mapped_channels[15] & 0x07FF) << 5); 58 | payload[21] = (uint8_t)((mapped_channels[15] & 0x07FF) >> 3); 59 | crsfPreparePacket(packet, CRSF_ADDR_MODULE, CRSF_TYPE_CHANNELS, CRSF_TYPE_CHANNELS_PAYLOAD_LENGTH, payload); 60 | } 61 | -------------------------------------------------------------------------------- /src/main.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | // General parameters 8 | 9 | #define DEFAULT_CHANNEL_VALUE 1500 10 | //#define DEBUG 11 | 12 | // PPM Parameters 13 | 14 | #define PPM_IN_PIN 4 15 | #define PPM_CHANNEL_COUNT 8 16 | #define PPM_DIRECTION FALLING 17 | 18 | // CRSF Parameters 19 | 20 | #define CRSF_OUT_PIN 15 21 | #define CRSF_IN_PIN 2 22 | #define CRSF_INVERT true 23 | 24 | // PPM Handlers 25 | 26 | void IRAM_ATTR onPPMInterruptCallback(); 27 | PPMReader reader(PPM_CHANNEL_COUNT); 28 | 29 | // PPM variables 30 | volatile uint8_t pulseCounter = 0; // Incremented when a new PPM pulse is ingested 31 | volatile uint8_t lastPulse = 0; // Set when a new PPM pulse has been handled 32 | 33 | // Transitive variables 34 | int channels[CRSF_MAX_CHANNEL_COUNT]; // Holds ingested PPM values so they can be sent over CRSF 35 | 36 | // CRSF variables 37 | uint8_t packet[CRSF_FRAME_SIZE_MAX]; // Buffer holding CRSF frame in construction 38 | volatile uint8_t frameCounter = 0; // Incremented when a new PPM frame is ready 39 | volatile uint8_t lastFrame = 0; // Set when a CRSF frame has been sent 40 | 41 | void onPPMReady() 42 | { 43 | noInterrupts(); 44 | // Fill channels from PPM values 45 | for (uint8_t i = 0; i < PPM_CHANNEL_COUNT; i++) 46 | { 47 | channels[i] = reader.latestValidChannelValue(i + 1, 1500); 48 | } 49 | // Fill in the remaining channels 50 | for (uint8_t i = PPM_CHANNEL_COUNT; i < CRSF_MAX_CHANNEL_COUNT; i++) 51 | { 52 | channels[i] = DEFAULT_CHANNEL_VALUE; 53 | } 54 | frameCounter++; 55 | interrupts(); 56 | } 57 | 58 | // Called when a PPM pulse has been ingested. 59 | void IRAM_ATTR onPPMInterruptCallback() 60 | { 61 | // Set the current time into the PPMReader as soon as the pulse fires so that nothing delays our PPM read accuracy 62 | reader.interruptMicros = micros(); 63 | // Ingest the pulse is ready so we ingest it 64 | reader.handleInterrupt(); 65 | } 66 | 67 | void crsfTask(void *param) 68 | { 69 | #ifdef DEBUG 70 | Serial.println("Starting CRSF task"); 71 | #endif 72 | for (;;) 73 | { 74 | // Handle a PPM frame ready, converting to CRSF 75 | if (lastFrame != frameCounter) 76 | { 77 | crsfPrepareChannelsPacket(packet, channels); 78 | lastFrame = frameCounter; 79 | #ifdef DEBUG 80 | for (uint8_t i = 0; i < CRSF_TYPE_CHANNELS_PAYLOAD_LENGTH + CRSF_OVERHEAD_LENGTH; i++) 81 | { 82 | Serial.print(packet[i], HEX); 83 | } 84 | Serial.printf(" %d\n", frameCounter); 85 | Serial.printf("%d %d %d %d %d\n", channels[0], channels[1], channels[2], channels[3], channels[4]); 86 | #endif 87 | Serial1.write(packet, CRSF_TYPE_CHANNELS_PAYLOAD_LENGTH + CRSF_OVERHEAD_LENGTH); 88 | } 89 | vTaskDelay(pdMS_TO_TICKS(10)); // TODO: can we reduce? 90 | } 91 | } 92 | 93 | void setup() 94 | { 95 | #ifdef DEBUG 96 | Serial.begin(921600); 97 | #endif 98 | Serial1.begin(CRSF_SERIAL_BAUDRATE, SERIAL_8N1, CRSF_IN_PIN, CRSF_OUT_PIN, CRSF_INVERT); 99 | 100 | pinMode(PPM_IN_PIN, INPUT); 101 | attachInterrupt(PPM_IN_PIN, onPPMInterruptCallback, PPM_DIRECTION); 102 | 103 | // Called when a full PPM frame is ready with all channels 104 | reader.onFrameComplete = onPPMReady; 105 | #ifdef DEBUG 106 | Serial.println("Starting PPM2CRSF"); 107 | #endif 108 | xTaskCreatePinnedToCore(crsfTask, "crsf", 4096, NULL, 5, NULL, 1); 109 | } 110 | 111 | void loop() 112 | { 113 | } 114 | --------------------------------------------------------------------------------