Snapshot iniziale: bridge telemetria CRSF->LTM/MAVLink (ESP-NOW)
Stato di partenza prima dei fix. Il repo git era inizializzato a meta' (config.lock + objects/ mancante, zero commit): riparato. my_config.h (UID reale, tutti e 3 i sottoprogetti) escluso via .gitignore; aggiunto my_config.h.example. .pio/ escluso. Co-Authored-By: Claude Opus 4.8 <noreply@anthropic.com>
This commit is contained in:
Francesco Zanin
164
01_telemetry_reader/src/main.cpp
Normal file
164
01_telemetry_reader/src/main.cpp
Normal file
@@ -0,0 +1,164 @@
|
||||
// ============================================================
|
||||
// INAV Telemetry Reader via ELRS Backpack (ESP-NOW) - RX only
|
||||
// Board: ESP32 DevKit (ESP32-WROOM)
|
||||
//
|
||||
// Catena dati:
|
||||
// INAV (GPS) --CRSF--> RX ELRS --link 2.4/900--> TX ELRS
|
||||
// --> TX Backpack --ESP-NOW--> QUESTO ESP32 --> Serial
|
||||
//
|
||||
// Requisiti lato radio:
|
||||
// - Firmware ELRS TX+RX >= 3.4, Backpack >= 1.5.1
|
||||
// - Lua ELRS: Backpack -> Telemetry = ESPNOW
|
||||
// - INAV: telemetria CRSF attiva sulla UART del RX
|
||||
// - my_config.h con il TUO UID (hash della bind phrase)
|
||||
//
|
||||
// Il parsing CRSF e' fatto dalla libreria terseCRSF (zs6buj).
|
||||
// ============================================================
|
||||
|
||||
#include <Arduino.h>
|
||||
#include <terseCRSF.h>
|
||||
|
||||
#include <WiFi.h>
|
||||
#include <esp_wifi.h>
|
||||
#include <esp_now.h>
|
||||
|
||||
// ---- UID (dalla bind phrase) -------------------------------
|
||||
#if __has_include("my_config.h")
|
||||
#include "my_config.h"
|
||||
#endif
|
||||
#ifdef MY_CUSTOM_UID
|
||||
uint8_t UID[6] = {30, 45, 22, 134, 74, 141};
|
||||
#else
|
||||
uint8_t UID[6] = {0, 0, 0, 0, 0, 0}; // placeholder: NON funziona finche' non metti il tuo
|
||||
#endif
|
||||
|
||||
// ---- Parser CRSF -------------------------------------------
|
||||
CRSF crsf;
|
||||
|
||||
// ---- Buffer condiviso ISR(callback) <-> loop ---------------
|
||||
static volatile bool frameReady = false;
|
||||
static volatile uint16_t frameLen = 0;
|
||||
static uint8_t espnowBuf[64];
|
||||
static portMUX_TYPE mux = portMUX_INITIALIZER_UNLOCKED;
|
||||
|
||||
// Contatori diagnostici
|
||||
static uint32_t pktCount = 0;
|
||||
|
||||
// ============================================================
|
||||
// Callback ricezione ESP-NOW (firma arduino-esp32 core 2.x)
|
||||
//
|
||||
// Il pacchetto del backpack ha un header di 8 byte; il frame
|
||||
// CRSF grezzo (sync 0xC8, len, type, payload..., crc) inizia
|
||||
// all'offset 8. Replichiamo il comportamento dell'esempio
|
||||
// druckgott/ELRS-Backpack-Example-ESPNOW (provato funzionante).
|
||||
// ============================================================
|
||||
void OnDataRecv(const uint8_t *mac, const uint8_t *incomingData, int len) {
|
||||
if (len <= 8 || incomingData == nullptr) return;
|
||||
|
||||
// Scarta i keep-alive (type/payload a zero subito dopo l'header)
|
||||
// (len > 9 prima degli accessi: con len==9 leggere incomingData[9] sarebbe OOB)
|
||||
if (len > 9 && incomingData[8] == 0 && incomingData[9] == 0) return;
|
||||
|
||||
uint16_t crsfLen = len - 8;
|
||||
if (crsfLen > sizeof(espnowBuf)) crsfLen = sizeof(espnowBuf);
|
||||
|
||||
portENTER_CRITICAL_ISR(&mux);
|
||||
memcpy(espnowBuf, incomingData + 8, crsfLen);
|
||||
frameLen = crsfLen;
|
||||
frameReady = true;
|
||||
portEXIT_CRITICAL_ISR(&mux);
|
||||
}
|
||||
|
||||
// ============================================================
|
||||
// Init radio: STA + 802.11b, MAC = UID, canale 1, ESP-NOW
|
||||
// ============================================================
|
||||
static void initRadio() {
|
||||
UID[0] &= ~0x01; // forza MAC unicast (bit0 = 0)
|
||||
|
||||
WiFi.mode(WIFI_STA);
|
||||
WiFi.disconnect();
|
||||
|
||||
esp_wifi_set_protocol(WIFI_IF_STA, WIFI_PROTOCOL_11B); // obbligatorio per i raw frame del backpack
|
||||
esp_wifi_set_mac(WIFI_IF_STA, UID); // il backpack invia al MAC = UID
|
||||
esp_wifi_start();
|
||||
esp_wifi_set_channel(1, WIFI_SECOND_CHAN_NONE); // backpack ESP-NOW = canale 1
|
||||
|
||||
if (esp_now_init() != ESP_OK) {
|
||||
Serial.println("[ERR] esp_now_init fallito");
|
||||
return;
|
||||
}
|
||||
esp_now_register_recv_cb(OnDataRecv);
|
||||
}
|
||||
|
||||
// ============================================================
|
||||
// Stampa "dashboard" leggibile dello stato corrente (1 Hz)
|
||||
// ============================================================
|
||||
static void printDashboard() {
|
||||
Serial.println(F("----------------------------------------------------"));
|
||||
Serial.printf("Pkt ricevuti : %lu\n", (unsigned long)pktCount);
|
||||
|
||||
// GPS (ID 0x02)
|
||||
Serial.printf("GPS : lat %.7f lon %.7f sat %u\n",
|
||||
crsf.gpsF_lat, crsf.gpsF_lon, crsf.gps_sats);
|
||||
Serial.printf(" alt %d m speed %.1f km/h hdg %.1f deg\n",
|
||||
crsf.gps_altitude, crsf.gpsF_groundspeed, crsf.gpsF_heading);
|
||||
|
||||
// Batteria (ID 0x08)
|
||||
Serial.printf("Batteria : %.1f V %.1f A %lu mAh %u%%\n",
|
||||
crsf.batF_voltage, crsf.batF_current,
|
||||
(unsigned long)crsf.bat_fuel_drawn, crsf.bat_remaining);
|
||||
|
||||
// Assetto (ID 0x1E)
|
||||
Serial.printf("Assetto : pitch %.1f roll %.1f yaw %.1f deg\n",
|
||||
crsf.attiF_pitch, crsf.attiF_roll, crsf.attiF_yaw);
|
||||
|
||||
// Flight mode (ID 0x21)
|
||||
Serial.printf("Flight mode : %s\n", crsf.flightMode.c_str());
|
||||
|
||||
// Link stats (ID 0x14)
|
||||
Serial.printf("Link : RSSI %ddBm LQ %u%% SNR %ddB TXpwr enum %u\n",
|
||||
(int8_t)crsf.link_up_rssi_ant_1, crsf.link_up_quality,
|
||||
crsf.link_up_snr, crsf.link_up_tx_power);
|
||||
}
|
||||
|
||||
void setup() {
|
||||
Serial.begin(115200);
|
||||
delay(3000); // tempo per aprire il monitor seriale
|
||||
Serial.println();
|
||||
Serial.println(F("=== INAV Telemetry Reader (ELRS Backpack / ESP-NOW) ==="));
|
||||
Serial.printf("UID: %u %u %u %u %u %u\n",
|
||||
UID[0], UID[1], UID[2], UID[3], UID[4], UID[5]);
|
||||
if (UID[0]==0 && UID[1]==0 && UID[2]==0) {
|
||||
Serial.println(F("[ATTENZIONE] UID non configurato! Modifica src/my_config.h"));
|
||||
}
|
||||
|
||||
crsf.initialise(Serial); // terseCRSF: solo per init/versione (non usiamo la sua UART)
|
||||
initRadio();
|
||||
|
||||
Serial.println(F("In ascolto su ESP-NOW (canale 1)..."));
|
||||
}
|
||||
|
||||
uint32_t lastDash = 0;
|
||||
|
||||
void loop() {
|
||||
// Processa eventuale frame ricevuto
|
||||
if (frameReady) {
|
||||
uint8_t buf[64];
|
||||
uint16_t len;
|
||||
portENTER_CRITICAL(&mux);
|
||||
len = frameLen;
|
||||
memcpy(buf, espnowBuf, len);
|
||||
frameReady = false;
|
||||
portEXIT_CRITICAL(&mux);
|
||||
|
||||
pktCount++;
|
||||
// decodeTelemetry popola i campi crsf.* in base al type (buf[2])
|
||||
crsf.decodeTelemetry(buf, (uint8_t)len);
|
||||
}
|
||||
|
||||
// Dashboard a 1 Hz
|
||||
if (millis() - lastDash >= 1000) {
|
||||
lastDash = millis();
|
||||
printDashboard();
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user