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
158
03_backpack_emulator/src/main.cpp
Normal file
158
03_backpack_emulator/src/main.cpp
Normal file
@@ -0,0 +1,158 @@
|
||||
// ============================================================
|
||||
// Backpack Emulator - bench Tier 3 per 02_telemetry_bridge
|
||||
// Board: ESP32-S3 DevKitC-1 (USB nativo)
|
||||
//
|
||||
// Spedisce via ESP-NOW pacchetti "backpack" sintetici (8 byte di
|
||||
// header + frame CRSF grezzo con CRC valido) all'UID del
|
||||
// ricevitore sotto test (src/my_config.h -> TARGET_UID). Cicla
|
||||
// GPS/BATTERY/ATTITUDE/LINK/FLIGHTMODE usando gli stessi payload
|
||||
// byte-esatti gia' verificati in
|
||||
// 02_telemetry_bridge/test/test_embedded_crsf_decode, cosi' i
|
||||
// valori mostrati dal modo HUMAN del ricevitore sono confrontabili
|
||||
// a vista con quelli documentati li'. Intercala anche keep-alive
|
||||
// (header + 2 byte zero) per esercitare il filtro su hardware
|
||||
// reale, non solo nel test unitario (Tier 2).
|
||||
//
|
||||
// Non serve aereo/GPS/radio ELRS: solo le due board sul banco,
|
||||
// collegate via USB.
|
||||
// ============================================================
|
||||
#include <Arduino.h>
|
||||
#include <WiFi.h>
|
||||
#include <esp_wifi.h>
|
||||
#include <esp_now.h>
|
||||
|
||||
#if __has_include("my_config.h")
|
||||
#include "my_config.h"
|
||||
#endif
|
||||
#ifndef TARGET_UID
|
||||
#define TARGET_UID { 0, 0, 0, 0, 0, 0 }
|
||||
#endif
|
||||
|
||||
static uint8_t targetMac[6] = TARGET_UID;
|
||||
|
||||
// ---- CRC8 CRSF (poly 0xD5, stesso algoritmo di terseCRSF) -----
|
||||
static uint8_t crc8_dvb_s2(uint8_t crc, uint8_t a) {
|
||||
crc ^= a;
|
||||
for (uint8_t i = 0; i < 8; ++i) {
|
||||
crc = (crc & 0x80) ? (uint8_t)((crc << 1) ^ 0xD5) : (uint8_t)(crc << 1);
|
||||
}
|
||||
return crc;
|
||||
}
|
||||
|
||||
// Costruisce un frame CRSF valido (sync, len, type, payload, crc)
|
||||
// in out[]. Ritorna la lunghezza totale del frame.
|
||||
static uint8_t buildCrsfFrame(uint8_t* out, uint8_t type, const uint8_t* payload, uint8_t payloadLen) {
|
||||
out[0] = 0xC8; // CRSF_TEL_SYNC_BYTE
|
||||
out[1] = payloadLen + 2; // len = type + payload + crc
|
||||
out[2] = type;
|
||||
memcpy(out + 3, payload, payloadLen);
|
||||
uint8_t crc = 0;
|
||||
for (uint8_t i = 0; i < payloadLen + 1; ++i) crc = crc8_dvb_s2(crc, out[2 + i]);
|
||||
out[3 + payloadLen] = crc;
|
||||
return 3 + payloadLen + 1;
|
||||
}
|
||||
|
||||
// ---- Payload di esempio (identici ai vettori verificati in
|
||||
// test_embedded_crsf_decode/test_decode.cpp) --------------------
|
||||
#define GPS_ID 0x02
|
||||
#define BATTERY_ID 0x08
|
||||
#define LINK_ID 0x14
|
||||
#define ATTITUDE_ID 0x1E
|
||||
#define FLIGHT_MODE_ID 0x21
|
||||
|
||||
static const uint8_t PAYLOAD_GPS[] = {27,25,73,180, 5,122,72,96, 0,125, 35,40, 4,128, 11};
|
||||
static const uint8_t PAYLOAD_BATTERY[] = {0,164, 0,12, 0,1,94, 78};
|
||||
static const uint8_t PAYLOAD_ATTITUDE[] = {6,209, 252,151, 235,140};
|
||||
static const uint8_t PAYLOAD_LINK[] = {52, 0, 100, 247, 0, 2, 3, 60, 98, 245};
|
||||
static const uint8_t PAYLOAD_FLIGHTMODE[] = {'A','C','R','O', 0x00}; // stringa + null (vedi CRSF flight mode)
|
||||
|
||||
struct TelemSample { uint8_t type; const uint8_t* payload; uint8_t len; const char* label; };
|
||||
static const TelemSample SAMPLES[] = {
|
||||
{ GPS_ID, PAYLOAD_GPS, sizeof(PAYLOAD_GPS), "GPS" },
|
||||
{ BATTERY_ID, PAYLOAD_BATTERY, sizeof(PAYLOAD_BATTERY), "BATTERY" },
|
||||
{ ATTITUDE_ID, PAYLOAD_ATTITUDE, sizeof(PAYLOAD_ATTITUDE), "ATTITUDE" },
|
||||
{ LINK_ID, PAYLOAD_LINK, sizeof(PAYLOAD_LINK), "LINK" },
|
||||
{ FLIGHT_MODE_ID, PAYLOAD_FLIGHTMODE, sizeof(PAYLOAD_FLIGHTMODE), "FLIGHTMODE" },
|
||||
};
|
||||
static const uint8_t NUM_SAMPLES = sizeof(SAMPLES) / sizeof(SAMPLES[0]);
|
||||
|
||||
static uint32_t tick = 0;
|
||||
static uint8_t sampleIdx = 0;
|
||||
|
||||
// Header "backpack": 8 byte, contenuto ignorato dal parser del
|
||||
// ricevitore (vedi EspNowCrsf::onRecv) -- qui solo per diagnostica.
|
||||
static void fillHeader(uint8_t* out) {
|
||||
memset(out, 0, 8);
|
||||
out[0] = 'E'; out[1] = 'M'; out[2] = 'U'; out[3] = 'L';
|
||||
memcpy(out + 4, &tick, 4);
|
||||
}
|
||||
|
||||
static void sendTelemetry(const TelemSample& sample) {
|
||||
uint8_t pkt[8 + 32];
|
||||
fillHeader(pkt);
|
||||
uint8_t frameLen = buildCrsfFrame(pkt + 8, sample.type, sample.payload, sample.len);
|
||||
esp_err_t res = esp_now_send(targetMac, pkt, 8 + frameLen);
|
||||
Serial.printf("[%lu] TX %-10s frame=%u byte %s\n",
|
||||
(unsigned long)tick, sample.label, frameLen,
|
||||
res == ESP_OK ? "ok" : "ERR");
|
||||
}
|
||||
|
||||
static void sendKeepAlive() {
|
||||
uint8_t pkt[10];
|
||||
fillHeader(pkt);
|
||||
pkt[8] = 0; pkt[9] = 0; // keep-alive: i due byte dopo l'header a zero
|
||||
esp_err_t res = esp_now_send(targetMac, pkt, sizeof(pkt));
|
||||
Serial.printf("[%lu] TX keep-alive %s\n",
|
||||
(unsigned long)tick, res == ESP_OK ? "ok" : "ERR");
|
||||
}
|
||||
|
||||
static void initRadio() {
|
||||
WiFi.mode(WIFI_STA);
|
||||
WiFi.disconnect();
|
||||
esp_wifi_set_protocol(WIFI_IF_STA, WIFI_PROTOCOL_11B); // stesso protocollo del backpack reale
|
||||
esp_wifi_start();
|
||||
esp_wifi_set_channel(1, WIFI_SECOND_CHAN_NONE); // deve combaciare col canale del ricevitore
|
||||
|
||||
if (esp_now_init() != ESP_OK) {
|
||||
Serial.println(F("[ERR] esp_now_init fallito"));
|
||||
return;
|
||||
}
|
||||
|
||||
esp_now_peer_info_t peer = {};
|
||||
memcpy(peer.peer_addr, targetMac, 6);
|
||||
peer.channel = 1;
|
||||
peer.encrypt = false;
|
||||
esp_now_add_peer(&peer);
|
||||
}
|
||||
|
||||
void setup() {
|
||||
Serial.begin(115200);
|
||||
delay(3000);
|
||||
Serial.println();
|
||||
Serial.println(F("=== Backpack Emulator (bench Tier 3) ==="));
|
||||
Serial.printf("Target UID: %u %u %u %u %u %u\n",
|
||||
targetMac[0], targetMac[1], targetMac[2], targetMac[3], targetMac[4], targetMac[5]);
|
||||
if (targetMac[0] == 0 && targetMac[1] == 0 && targetMac[2] == 0)
|
||||
Serial.println(F("[ATTENZIONE] TARGET_UID non configurato: modifica src/my_config.h"));
|
||||
|
||||
initRadio();
|
||||
Serial.println(F("Invio telemetria sintetica ogni 250ms (ciclo GPS/BATTERY/ATTITUDE/LINK/FLIGHTMODE + keep-alive)..."));
|
||||
}
|
||||
|
||||
static uint32_t lastSend = 0;
|
||||
static const uint16_t SEND_PERIOD_MS = 250;
|
||||
static const uint8_t KEEPALIVE_EVERY = 8; // 1 keep-alive ogni 8 invii (~2s)
|
||||
|
||||
void loop() {
|
||||
if (millis() - lastSend < SEND_PERIOD_MS) return;
|
||||
lastSend = millis();
|
||||
tick++;
|
||||
|
||||
if (tick % KEEPALIVE_EVERY == 0) {
|
||||
sendKeepAlive();
|
||||
return;
|
||||
}
|
||||
|
||||
sendTelemetry(SAMPLES[sampleIdx]);
|
||||
sampleIdx = (sampleIdx + 1) % NUM_SAMPLES;
|
||||
}
|
||||
Reference in New Issue
Block a user