├── lib └── terseCRSF │ ├── v0.0.8.txt │ ├── terseCRSF.h │ └── terseCRSF.cpp ├── .gitattributes ├── img ├── uid.png ├── config.png └── output.png ├── .gitignore ├── .vscode ├── settings.json └── extensions.json ├── src ├── crsf.h ├── main.cpp └── crsf.cpp ├── README.md └── platformio.ini /lib/terseCRSF/v0.0.8.txt: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /.gitattributes: -------------------------------------------------------------------------------- 1 | # Auto detect text files and perform LF normalization 2 | * text=auto 3 | -------------------------------------------------------------------------------- /img/uid.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/druckgott/ELRS-Backpack-Example-ESPNOW/HEAD/img/uid.png -------------------------------------------------------------------------------- /img/config.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/druckgott/ELRS-Backpack-Example-ESPNOW/HEAD/img/config.png -------------------------------------------------------------------------------- /img/output.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/druckgott/ELRS-Backpack-Example-ESPNOW/HEAD/img/output.png -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | .pio 2 | .vscode/.browse.c_cpp.db* 3 | .vscode/c_cpp_properties.json 4 | .vscode/launch.json 5 | .vscode/ipch 6 | -------------------------------------------------------------------------------- /.vscode/settings.json: -------------------------------------------------------------------------------- 1 | { 2 | "files.associations": { 3 | "bitset": "cpp", 4 | "chrono": "cpp" 5 | } 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 | -------------------------------------------------------------------------------- /src/crsf.h: -------------------------------------------------------------------------------- 1 | // crsf.h 2 | #ifndef CRSF_H 3 | #define CRSF_H 4 | 5 | // Deklarationen der Variablen 6 | extern int16_t espnow_len; 7 | extern int16_t crsf_len; 8 | extern bool espnow_received; 9 | 10 | // Hier die Prototypen der Funktionen und Deklarationen der Variablen 11 | void crsfReceive(); // Beispiel für eine Funktion 12 | 13 | #endif -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # ELRS Backpack send data via ESPNOW to ESP8266/ESP32 2 | Backpack Expresslrs Example with ESP8266/ESP32 and CRSF Protokoll 3 | 4 | # Config the sketch: 5 | * On the website https://expresslrs.github.io/web-flasher/, the private UID must be generated (using the same password as the ELRS system). 6 | * This UID must be entered in the sketch under ``` uint8_t UID[6] = {106,19,19,206,193,30};```. 7 | 8 | * In the ELRS Backpack, Telemetry ESPNOW must be enabled (tested with Backpack version 1.5.1). 9 | 10 | * A telemetry connection between the transmitter and, for example, the drone/flight controller must be established. 11 | 12 | ![config screenshot](img/config.png) 13 | ![config screenshot](img/uid.png) 14 | ![output screenshot](img/output.png) 15 | -------------------------------------------------------------------------------- /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:nodemcuv2] 12 | platform = espressif8266 13 | framework = arduino 14 | board = nodemcuv2 15 | upload_protocol = esptool 16 | ; change microcontroller 17 | board_build.mcu = esp8266 18 | ; change MCU frequency 19 | board_build.f_cpu = 160000000L 20 | monitor_speed = 115200 21 | 22 | build_flags = -D PIO_FRAMEWORK_ARDUINO_ESP8266_GIT_REV="\"$(git rev-parse HEAD)\"" 23 | 24 | 25 | [env:esp32dev] 26 | platform = espressif32 27 | board = esp32dev 28 | framework = arduino 29 | upload_protocol = esptool 30 | ; change microcontroller 31 | board_build.mcu = esp32 32 | ; change MCU frequency 33 | board_build.f_cpu = 240000000L 34 | monitor_speed = 115200 35 | 36 | build_flags = -D PIO_FRAMEWORK_ARDUINO_ESP8266_GIT_REV="\"$(git rev-parse HEAD)\"" -------------------------------------------------------------------------------- /src/main.cpp: -------------------------------------------------------------------------------- 1 | 2 | #if ESP32 3 | #pragma message "ESP32 stuff happening!" 4 | #else 5 | #pragma message "ESP8266 stuff happening!" 6 | #endif 7 | 8 | #ifdef ESP32 9 | #include 10 | #include 11 | #include 12 | #else 13 | #include 14 | #include 15 | #endif 16 | 17 | #include // https://github.com/zs6buj/terseCRSF use v 0.0.6 or later 18 | #include "crsf.h" // Einbinden der crsf.h 19 | 20 | // Please use ExpressLRS Configurator Runtime Options to obtain your UID (unique MAC hashed 21 | // from binding_phrase) Insert the six numbers between the curly brackets below 22 | //For ESP NOW / ELRS Backpack you need to enter the UID resulting from hashing your 23 | //secret binding phrase. This must be obtained by launching https://expresslrs.github.io/web-flasher/, 24 | //enter your binding phrase, then make a note of the UID. Enter the 6 numbers between the commas 25 | // Config Elrs Binding 26 | //uint8_t UID[6] = {0,0,0,0,0,0}; // this is my UID. You have to change it to your once, should look 27 | uint8_t UID[6] = {106,19,19,206,193,30}; // this is my UID. You have to change it to your once (UID Input: 12345678) 28 | 29 | const char* ssid = "Backpack_ELRS_Crsf"; // SSID des Access Points 30 | const char* password = "12345678"; // Passwort des Access Points 31 | 32 | // Externe Deklaration des CRSF-Objekts 33 | extern CRSF crsf; // Instanziierung des CRSF-Objekts 34 | 35 | // Callback when data is received 36 | #ifdef ESP32 37 | void OnDataRecv(const uint8_t *mac, const uint8_t *incomingData, int len) { 38 | #else 39 | void OnDataRecv(uint8_t * mac, uint8_t *incomingData, uint8_t len) { 40 | #endif 41 | Serial.print("ESPNow: len: "); 42 | Serial.print(len); 43 | Serial.print(", "); 44 | // Annahme: printBytes(&*data, len); gibt jedes Byte des Arrays `data` im Hex-Format aus 45 | for (int i = 0; i < len; i++) { 46 | Serial.print(incomingData[i], HEX); // Ausgabe jedes Bytes als Hexadezimalzahl 47 | if (i < len - 1) { 48 | Serial.print(" "); // Leerzeichen zwischen den Bytes 49 | } 50 | } 51 | Serial.println(); // Neue Zeile am Ende der Ausgabe 52 | // For CRSF protocol from here 53 | espnow_len = len; 54 | crsf_len = len - 8; 55 | espnow_received = true; 56 | memcpy(&crsf.crsf_buf, &*(incomingData+8), sizeof(crsf.crsf_buf)); 57 | } 58 | 59 | void setup() { 60 | // Init Serial Monitor 61 | Serial.begin(115200); 62 | Serial.println("Start"); 63 | // MAC address can only be set with unicast, so first byte must be even, not odd --> important for BACKPACK 64 | UID[0] = UID[0] & ~0x01; 65 | WiFi.mode(WIFI_STA); 66 | // Set device as a Wi-Fi Station 67 | //WiFi.mode(WIFI_AP_STA); // Ermöglicht AP und Station gleichzeitig 68 | WiFi.disconnect(); 69 | // Set a custom MAC address for the device in station mode (important for BACKPACK compatibility) 70 | #ifdef ESP32 71 | esp_wifi_set_mac(WIFI_IF_STA, UID); 72 | #else 73 | wifi_set_macaddr(STATION_IF, UID); //--> important for BACKPACK 74 | #endif 75 | // Start the device in Access Point mode with the specified SSID and password 76 | WiFi.softAP(ssid, password); 77 | // Init ESP-NOW 78 | #ifdef ESP32 79 | if (esp_now_init() != ESP_OK) { 80 | #else 81 | if (esp_now_init() != 0) { 82 | #endif 83 | Serial.println("Error initializing ESP-NOW"); 84 | return; 85 | } 86 | 87 | #ifdef ESP32 88 | esp_now_register_recv_cb(OnDataRecv); 89 | #else 90 | // Set ESP-NOW Role 91 | esp_now_set_self_role(ESP_NOW_ROLE_COMBO); 92 | // Register peer 93 | esp_now_add_peer(UID, ESP_NOW_ROLE_COMBO, 0, NULL, 0); 94 | // Register a callback function to handle received ESP-NOW data 95 | esp_now_register_recv_cb(esp_now_recv_cb_t(OnDataRecv)); 96 | #endif 97 | } 98 | 99 | void loop() { 100 | //only for crsf Output 101 | crsfReceive(); 102 | } 103 | -------------------------------------------------------------------------------- /src/crsf.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include // https://github.com/zs6buj/terseCRSF use v 0.0.6 or later 3 | // crsf.cpp 4 | #include "crsf.h" 5 | 6 | CRSF crsf; // Instanziierung des CRSF-Objekts 7 | 8 | int16_t espnow_len = 0; 9 | int16_t crsf_len = 0; 10 | bool espnow_received = false; 11 | 12 | uint8_t hud_num_sats = 0; 13 | float hud_grd_spd = 0; 14 | int16_t hud_bat1_volts = 0; // dV (V * 10) 15 | int16_t hud_bat1_amps = 0; // dA )A * 10) 16 | uint16_t hud_bat1_mAh = 0; 17 | 18 | struct Location { 19 | float lat; //long 20 | float lon; 21 | float alt; 22 | float hdg; 23 | float alt_ag; 24 | }; 25 | 26 | struct Location hom = { 27 | 0,0,0,0 28 | }; // home location 29 | struct Location cur = { 30 | 0,0,0,0 31 | }; // current location 32 | 33 | bool gpsGood = false; 34 | bool gpsPrev = false; 35 | bool hdgGood = false; 36 | bool serGood = false; 37 | bool lonGood = false; 38 | bool latGood = false; 39 | bool altGood = false; 40 | bool boxhdgGood = false; 41 | bool motArmed = false; // motors armed flag 42 | bool gpsfixGood = false; 43 | 44 | bool finalHomeStored = false; 45 | 46 | uint32_t gpsGood_millis = 0; 47 | 48 | void crsfReceive() 49 | { 50 | if (espnow_received) // flag 51 | { 52 | espnow_received = false; 53 | int16_t len = espnow_len; 54 | uint8_t crsf_id = crsf.decodeTelemetry(&*crsf.crsf_buf, len); 55 | //log.printf("crsf_id:%2X\n", crsf_id); 56 | if (crsf_id == GPS_ID) // 0x02 57 | { 58 | // don't use gps heading, use attitude yaw below 59 | cur.lon = crsf.gpsF_lon; 60 | cur.lat = crsf.gpsF_lat; 61 | cur.alt = crsf.gps_altitude; 62 | gpsfixGood = (crsf.gps_sats >=5); // with 4 sats, altitude value can be bad 63 | lonGood = (crsf.gpsF_lon != 0.0); 64 | latGood = (crsf.gpsF_lat != 0.0); 65 | altGood = (crsf.gps_altitude != 0); 66 | hdgGood = true; 67 | if ((finalHomeStored)) 68 | { 69 | cur.alt_ag = cur.alt - hom.alt; 70 | } else 71 | { 72 | cur.alt_ag = 0; 73 | } 74 | hud_num_sats = crsf.gps_sats; // these for the display 75 | hud_grd_spd = crsf.gpsF_groundspeed; 76 | log.print("GPS id:"); 77 | crsf.printByte(crsf_id, ' '); 78 | log.printf("lat:%2.7f lon:%2.7f", crsf.gpsF_lat, crsf.gpsF_lon); 79 | log.printf(" ground_spd:%.1fkm/hr", crsf.gpsF_groundspeed); 80 | log.printf(" hdg:%.2fdeg", crsf.gpsF_heading); 81 | log.printf(" alt:%dm", crsf.gps_altitude); 82 | log.printf(" sats:%d", crsf.gps_sats); 83 | log.printf(" gpsfixGood:%d", gpsfixGood); 84 | log.printf(" hdgGood:%d\n", hdgGood); 85 | } 86 | if (crsf_id == BATTERY_ID) 87 | { 88 | hud_bat1_volts = crsf.batF_voltage; 89 | hud_bat1_amps = crsf.batF_current; 90 | hud_bat1_mAh = crsf.batF_fuel_drawn * 1000; 91 | log.print("BATTERY id:"); 92 | crsf.printByte(crsf_id, ' '); 93 | log.printf("volts:%2.1f", crsf.batF_voltage); 94 | log.printf(" amps:%3.1f", crsf.batF_current); 95 | log.printf(" Ah_drawn:%3.1f", crsf.batF_fuel_drawn); 96 | log.printf(" remaining:%3u%%\n", crsf.bat_remaining); 97 | } 98 | 99 | if (crsf_id == ATTITUDE_ID) 100 | { 101 | cur.hdg = crsf.attiF_yaw; 102 | log.print("ATTITUDE id:"); 103 | crsf.printByte(crsf_id, ' '); 104 | log.printf("pitch:%3.1fdeg", crsf.attiF_pitch); 105 | log.printf(" roll:%3.1fdeg", crsf.attiF_roll); 106 | log.printf(" yaw:%3.1fdeg\n", crsf.attiF_yaw); 107 | } 108 | 109 | if (crsf_id == FLIGHT_MODE_ID) 110 | { 111 | motArmed = !(crsf.flightMode.compare("ARM")); // Returns 0 if both the strings are the same 112 | log.print("FLIGHT_MODE id:"); 113 | crsf.printByte(crsf_id, ' '); 114 | log.printf("lth:%u %s motArmed:%u\n", crsf.flight_mode_lth, crsf.flightMode.c_str(), motArmed); 115 | gpsGood = (gpsfixGood & lonGood & latGood & altGood); 116 | if (gpsGood) gpsGood_millis = millis(); // Time of last good GPS packet 117 | } 118 | } 119 | 120 | static bool prevGpsGood = 0; 121 | if (gpsGood != prevGpsGood) 122 | { 123 | log.printf("gpsGood:%u gpsfixGood:%u lonGood:%u latGood:%u altGood:%u hdgGood:%u boxhdgGood:%u \n", gpsGood, gpsfixGood, lonGood, latGood, altGood, hdgGood, boxhdgGood); 124 | prevGpsGood = gpsGood; 125 | } 126 | } -------------------------------------------------------------------------------- /lib/terseCRSF/terseCRSF.h: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | //#define RC_BUILD // else TELEMETRY_BUILD 7 | #if defined RC_BUILD 8 | //#define SUPPORT_SBUS_OUT 9 | #endif 10 | 11 | #define MAJOR_VER 0 12 | #define MINOR_VER 0 13 | #define PATCH_LEV 8 14 | 15 | //#define TELEMETRY_SOURCE 1 // BetaFlight/CF 16 | #define TELEMETRY_SOURCE 2 // EdgeTX/OpenTX 17 | 18 | #if not defined TELEMETRY_SOURCE 19 | #define TELEMETRY_SOURCE 1 20 | #endif 21 | 22 | /* 23 | Changelog 24 | v0.0.3 2024-05-13 Add SHOW_BYTE_STREAM debug option 25 | v0.0.4 2024-05-17 Fix flight-mode position and length 26 | v0.0.5 2024-05-18 Rationalise macros 27 | Add Telemetry source selection 28 | v0.0.6 2024-06-26 Add UART, UDP, BT telemetry example 29 | Divide Battery Volts and Amps by 10 30 | V0.0.7 2024-07-09 Divide by V & A by a further 10 31 | */ 32 | 33 | //========= D E M O / D E B U G M A C R O S ======== 34 | 35 | //#define DEMO_PWM_VALUES 36 | //#define DEMO_SBUS 37 | #define DEMO_CRSF_GPS 38 | #define DEMO_CRSF_BATTERY 39 | //#define DEMO_CRSF_LINK 40 | #define DEMO_CRSF_ATTITUDE 41 | #define DEMO_CRSF_FLIGHT_MODE 42 | 43 | //#define SHOW_BUFFER 44 | //#define SHOW_BYTE_STREAM 45 | //#define SHOW_LOOP_PERIOD 46 | 47 | #define SHOW_CRSF_CF_VARIO 48 | #define SHOW_CRSF_BARO 49 | #define SHOW_LINK_STATS 50 | #define SHOW_CRSF_CHANNELS 51 | #define SHOW_CRSF_LINK_RX 52 | #define SHOW_CRSF_LINK_TX 53 | #define SHOW_CRSF_DEVIDE_INFO 54 | #define SHOW_CRSF_REQUEST_SETTINGS 55 | #define SHOW_CRSF_COMMAND 56 | #define SHOW_CRSF_RADIO 57 | #define SHOW_OTHER_FRAME_IDs 58 | 59 | //========================================== 60 | 61 | #define log Serial 62 | 63 | #define RADS2DEGS 180 / PI 64 | 65 | typedef enum sbus_mode_state 66 | { 67 | sbm_normal = 0, 68 | sbm_fast = 1 69 | } sbmode_t; 70 | 71 | // Frame id 72 | #define GPS_ID 0x02 73 | #define CF_VARIO_ID 0x07 74 | #define BATTERY_ID 0x08 75 | #define BARO_ALT_ID 0x09 76 | #define HEARTBEAT_ID 0x0B // added 77 | #define LINK_ID 0x14 // link statistics 78 | #define CHANNELS_ID 0x16 79 | #define LINK_RX_ID 0x1C 80 | #define LINK_TX_ID 0x1D 81 | #define ATTITUDE_ID 0x1E 82 | #define FLIGHT_MODE_ID 0x21 83 | #define PING_DEVICES_ID 0x28 84 | #define DEVICE_INFO_ID 0x29 85 | #define REQUEST_SETTINGS_ID 0x2A 86 | #define COMMAND_ID 0x32 87 | #define RADIO_ID 0x3A 88 | 89 | #if (TELEMETRY_SOURCE == 1) // BetaFlight 90 | #define CRSF_TEL_SYNC_BYTE 0xC8 91 | #elif (TELEMETRY_SOURCE == 2) // EdgeTX/OpenTx 92 | #define CRSF_TEL_SYNC_BYTE 0xEA 93 | #endif 94 | 95 | #define CRSF_RC_SYNC_BYTE 0xEE 96 | 97 | class CRSF 98 | { 99 | // Data members 100 | public: 101 | // quote "416KBaud that CRSF uses", ELRS uses 420000 102 | #if defined RC_BUILD 103 | const uint32_t crfs_baud = 420000; // works for both 104 | #else 105 | const uint32_t crfs_baud = 420000; // works for both 106 | #endif 107 | 108 | const uint8_t crsf_buffer_size = 64; 109 | const uint8_t max_rc_bytes = 22; // just the RC bytes, not the full sbus 110 | const uint8_t sbus_buffer_size = 25; // Header(1) + RC_bytes(22) + status(1)(los+fs) + footer(1) 111 | const uint8_t max_ch = 8; // max 18 112 | 113 | uint8_t frame_lth = 0; 114 | uint8_t crsf_buf[64] {}; // sizes as per above 115 | uint8_t rc_bytes[22] {}; 116 | uint8_t sb_bytes[25] {}; 117 | uint16_t pwm_val[8] {}; 118 | 119 | uint8_t crsf_id = 0; 120 | uint8_t crsf_lth = 0; 121 | 122 | /* GPS ID:0x02 */ 123 | int32_t gps_lat = 0; // deg * 1e7 124 | int32_t gps_lon = 0; 125 | float gpsF_lat = 0; // deg 126 | float gpsF_lon = 0.0; 127 | uint16_t gps_groundspeed = 0; 128 | float gpsF_groundspeed = 0.0; // km/hr 129 | uint16_t gps_heading = 0; 130 | float gpsF_heading = 0.0; // deg 131 | int16_t gps_altitude = 0; // metres, 1000m offset 132 | uint8_t gps_sats = 0; 133 | 134 | /* Battery ID:0x08 */ 135 | uint16_t bat_voltage = 0; // mV * 100 136 | float batF_voltage = 0.0; // volts 137 | uint16_t bat_current = 0; // mA * 100 138 | float batF_current = 0.0; // amps 139 | uint32_t bat_fuel_drawn = 0; // uint24_t mAh drawn 140 | float batF_fuel_drawn = 0.0; // Ah drawn 141 | uint8_t bat_remaining = 0; // percent 142 | 143 | /* Link Statistics ID 0x14*/ 144 | uint8_t link_up_rssi_ant_1 = 0; // dBm * -1 145 | uint8_t link_up_rssi_ant_2 = 0; // dBm * -1 146 | uint8_t link_up_quality = 0; // packet_success_rate (%) 147 | int8_t link_up_snr = 0; // db 148 | uint8_t link_diversity_active_ant = 0; //(enum ant_1 = 0, ant_2) 149 | uint8_t link_rf_mode = 0; //(enum 4fps = 0, 50fps, 150hz) 150 | uint8_t link_up_tx_power = 0; //(enum 0mW = 0, 10mW, 25 mW, 100 mW, 500 mW, 1000 mW, 2000mW) 151 | uint8_t link_dn_rssi = 0; // RSSI(dBm * -1) 152 | uint8_t link_dn_quality = 0; // packet_success_rate (%) 153 | int8_t link_dn_snr = 0; // db 154 | 155 | /* Attitude ID:0x1E */ 156 | int16_t atti_pitch = 0; // rad / 10000 157 | float attiF_pitch = 0.0; // deg 158 | int16_t atti_roll = 0; // rad / 10000 159 | float attiF_roll = 0.0; // deg 160 | int16_t atti_yaw = 0; // rad / 10000) 161 | float attiF_yaw = 0.0; // deg 162 | 163 | /* Flight Mode ID:0x21*/ 164 | uint8_t flight_mode_lth = 0; 165 | std::string flightMode; 166 | 167 | //= (char *)"ACRO"; 168 | 169 | // Member function prototypes 170 | 171 | private: 172 | Stream* crsf_port; // pointer type 173 | Stream* sbus_port; // pointer type 174 | // own link stats 175 | uint32_t frames_read = 0; 176 | uint32_t good_frames = 0; 177 | uint32_t crc_errors = 0; 178 | uint32_t frame_errors = 0; 179 | uint16_t unknown_ids = 0; 180 | 181 | public: 182 | //CRSF(); // for 183 | bool initialise(Stream& port); 184 | bool sbus_initialise(Stream& port); 185 | bool readCrsfFrame(uint8_t <h); 186 | uint8_t decodeTelemetry(uint8_t *_buf, uint8_t len); 187 | void decodeRC(); 188 | void printByte(byte b, char delimiter); 189 | void printBytes(uint8_t *buf, uint8_t len); 190 | void printPWM(uint16_t *ch, uint8_t num_of_channels); 191 | void printLinkStats(); 192 | private: 193 | uint8_t crc8_dvb_s2(uint8_t, unsigned char); 194 | uint8_t crc8_dvb_s2_update(uint8_t, const void *, uint32_t); 195 | int32_t bytes2int32(uint8_t *byt); 196 | uint16_t bytes2uint16(uint8_t *byt);; 197 | int16_t bytes2int16(uint8_t *byt); 198 | uint16_t wrap360(int16_t); 199 | bool fixBadRc(uint8_t *); 200 | void prepSBUS(uint8_t *rc_buf, uint8_t *sb_buf, bool _los, bool _failsafe); 201 | #if defined SUPPORT_SBUS_OUT 202 | void sendSBUS(uint8_t *sb_buf); 203 | #endif 204 | bool bytesToPWM(uint8_t *sb_byte, uint16_t *ch_val, uint8_t max_ch); 205 | void pwmToBytes(uint16_t *in_pwm, uint8_t *rc_byt, uint8_t max_ch); 206 | 207 | }; // end of class -------------------------------------------------------------------------------- /lib/terseCRSF/terseCRSF.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | /* 3 | CRSF::CRSF() : 4 | crsf_crc(0xd5), 5 | blah(0) 6 | { 7 | // constructor for CRSF class 8 | } 9 | */ 10 | //======================================================= 11 | bool CRSF::sbus_initialise(Stream &port) 12 | { 13 | this->sbus_port = &port; // object pointer 14 | return true; 15 | } 16 | //======================================================= 17 | bool CRSF::initialise(Stream &port) 18 | { 19 | this->crsf_port = &port; // object pointer 20 | log.print("terseCRSF by zs6buj"); 21 | log.printf(" version:%d.%02d.%02d\n", MAJOR_VER, MINOR_VER, PATCH_LEV); 22 | #if defined RC_BUILD 23 | log.println("RC Build. Expected source is EdgeTX/OpenTX"); 24 | #else 25 | log.print("Telemetry Build. "); 26 | #if (TELEMETRY_SOURCE == 1) // BetaFlight 27 | log.println("Expected source is BetaFlight/CF"); 28 | #elif (TELEMETRY_SOURCE == 2) // EdgeTX/OpenTx 29 | log.println("Expected source is EdgeTX/OpenTX"); 30 | #endif 31 | 32 | #endif 33 | return true; 34 | } 35 | //======================================================= 36 | uint8_t crc8_dvb_s2(uint8_t crc, unsigned char a) 37 | { 38 | crc ^= a; 39 | for (int ii = 0; ii < 8; ++ii) { 40 | if (crc & 0x80) { 41 | crc = (crc << 1) ^ 0xD5; 42 | } else { 43 | crc = crc << 1; 44 | } 45 | } 46 | return crc; 47 | } 48 | //======================================================= 49 | uint8_t crc8_dvb_s2_sbuf_accum(const void *data, uint8_t frm_lth) 50 | { 51 | uint8_t crc = 0; 52 | const uint8_t *p = (const uint8_t *)data; 53 | const uint8_t *pend = p + frm_lth; 54 | 55 | for (; p != pend; p++) 56 | { 57 | //log.printf("crc *p 0x%2X\n", *p); 58 | crc = crc8_dvb_s2(crc, *p); 59 | } 60 | return crc; 61 | } 62 | //======================================================= 63 | int32_t CRSF::bytes2int32(uint8_t *byt) 64 | { 65 | return ((byt[3] << 0) & 0xFF) + ((byt[2] << 8) & 0xFFFF) + ((byt[1] << 16) & 0xFFFFFF) + ((byt[0] << 24) & 0xFFFFFFFF); 66 | } 67 | //======================================================= 68 | uint16_t CRSF::bytes2uint16(uint8_t *byt) 69 | { 70 | return ((byt[1] << 0) & 0xFF) + ((byt[0] << 8) & 0xFFFF); 71 | } 72 | //======================================================= 73 | int16_t CRSF::bytes2int16(uint8_t *byt) 74 | { 75 | return ((byt[1] << 0) & 0xFF) + ((byt[0] << 8) & 0xFFFF); 76 | } 77 | //======================================================= 78 | void CRSF::printByte(byte b, char delimiter) 79 | { 80 | if (b <= 0xf) 81 | log.print("0"); 82 | log.print(b, HEX); 83 | log.write(delimiter); 84 | } 85 | //======================================================== 86 | void CRSF::printPWM(uint16_t *ch, uint8_t num_of_channels) 87 | { 88 | log.print("PWM: "); 89 | for (int i = 0; i < num_of_channels; i++) 90 | { 91 | log.printf("%2d:%4d ", i + 1, *(ch + i)); 92 | } 93 | // log.printf(" rssi:%d", pwm_rssi ); 94 | log.println(); 95 | } 96 | //======================================================== 97 | void CRSF::printBytes(uint8_t *buf, uint8_t len) 98 | { 99 | //log.printf("len:%2u:", len); 100 | for (int i = 0; i < len; i++) 101 | { 102 | printByte(buf[i], ' '); 103 | } 104 | log.println(); 105 | } 106 | //======================================================== 107 | void CRSF::printLinkStats() 108 | { 109 | #if defined SHOW_LINK_STATS 110 | static uint32_t error_millis = 0; 111 | if ((millis() - error_millis) > 1.2E5) // 2 minutes 112 | { 113 | error_millis = millis(); 114 | log.printf("frames_C:%u good_frames:%u crc_errors:%u frame_errors:%u unknown_ids:%u\n", frames_read, good_frames, crc_errors, frame_errors, unknown_ids); 115 | } 116 | #endif 117 | } 118 | //=================================================================== 119 | uint16_t CRSF::wrap360(int16_t ang) 120 | { 121 | if (ang < 0) 122 | ang += 360; 123 | if (ang > 359) 124 | ang -= 360; 125 | return ang; 126 | } 127 | //=================================================================== 128 | void CRSF::prepSBUS(uint8_t *rc_buf, uint8_t *sb_buf, bool _los, bool _failsafe) 129 | { 130 | uint8_t statusByte = 0x00; 131 | if (_los) 132 | { // loss of signal 133 | statusByte |= 0x04; 134 | } 135 | if (_failsafe) 136 | { 137 | statusByte |= 0x08; 138 | } 139 | 140 | sb_buf[0] = 0x0F; // sbus byte [0], header byte 141 | for (int i = 0; i < 22; i++) 142 | { // rc byte [0] thru [21] 143 | sb_buf[i + 1] = rc_buf[i]; // sbus byte [1] thru [22] 144 | } 145 | 146 | sb_buf[23] = statusByte; // sbus byte [23], status byte 147 | sb_buf[24] = 0x00; // sbus byte [24], footer byte 148 | } 149 | //================================================================== 150 | bool CRSF::bytesToPWM(uint8_t *sb_byte, uint16_t *ch_val, uint8_t max_ch) 151 | { 152 | // remember these are SBus PWM values, which range from 192 thru 1792, not 1000 thru 2000 153 | *ch_val = ((*sb_byte | *(sb_byte + 1) << 8) & 0x07FF); 154 | *(ch_val + 1) = ((*(sb_byte + 1) >> 3 | *(sb_byte + 2) << 5) & 0x07FF); 155 | *(ch_val + 2) = ((*(sb_byte + 2) >> 6 | *(sb_byte + 3) << 2 | *(sb_byte + 4) << 10) & 0x07FF); 156 | *(ch_val + 3) = ((*(sb_byte + 4) >> 1 | *(sb_byte + 5) << 7) & 0x07FF); 157 | *(ch_val + 4) = ((*(sb_byte + 5) >> 4 | *(sb_byte + 6) << 4) & 0x07FF); 158 | *(ch_val + 5) = ((*(sb_byte + 6) >> 7 | *(sb_byte + 7) << 1 | *(sb_byte + 8) << 9) & 0x07FF); 159 | *(ch_val + 6) = ((*(sb_byte + 8) >> 2 | *(sb_byte + 9) << 6) & 0x07FF); 160 | *(ch_val + 7) = ((*(sb_byte + 9) >> 5 | *(sb_byte + 10) << 3) & 0x07FF); 161 | 162 | if ((max_ch == 16) || (max_ch == 24)) 163 | { 164 | *(ch_val + 8) = ((*(sb_byte + 11) | *(sb_byte + 12) << 8) & 0x07FF); 165 | *(ch_val + 9) = ((*(sb_byte + 12) >> 3 | *(sb_byte + 13) << 5) & 0x07FF); 166 | *(ch_val + 10) = ((*(sb_byte + 13) >> 6 | *(sb_byte + 14) << 2 | *(sb_byte + 15) << 10) & 0x07FF); 167 | *(ch_val + 11) = ((*(sb_byte + 15) >> 1 | *(sb_byte + 16) << 7) & 0x07FF); 168 | *(ch_val + 12) = ((*(sb_byte + 16) >> 4 | *(sb_byte + 17) << 4) & 0x07FF); 169 | *(ch_val + 13) = ((*(sb_byte + 17) >> 7 | *(sb_byte + 18) << 1 | *(sb_byte + 19) << 9) & 0x07FF); 170 | *(ch_val + 14) = ((*(sb_byte + 19) >> 2 | *(sb_byte + 20) << 6) & 0x07FF); 171 | *(ch_val + 15) = ((*(sb_byte + 20) >> 5 | *(sb_byte + 21) << 3) & 0x07FF); 172 | } 173 | 174 | if (max_ch == 24) 175 | { 176 | *(ch_val + 16) = ((*(sb_byte + 22) | *(sb_byte + 23) << 8) & 0x07FF); 177 | *(ch_val + 17) = ((*(sb_byte + 23) >> 3 | *(sb_byte + 24) << 5) & 0x07FF); 178 | *(ch_val + 18) = ((*(sb_byte + 24) >> 6 | *(sb_byte + 25) << 2 | *(sb_byte + 26) << 10) & 0x07FF); 179 | *(ch_val + 19) = ((*(sb_byte + 26) >> 1 | *(sb_byte + 27) << 7) & 0x07FF); 180 | *(ch_val + 20) = ((*(sb_byte + 27) >> 4 | *(sb_byte + 28) << 4) & 0x07FF); 181 | *(ch_val + 21) = ((*(sb_byte + 28) >> 7 | *(sb_byte + 29) << 1 | *(sb_byte + 30) << 9) & 0x07FF); 182 | *(ch_val + 22) = ((*(sb_byte + 30) >> 2 | *(sb_byte + 31) << 6) & 0x07FF); 183 | *(ch_val + 23) = ((*(sb_byte + 31) >> 5 | *(sb_byte + 32) << 3) & 0x07FF); 184 | } 185 | 186 | for (int i = max_ch; i < 25; i++) 187 | { // pad out pwm to 24 channels with 1500 188 | *(ch_val + i) = 1500; 189 | } 190 | // pwm_rssi = *(sb_byte+23); // if we have a designated rssi channel 191 | // remap SBUS pwm values in the range of 192 - 1792 (0x00 thu 0xFF) to regular pwm values 192 | for (int i = 0; i < max_ch; i++) 193 | { 194 | if (*(ch_val + i) > 0) 195 | { 196 | *(ch_val + i) = map(*(ch_val + i), 192, 1792, 1000, 2000); // regular PWM uS limits 197 | } 198 | } 199 | if (*ch_val > 0) 200 | { 201 | return true; 202 | } 203 | else 204 | { 205 | return false; 206 | } 207 | } 208 | //=============================================================== 209 | void CRSF::pwmToBytes(uint16_t *in_pwm, uint8_t *rc_byt, uint8_t max_ch) 210 | { 211 | uint16_t ch_pwm[max_ch] {}; 212 | // remap PWM values to sbus byte values in the range of 192 - 1792 (0x00 thu 0xFF) 213 | for (int i = 0; i < max_ch; i++) 214 | { 215 | if (*(in_pwm + i) > 0) 216 | { 217 | *(ch_pwm + i) = map(*(in_pwm + i), 1000, 2000, 192, 1792); // new SBUS uS limits 218 | //log.printf("in_pwm:%u ch_pwm:%u", *(in_pwm + i), *(ch_pwm + i)); 219 | } 220 | } 221 | //log.println(); 222 | uint8_t byte_cnt = 0; 223 | uint8_t ch_cnt = 0; 224 | uint8_t rc_bit = 0; 225 | uint8_t ch_bit = 0; 226 | 227 | ch_bit = 0; 228 | byte_cnt = 0; // no header 229 | rc_bit = 0; 230 | 231 | uint16_t max_bits = max_ch * 11; // each pwm ch = 11 bits 8 x 11 = 88 or 8 x 22 = 176 or 8 x 33 = 264 232 | for (int i = 0; i < max_bits; i++) 233 | { 234 | if (*(ch_pwm + ch_cnt) & (1 << ch_bit)) 235 | { 236 | *(rc_byt + byte_cnt) |= (1 << rc_bit); 237 | } 238 | rc_bit++; 239 | ch_bit++; 240 | if (rc_bit == 8) 241 | { // byte full, so increment and do next 242 | rc_bit = 0; 243 | // log.printf("byt:%2X ", rc_byt[byte_cnt]); 244 | byte_cnt++; 245 | } 246 | if (ch_bit == 11) 247 | { // pwm ch overflow, so increment ch_no and do next 248 | // log.printf("ch_pwm:%u ", ch_pwm[ch_cnt]); 249 | ch_bit = 0; 250 | ch_cnt++; 251 | } 252 | } 253 | // log.println(); 254 | } 255 | //======================================================== 256 | bool CRSF::readCrsfFrame(uint8_t &frm_lth) 257 | { 258 | static uint8_t b = 0; 259 | static uint8_t idx = 0; 260 | static uint8_t embed_lth = 0; 261 | static uint8_t crsf_crc = 0; 262 | uint8_t embed_crc = 0; 263 | 264 | while (crsf_port->available()) 265 | { 266 | if (idx == 0) 267 | { 268 | memset(crsf_buf, 0, crsf_buffer_size); // flush the crsf_buf 269 | #if defined RC_BUILD 270 | if (b == CRSF_RC_SYNC_BYTE) // prev read byte 271 | { 272 | crsf_buf[0] = b; // to front of buf 273 | } 274 | } 275 | #else // TELEM BUILD 276 | if (b == CRSF_TEL_SYNC_BYTE) // prev read byte 277 | { 278 | crsf_buf[0] = b; // to front of buf 279 | } 280 | } 281 | #endif 282 | static uint8_t prev_b = 0; 283 | prev_b = b; 284 | b = crsf_port->read(); 285 | #if defined SHOW_BYTE_STREAM 286 | printByte(b, ' '); 287 | static uint8_t col = 0; 288 | col++; 289 | if (col > 35) 290 | { 291 | col = 0; 292 | log.print("\n"); 293 | } 294 | #endif 295 | #if defined RC_BUILD 296 | if (b == CRSF_RC_SYNC_BYTE) 297 | #else 298 | if (b == CRSF_TEL_SYNC_BYTE) // new frame, so now process prev buffer 299 | #endif 300 | { 301 | frames_read++; 302 | frm_lth = idx; 303 | idx = 0; 304 | #if defined RC_BUILD // RC BUILD 305 | //add in crc from and including buf[2] == lth_byte 0x16, series == (EE 18) 16 E0 03 5F 2B C0 .... 306 | crsf_crc = crc8_dvb_s2_sbuf_accum(&crsf_buf[2], frm_lth-3); 307 | embed_crc = crsf_buf[frm_lth - 1]; 308 | #else // TELEMETRY BUILD 309 | crsf_crc = crc8_dvb_s2_sbuf_accum(&crsf_buf[2], frm_lth-2); // lth - start 310 | embed_crc = crsf_buf[frm_lth]; 311 | #endif 312 | if (embed_crc != crsf_crc) 313 | { 314 | crc_errors++; 315 | //log.printf("embedded_crc:0x%2X calc_crc:0x%2X mismatch - lth:%u\n", embed_crc, crsf_crc, frm_lth); 316 | return false; 317 | } 318 | good_frames++; 319 | return true; 320 | } 321 | // prevent array overflow 322 | if (idx < crsf_buffer_size-1) idx++; 323 | crsf_buf[idx] = b; 324 | //log.printf("%u:", idx); 325 | //printByte(b, ' '); 326 | if (idx == 1) 327 | { 328 | embed_lth = b+1; // 2nd byte, + 1 329 | } 330 | } 331 | 332 | return false; // drop thru and loop 333 | } 334 | //=================================================================== 335 | #if defined SUPPORT_SBUS_OUT 336 | void CRSF::sendSBUS(uint8_t *sb_buf) 337 | { 338 | /* 339 | A single SBUS message is 25 bytes long and therefore takes 3ms to be transmitted. 340 | It can be sent every 15mS, and consists of the following bytes: 341 | 342 | 1 Header byte 00001111b (0x0F) 343 | 16 * 11 bit channels -> 22 bytes 344 | 1 status byte for frame-lost (los) and failsafe 345 | 1 Footer byte 00000000b (0x00) 346 | */ 347 | 348 | sbus_port->write(sb_buf, 25); 349 | } 350 | #endif 351 | //======================================================== 352 | uint8_t CRSF::decodeTelemetry(uint8_t *_buf, uint8_t len) 353 | { 354 | uint8_t crsf_frm_lth = _buf[1]; 355 | uint8_t crsf_id = _buf[2]; 356 | if (crsf_id == 0) 357 | { 358 | return 0; 359 | } 360 | #if defined SHOW_BUFFER 361 | log.print("CRSF_BUF:"); 362 | printBytes(&*_buf, len); // plus header and crc bytes 363 | #endif 364 | switch (crsf_id) 365 | { 366 | case GPS_ID: 367 | gps_lat = bytes2int32(&_buf[3]); // offset (&*(_buf+3)) 368 | gps_lon = bytes2int32(&_buf[7]); 369 | gpsF_lat = (float)(gps_lat / 1e7); // degrees+decimals 370 | gpsF_lon = (float)(gps_lon / 1e7); 371 | gps_groundspeed = bytes2uint16(&_buf[11]); 372 | gpsF_groundspeed = (float)(gps_groundspeed * 0.1); // km\hr 373 | gps_heading = bytes2uint16(&_buf[13]); 374 | gpsF_heading = (float)(gps_heading * 0.01); // degrees+decimals 375 | gps_altitude = bytes2uint16(&_buf[15]); // metres, ­1000m offset 376 | gps_altitude = gps_altitude > 100 ? gps_altitude - 1000: gps_altitude; 377 | gps_sats = (uint8_t)_buf[17]; 378 | break; 379 | case CF_VARIO_ID: 380 | #if defined SHOW_CRSF_CF_VARIO 381 | log.print("CF_VARIO:"); 382 | printBytes(&*_buf, len); // plus header and crc bytes 383 | #endif 384 | break; 385 | case BATTERY_ID: 386 | bat_voltage = bytes2uint16(&_buf[3]); // mV * 100 387 | batF_voltage = (float)bat_voltage * 0.1; // volts 388 | bat_current = bytes2uint16(&_buf[5]); // mA * 100 389 | batF_current = bat_current * 0.1; // amps 390 | bat_fuel_drawn = bytes2int32(&_buf[7]); // uint24_t mAh drawn 391 | batF_fuel_drawn = bat_fuel_drawn; // Ah drawn 392 | bat_remaining = (uint8_t)_buf[10]; // percent 393 | break; 394 | case BARO_ALT_ID: 395 | #if defined SHOW_CRSF_BARO 396 | log.print("BARO_ALT:"); 397 | printBytes(&*_buf, len); // plus header and crc bytes 398 | #endif 399 | break; 400 | case HEARTBEAT_ID: 401 | #if defined SHOW_CRSF_HEARTBEAT 402 | log.print("HEARTBEAT:"); 403 | printBytes(&*_buf, len); // plus header and crc bytes 404 | #endif 405 | break; 406 | case LINK_ID: // 0x14 Link statistics 407 | link_up_rssi_ant_1 = (uint8_t)_buf[3]; // dBm * -1 408 | link_up_rssi_ant_2 = (uint8_t)_buf[4]; // dBm * -1 409 | link_up_quality = (uint8_t)_buf[5]; // packet_success_rate (%) 410 | link_up_snr = (int8_t)_buf[6]; // db 411 | link_diversity_active_ant = (uint8_t)_buf[7]; // (enum ant_1 = 0, ant_2) 412 | link_rf_mode = (uint8_t)_buf[8]; // (enum 4fps = 0, 50fps, 150hz) 413 | link_up_tx_power = (uint8_t)_buf[9]; // (enum 0mW = 0, 10mW, 25 mW, 100 mW, 500 mW, 1000 mW, 2000mW) 414 | link_dn_rssi = (uint8_t)_buf[10]; // RSSI(dBm * -1) 415 | link_dn_quality = (uint8_t)_buf[11]; // packet_success_rate (%) 416 | link_dn_snr = (int8_t)_buf[12]; // db 417 | break; 418 | case CHANNELS_ID: 419 | #if defined SHOW_CRSF_CHANNELS 420 | log.print("CHANNELS:"); 421 | printBytes(&*_buf, len); // plus header and crc bytes 422 | #endif 423 | break; 424 | case LINK_RX_ID: 425 | #if defined SHOW_CRSF_LINK_RX 426 | log.print("LINK_RX:"); 427 | printBytes(&*_buf, len); // plus header and crc bytes 428 | #endif 429 | break; 430 | case LINK_TX_ID: 431 | #if defined SHOW_CRSF_LINK_TX 432 | log.print("LINK_TX:"); 433 | printBytes(&*_buf, len); // plus header and crc bytes 434 | #endif 435 | break; 436 | case ATTITUDE_ID: 437 | atti_pitch = bytes2int16(&_buf[3]); // rad / 10000 438 | atti_roll = bytes2int16(&_buf[5]); // rad / 10000 439 | atti_yaw = bytes2int16(&_buf[7]); // rad / 10000 440 | attiF_pitch = (float)(atti_pitch * RADS2DEGS * 0.0001); // deg 441 | attiF_roll = (float)(atti_roll * RADS2DEGS * 0.0001); // deg 442 | atti_yaw = (int16_t)(atti_yaw * RADS2DEGS * 0.0001); // deg 443 | atti_yaw = wrap360(atti_yaw); 444 | attiF_yaw = (float)atti_yaw; 445 | break; 446 | case FLIGHT_MODE_ID: 447 | /* HUH! Flight mode is a string*/ 448 | flight_mode_lth = crsf_frm_lth - 3; // fix 2024-05-17 449 | flightMode.resize(flight_mode_lth); // fix 2024-09-12 450 | memcpy(&flightMode[0], &_buf[3], flight_mode_lth); // fix 2024-05-17 451 | //printBytes(&_buf[3], flight_mode_lth); 452 | break; 453 | case PING_DEVICES_ID: 454 | #if defined SHOW_CRSF_GPS_PING_DEVICES 455 | log.print("PING_DEVICES:"); 456 | printBytes(&*_buf, len); // plus header and crc bytes 457 | #endif 458 | break; 459 | case DEVICE_INFO_ID: 460 | #if defined SHOW_CRSF_DEVIDE_INFO 461 | log.print("DEVICE_INFO:"); 462 | printBytes(&*_buf, len); // plus header and crc bytes 463 | #endif 464 | break; 465 | case REQUEST_SETTINGS_ID: 466 | #if defined SHOW_CRSF_REQUEST_SETTINGS 467 | log.print("REQUEST_SETTINGS:"); 468 | printBytes(&*_buf, len); // plus header and crc bytes 469 | #endif 470 | break; 471 | case COMMAND_ID: 472 | #if defined SHOW_CRSF_COMMAND 473 | log.print("COMMAND:"); 474 | printBytes(&*_buf, len); // plus header and crc bytes 475 | #endif 476 | break; 477 | case RADIO_ID: 478 | #if defined SHOW_CRSF_RADIO 479 | log.print("RADIO id:"); 480 | printBytes(&*_buf, len); // plus header and crc bytes 481 | #endif 482 | break; 483 | default: 484 | #if defined SHOW_OTHER_FRAME_IDs 485 | log.print("crsf_id:"); 486 | printByte(crsf_id, ' '); 487 | log.println(); 488 | //log.print("UNKNOWN "); 489 | //printBytes(&*_buf, len); // plus header and CRC bytes 490 | #endif 491 | unknown_ids++; 492 | return 0; 493 | } 494 | return crsf_id; 495 | } 496 | //======================================================== 497 | void CRSF::decodeRC() 498 | { 499 | #if defined SHOW_BUFFER 500 | log.print("CRSF_BUF:"); 501 | printBytes(&*crsf_buf, 26); // sync byte(0xEE) + 2 + 22 RC bytes +CRC 502 | #endif 503 | bytesToPWM(&*(crsf_buf+3), &*pwm_val, max_ch); // note skip 3B 504 | #if defined SUPPORT_SBUS_OUT || defined SHOW_SBUS 505 | memcpy(&*rc_bytes, &*(crsf_buf + 3), 22); // note skip 3B 506 | prepSBUS(&*rc_bytes, &*sb_bytes, false, false); 507 | #endif 508 | #if defined SUPPORT_SBUS_OUT 509 | sendSBUS(&*sb_bytes); 510 | #endif 511 | 512 | } --------------------------------------------------------------------------------