From 5d6f646a5ef88693f5f88d36b3f2b6b53ad0707e Mon Sep 17 00:00:00 2001 From: Francesco Zanin Date: Wed, 24 Jun 2026 16:55:41 +0200 Subject: [PATCH] Pulizia: rimosso codice morto RC/SBUS/PWM e uplink ESP-NOW inutilizzato terseCRSF vendorizzata: rimosse initialise/sbus_initialise/readCrsfFrame/ decodeRC/printPWM/prepSBUS/bytesToPWM/pwmToBytes/sendSBUS e i membri/buffer SBUS-PWM (sb_bytes, pwm_val, crsf_port, sbus_port, RC_BUILD/SBUS machinery). Usiamo solo decodeTelemetry: mantenuti i decoder + helper (bytes2*, wrap360, crc8_dvb_s2[_sbuf_accum] usata dalla patch CRC, printByte/printBytes). ~283 righe in meno, nessun riferimento pendente (verificato con grep su 01/02/03). EspNowCrsf: rimosso sendRaw() e il peer broadcast in begin() (uplink sperimentale morto, il backpack e' downlink-only). Ora RX-only pulito. main.cpp: commento "MAVLink v1" -> "v2" (la lib emette v2). DA VERIFICARE A BANCO con un build (qui non compilabile). Co-Authored-By: Claude Opus 4.8 --- .../lib/EspNowCrsf/EspNowCrsf.cpp | 12 - .../lib/EspNowCrsf/EspNowCrsf.h | 10 +- .../lib/terseCRSF/src/terseCRSF.cpp | 283 ------------------ .../lib/terseCRSF/src/terseCRSF.h | 31 +- 02_telemetry_bridge/src/main.cpp | 2 +- 5 files changed, 4 insertions(+), 334 deletions(-) diff --git a/02_telemetry_bridge/lib/EspNowCrsf/EspNowCrsf.cpp b/02_telemetry_bridge/lib/EspNowCrsf/EspNowCrsf.cpp index 19d532c..c6e2b52 100644 --- a/02_telemetry_bridge/lib/EspNowCrsf/EspNowCrsf.cpp +++ b/02_telemetry_bridge/lib/EspNowCrsf/EspNowCrsf.cpp @@ -56,13 +56,6 @@ void begin(const uint8_t uid[6]) { if (esp_now_init() != ESP_OK) return; esp_now_register_recv_cb(onRecv); - - // Peer broadcast per l'invio sperimentale (uplink) - esp_now_peer_info_t peer = {}; - memset(peer.peer_addr, 0xFF, 6); // broadcast - peer.channel = 1; - peer.encrypt = false; - esp_now_add_peer(&peer); } bool poll(uint8_t* out, uint16_t* outLen, uint16_t maxLen) { @@ -83,11 +76,6 @@ bool poll(uint8_t* out, uint16_t* outLen, uint16_t maxLen) { uint32_t framesReceived() { return s_received; } uint32_t framesDropped() { return s_dropped; } -bool sendRaw(const uint8_t* data, uint16_t len) { - static const uint8_t bcast[6] = {0xFF,0xFF,0xFF,0xFF,0xFF,0xFF}; - return esp_now_send(bcast, data, len) == ESP_OK; -} - #ifdef UNIT_TEST void test_injectRaw(const uint8_t* data, uint16_t len) { onRecv(nullptr, data, len); } diff --git a/02_telemetry_bridge/lib/EspNowCrsf/EspNowCrsf.h b/02_telemetry_bridge/lib/EspNowCrsf/EspNowCrsf.h index 1252dd9..95d3392 100644 --- a/02_telemetry_bridge/lib/EspNowCrsf/EspNowCrsf.h +++ b/02_telemetry_bridge/lib/EspNowCrsf/EspNowCrsf.h @@ -1,11 +1,9 @@ // ============================================================ -// EspNowCrsf - ricezione (e invio sperimentale) di frame CRSF -// trasportati su ESP-NOW dal TX Backpack ExpressLRS. +// EspNowCrsf - ricezione di frame CRSF trasportati su ESP-NOW +// dal TX Backpack ExpressLRS. // // - begin(uid): imposta WiFi STA, MAC=UID, canale 1, ESP-NOW. // - poll(): estrae il prossimo frame CRSF grezzo ricevuto. -// - sendRaw(): invio ESP-NOW (uplink) -- vedi nota uplink in -// mode_msp.cpp: NON raggiunge l'FC con ELRS stock. // // Il frame CRSF restituito inizia al sync byte (0xC8/0xEA): // [0]=sync [1]=len [2]=type [3..]=payload [..]=crc @@ -26,10 +24,6 @@ bool poll(uint8_t* out, uint16_t* outLen, uint16_t maxLen); uint32_t framesReceived(); uint32_t framesDropped(); -// Invio ESP-NOW grezzo verso il peer (broadcast). Sperimentale. -// Ritorna true se esp_now_send ha accettato il pacchetto. -bool sendRaw(const uint8_t* data, uint16_t len); - #ifdef UNIT_TEST // Hook di test (definita solo in build di test, PlatformIO imposta // UNIT_TEST automaticamente per `pio test`: zero impatto sul firmware diff --git a/02_telemetry_bridge/lib/terseCRSF/src/terseCRSF.cpp b/02_telemetry_bridge/lib/terseCRSF/src/terseCRSF.cpp index b400451..fe7f6b2 100644 --- a/02_telemetry_bridge/lib/terseCRSF/src/terseCRSF.cpp +++ b/02_telemetry_bridge/lib/terseCRSF/src/terseCRSF.cpp @@ -8,31 +8,6 @@ CRSF::CRSF() : } */ //======================================================= -bool CRSF::sbus_initialise(Stream &port) -{ - this->sbus_port = &port; // object pointer -return true; -} -//======================================================= -bool CRSF::initialise(Stream &port) -{ - this->crsf_port = &port; // object pointer - log.print("terseCRSF by zs6buj"); - log.printf(" version:%d.%02d.%02d\n", MAJOR_VER, MINOR_VER, PATCH_LEV); -#if defined RC_BUILD - log.println("RC Build. Expected source is EdgeTX/OpenTX"); -#else - log.print("Telemetry Build. "); -#if (TELEMETRY_SOURCE == 1) // BetaFlight - log.println("Expected source is BetaFlight/CF"); -#elif (TELEMETRY_SOURCE == 2) // EdgeTX/OpenTx - log.println("Expected source is EdgeTX/OpenTX"); -#endif - -#endif -return true; -} -//======================================================= uint8_t crc8_dvb_s2(uint8_t crc, unsigned char a) { crc ^= a; @@ -83,17 +58,6 @@ void CRSF::printByte(byte b, char delimiter) log.write(delimiter); } //======================================================== -void CRSF::printPWM(uint16_t *ch, uint8_t num_of_channels) -{ - log.print("PWM: "); - for (int i = 0; i < num_of_channels; i++) - { - log.printf("%2d:%4d ", i + 1, *(ch + i)); - } - log.printf(" rssi:%d%%", rssi_percent ); - log.println(); -} -//======================================================== void CRSF::printBytes(uint8_t *buf, uint8_t len) { //log.printf("len:%2u:", len); @@ -124,231 +88,6 @@ uint16_t CRSF::wrap360(int16_t ang) ang -= 360; return ang; } -//=================================================================== -void CRSF::prepSBUS(uint8_t *rc_buf, uint8_t *sb_buf, bool _los, bool _failsafe) -{ - uint8_t statusByte = 0x00; - if (_los) - { // loss of signal - statusByte |= 0x04; - } - if (_failsafe) - { - statusByte |= 0x08; - } - - uint16_t sbus_rssi_pwm = map(rssi_percent, 0, 100, 192, 1792); - - sb_buf[0] = 0x0F; // sbus byte [0], header byte - for (int i = 0; i < 22; i++) - { // rc byte [0] thru [21] - sb_buf[i + 1] = rc_buf[i]; // sbus byte [1] thru [22] - } - - sb_buf[23] = statusByte; // sbus byte [23], status byte - sb_buf[24] = 0x00; // sbus byte [24], footer byte -} -//================================================================== -bool CRSF::bytesToPWM(uint8_t *sb_byte, uint16_t *ch_val, uint8_t max_ch, uint8_t rssi_per) -{ - // remember these are SBus PWM values, which range from 192 thru 1792, not 1000 thru 2000 - *ch_val = ((*sb_byte | *(sb_byte + 1) << 8) & 0x07FF); - *(ch_val + 1) = ((*(sb_byte + 1) >> 3 | *(sb_byte + 2) << 5) & 0x07FF); - *(ch_val + 2) = ((*(sb_byte + 2) >> 6 | *(sb_byte + 3) << 2 | *(sb_byte + 4) << 10) & 0x07FF); - *(ch_val + 3) = ((*(sb_byte + 4) >> 1 | *(sb_byte + 5) << 7) & 0x07FF); - *(ch_val + 4) = ((*(sb_byte + 5) >> 4 | *(sb_byte + 6) << 4) & 0x07FF); - *(ch_val + 5) = ((*(sb_byte + 6) >> 7 | *(sb_byte + 7) << 1 | *(sb_byte + 8) << 9) & 0x07FF); - *(ch_val + 6) = ((*(sb_byte + 8) >> 2 | *(sb_byte + 9) << 6) & 0x07FF); - *(ch_val + 7) = ((*(sb_byte + 9) >> 5 | *(sb_byte + 10) << 3) & 0x07FF); - - if ((max_ch == 16) || (max_ch == 24)) - { - *(ch_val + 8) = ((*(sb_byte + 11) | *(sb_byte + 12) << 8) & 0x07FF); - *(ch_val + 9) = ((*(sb_byte + 12) >> 3 | *(sb_byte + 13) << 5) & 0x07FF); - *(ch_val + 10) = ((*(sb_byte + 13) >> 6 | *(sb_byte + 14) << 2 | *(sb_byte + 15) << 10) & 0x07FF); - *(ch_val + 11) = ((*(sb_byte + 15) >> 1 | *(sb_byte + 16) << 7) & 0x07FF); - *(ch_val + 12) = ((*(sb_byte + 16) >> 4 | *(sb_byte + 17) << 4) & 0x07FF); - *(ch_val + 13) = ((*(sb_byte + 17) >> 7 | *(sb_byte + 18) << 1 | *(sb_byte + 19) << 9) & 0x07FF); - *(ch_val + 14) = ((*(sb_byte + 19) >> 2 | *(sb_byte + 20) << 6) & 0x07FF); - *(ch_val + 15) = ((*(sb_byte + 20) >> 5 | *(sb_byte + 21) << 3) & 0x07FF); - } - - if (max_ch == 24) - { - *(ch_val + 16) = ((*(sb_byte + 22) | *(sb_byte + 23) << 8) & 0x07FF); - *(ch_val + 17) = ((*(sb_byte + 23) >> 3 | *(sb_byte + 24) << 5) & 0x07FF); - *(ch_val + 18) = ((*(sb_byte + 24) >> 6 | *(sb_byte + 25) << 2 | *(sb_byte + 26) << 10) & 0x07FF); - *(ch_val + 19) = ((*(sb_byte + 26) >> 1 | *(sb_byte + 27) << 7) & 0x07FF); - *(ch_val + 20) = ((*(sb_byte + 27) >> 4 | *(sb_byte + 28) << 4) & 0x07FF); - *(ch_val + 21) = ((*(sb_byte + 28) >> 7 | *(sb_byte + 29) << 1 | *(sb_byte + 30) << 9) & 0x07FF); - *(ch_val + 22) = ((*(sb_byte + 30) >> 2 | *(sb_byte + 31) << 6) & 0x07FF); - *(ch_val + 23) = ((*(sb_byte + 31) >> 5 | *(sb_byte + 32) << 3) & 0x07FF); - } - - for (int i = max_ch; i < 25; i++) - { // pad out pwm to 24 channels with 1500 - *(ch_val + i) = 1500; - } - - uint16_t rssi_pwm = *(ch_val + RSSI_CHANNEL-1); // 1000–2000 µs - rssi_per = map(rssi_pwm, 1000, 2000, 0, 100); - - // remap SBUS pwm values in the range of 192 - 1792 (0x00 thu 0xFF) to regular pwm values - for (int i = 0; i < max_ch; i++) - { - if (*(ch_val + i) > 0) - { - *(ch_val + i) = map(*(ch_val + i), 192, 1792, 1000, 2000); // regular PWM uS limits - } - } - if (*ch_val > 0) - { - return true; - } - else - { - return false; - } -} -//=============================================================== -void CRSF::pwmToBytes(uint16_t *in_pwm, uint8_t *rc_byt, uint8_t max_ch) -{ - uint16_t ch_pwm[max_ch] {}; - // remap PWM values to sbus byte values in the range of 192 - 1792 (0x00 thu 0xFF) - for (int i = 0; i < max_ch; i++) - { - if (*(in_pwm + i) > 0) - { - *(ch_pwm + i) = map(*(in_pwm + i), 1000, 2000, 192, 1792); // new SBUS uS limits - //log.printf("in_pwm:%u ch_pwm:%u", *(in_pwm + i), *(ch_pwm + i)); - } - } - //log.println(); - uint8_t byte_cnt = 0; - uint8_t ch_cnt = 0; - uint8_t rc_bit = 0; - uint8_t ch_bit = 0; - - ch_bit = 0; - byte_cnt = 0; // no header - rc_bit = 0; - - 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 - for (int i = 0; i < max_bits; i++) - { - if (*(ch_pwm + ch_cnt) & (1 << ch_bit)) - { - *(rc_byt + byte_cnt) |= (1 << rc_bit); - } - rc_bit++; - ch_bit++; - if (rc_bit == 8) - { // byte full, so increment and do next - rc_bit = 0; - // log.printf("byt:%2X ", rc_byt[byte_cnt]); - byte_cnt++; - } - if (ch_bit == 11) - { // pwm ch overflow, so increment ch_no and do next - // log.printf("ch_pwm:%u ", ch_pwm[ch_cnt]); - ch_bit = 0; - ch_cnt++; - } - } - // log.println(); -} -//======================================================== -bool CRSF::readCrsfFrame(uint8_t &frm_lth) -{ - static uint8_t b = 0; - static uint8_t idx = 0; - static uint8_t embed_lth = 0; - static uint8_t crsf_crc = 0; - uint8_t embed_crc = 0; - - while (crsf_port->available()) - { - if (idx == 0) - { - memset(crsf_buf, 0, crsf_buffer_size); // flush the crsf_buf -#if defined RC_BUILD - if (b == CRSF_RC_SYNC_BYTE) // prev read byte -#else // TELEM BUILD - if (b == CRSF_TEL_SYNC_BYTE) // prev read byte -#endif - { - crsf_buf[0] = b; // to front of buf - } - } - static uint8_t prev_b = 0; - prev_b = b; - b = crsf_port->read(); -#if defined SHOW_BYTE_STREAM - printByte(b, ' '); - static uint8_t col = 0; - col++; - if (col > 35) - { - col = 0; - log.print("\n"); - } -#endif -#if defined RC_BUILD - if (b == CRSF_RC_SYNC_BYTE) -#else - if (b == CRSF_TEL_SYNC_BYTE) // new frame, so now process prev buffer -#endif - { - frames_read++; - frm_lth = idx; - idx = 0; - crsf_crc = crc8_dvb_s2_sbuf_accum(&crsf_buf[2], frm_lth-2); // lth - start - embed_crc = crsf_buf[frm_lth]; - if (embed_crc != crsf_crc) - { - crc_errors++; - log.printf("embedded_crc:0x%2X calc_crc:0x%2X mismatch - lth:%u\n", embed_crc, crsf_crc, frm_lth); - return false; - } - good_frames++; - return true; - } - // prevent array overflow - if (idx < crsf_buffer_size-1) - { - idx++; - crsf_buf[idx] = b; - } - //log.printf("%u:", idx); - //printByte(b, ' '); - if (idx == 1) - { - embed_lth = b+1; // 2nd byte, + 1 - } - } - - return false; // drop thru and loop -} -//=================================================================== -#if defined SUPPORT_SBUS_OUT -void CRSF::sendSBUS() -{ - /* - A single SBUS message is 25 bytes long and therefore takes 3ms to be transmitted. - It can be sent every 15mS, and consists of the following bytes: - - 1 Header byte 00001111b (0x0F) - 16 * 11 bit channels -> 22 bytes - 1 status byte for frame-lost (los) and failsafe - 1 Footer byte 00000000b (0x00) - */ - - size_t written = sbus_port->write(sb_bytes, sizeof(sb_bytes)); - #if defined DEMO_SBUS - log.printf("SBUS frame sent (%u bytes)\n", written); - #endif -} -#endif //======================================================== uint8_t CRSF::decodeTelemetry(uint8_t *_buf, uint8_t len) { @@ -508,26 +247,4 @@ uint8_t CRSF::decodeTelemetry(uint8_t *_buf, uint8_t len) return 0; } return crsf_id; -} -//======================================================== -void CRSF::decodeRC(uint8_t *_buf) -{ -#if defined SHOW_BUFFER - log.print("RC_BUF:"); - printBytes(&*_buf, 22); // 22 RC bytes -#endif - bytesToPWM(&*(_buf), &*pwm_val, max_ch, rssi_percent); - #if defined DEMO_PWM_VALUES - printPWM(&*pwm_val, max_ch); - #endif -#if defined SUPPORT_SBUS_OUT - prepSBUS(_buf, &*sb_bytes, false, false); - #if defined DEMO_SBUS - log.print("SBUS:"); - printBytes(&*sb_bytes, 25); -#endif -#endif -#if defined SUPPORT_SBUS_OUT - sendSBUS(); -#endif } \ No newline at end of file diff --git a/02_telemetry_bridge/lib/terseCRSF/src/terseCRSF.h b/02_telemetry_bridge/lib/terseCRSF/src/terseCRSF.h index a45cd81..67e67cf 100644 --- a/02_telemetry_bridge/lib/terseCRSF/src/terseCRSF.h +++ b/02_telemetry_bridge/lib/terseCRSF/src/terseCRSF.h @@ -24,12 +24,6 @@ #include #include -#define RC_BUILD // else TELEMETRY_BUILD -#if defined RC_BUILD - #define RSSI_CHANNEL 16 // Often channel 16 - #define SUPPORT_SBUS_OUT -#endif - #define MAJOR_VER 0 #define MINOR_VER 0 #define PATCH_LEV 9 @@ -112,23 +106,14 @@ class CRSF // Data members public: // quote "416KBaud that CRSF uses", ELRS uses 420000 -#if defined RC_BUILD const uint32_t crfs_baud = 420000; // works for both -#else -const uint32_t crfs_baud = 420000; // works for both -#endif const uint8_t crsf_buffer_size = 64; -const uint8_t max_rc_bytes = 22; // just the RC bytes, not the full sbus -const uint8_t sbus_buffer_size = 25; // Header(1) + RC_bytes(22) + status(1)(los+fs) + footer(1) -const uint8_t max_ch = 8; // max 18 uint8_t rssi_percent = 0; uint8_t frame_lth = 0; -uint8_t crsf_buf[64] {}; // sizes as per above -uint8_t sb_bytes[25] {}; -uint16_t pwm_val[8] {}; +uint8_t crsf_buf[64] {}; // sizes as per above uint8_t crsf_id = 0; uint8_t crsf_lth = 0; @@ -183,8 +168,6 @@ std::string flightMode; // Member function prototypes private: - Stream* crsf_port; // pointer type - Stream* sbus_port; // pointer type // own link stats uint32_t frames_read = 0; uint32_t good_frames = 0; @@ -194,14 +177,9 @@ private: public: - bool initialise(Stream& port); - bool sbus_initialise(Stream& port); - bool readCrsfFrame(uint8_t <h); uint8_t decodeTelemetry(uint8_t *_buf, uint8_t len); - void decodeRC(uint8_t *_buf); void printByte(byte b, char delimiter); void printBytes(uint8_t *buf, uint8_t len); - void printPWM(uint16_t *ch, uint8_t num_of_channels); void printLinkStats(); private: uint8_t crc8_dvb_s2(uint8_t, unsigned char); @@ -210,12 +188,5 @@ private: uint16_t bytes2uint16(uint8_t *byt);; int16_t bytes2int16(uint8_t *byt); uint16_t wrap360(int16_t); - bool fixBadRc(uint8_t *); - void prepSBUS(uint8_t *rc_buf, uint8_t *sb_buf, bool _los, bool _failsafe); -#if defined SUPPORT_SBUS_OUT - void sendSBUS(); -#endif - bool bytesToPWM(uint8_t *sb_byte, uint16_t *ch_val, uint8_t max_ch, uint8_t rssi_per); - void pwmToBytes(uint16_t *in_pwm, uint8_t *rc_byt, uint8_t max_ch); }; // end of class diff --git a/02_telemetry_bridge/src/main.cpp b/02_telemetry_bridge/src/main.cpp index cb32f07..b2764bd 100644 --- a/02_telemetry_bridge/src/main.cpp +++ b/02_telemetry_bridge/src/main.cpp @@ -6,7 +6,7 @@ // 0 HUMAN - telemetria leggibile su seriale // 1 CRSF_DUMP - dump esadecimale dei frame CRSF // 2 LTM - stream LTM v2 per mwptools (downlink/monitoraggio) -// 3 MAVLINK - stream MAVLink v1 per mwptools (include corrente in A) +// 3 MAVLINK - stream MAVLink v2 per mwptools (include corrente in A) // // SELEZIONE MODO (da monitor seriale @115200): // '!' poi '0'..'3' -> seleziona direttamente quel modo