e22a47fce1
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>
105 lines
3.0 KiB
C++
105 lines
3.0 KiB
C++
#include "EspNowCrsf.h"
|
|
#include <WiFi.h>
|
|
#include <esp_wifi.h>
|
|
#include <esp_now.h>
|
|
|
|
namespace EspNowCrsf {
|
|
|
|
// ---- Ring buffer dei frame CRSF ----------------------------
|
|
static const uint8_t RING_SLOTS = 8;
|
|
static const uint16_t SLOT_SIZE = 64;
|
|
|
|
struct Slot { uint8_t data[SLOT_SIZE]; uint16_t len; };
|
|
static Slot ring[RING_SLOTS];
|
|
static volatile uint8_t head = 0; // scritto dalla callback
|
|
static volatile uint8_t tail = 0; // letto dal loop
|
|
static portMUX_TYPE mux = portMUX_INITIALIZER_UNLOCKED;
|
|
|
|
static volatile uint32_t s_received = 0;
|
|
static volatile uint32_t s_dropped = 0;
|
|
|
|
static uint8_t s_uid[6];
|
|
|
|
// ---- Callback ricezione (firma arduino-esp32 core 2.x) -----
|
|
static void onRecv(const uint8_t* mac, const uint8_t* in, int len) {
|
|
if (len <= 8 || in == nullptr) return;
|
|
// keep-alive: type/payload a zero subito dopo l'header di 8 byte
|
|
// (len > 9 prima degli accessi: con len==9 leggere in[9] sarebbe OOB)
|
|
if (len > 9 && in[8] == 0 && in[9] == 0) return;
|
|
|
|
uint16_t crsfLen = len - 8;
|
|
if (crsfLen > SLOT_SIZE) crsfLen = SLOT_SIZE;
|
|
|
|
portENTER_CRITICAL_ISR(&mux);
|
|
uint8_t next = (head + 1) % RING_SLOTS;
|
|
if (next == tail) { // pieno -> scarta il piu' vecchio
|
|
tail = (tail + 1) % RING_SLOTS;
|
|
s_dropped++;
|
|
}
|
|
memcpy(ring[head].data, in + 8, crsfLen);
|
|
ring[head].len = crsfLen;
|
|
head = next;
|
|
s_received++;
|
|
portEXIT_CRITICAL_ISR(&mux);
|
|
}
|
|
|
|
void begin(const uint8_t uid[6]) {
|
|
memcpy(s_uid, uid, 6);
|
|
s_uid[0] &= ~0x01; // MAC unicast
|
|
|
|
WiFi.mode(WIFI_STA);
|
|
WiFi.disconnect();
|
|
esp_wifi_set_protocol(WIFI_IF_STA, WIFI_PROTOCOL_11B);
|
|
esp_wifi_set_mac(WIFI_IF_STA, s_uid);
|
|
esp_wifi_start();
|
|
esp_wifi_set_channel(1, WIFI_SECOND_CHAN_NONE);
|
|
|
|
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) {
|
|
bool got = false;
|
|
portENTER_CRITICAL(&mux);
|
|
if (tail != head) {
|
|
uint16_t n = ring[tail].len;
|
|
if (n > maxLen) n = maxLen;
|
|
memcpy(out, ring[tail].data, n);
|
|
*outLen = n;
|
|
tail = (tail + 1) % RING_SLOTS;
|
|
got = true;
|
|
}
|
|
portEXIT_CRITICAL(&mux);
|
|
return got;
|
|
}
|
|
|
|
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); }
|
|
|
|
void test_reset() {
|
|
portENTER_CRITICAL(&mux);
|
|
head = 0;
|
|
tail = 0;
|
|
s_received = 0;
|
|
s_dropped = 0;
|
|
portEXIT_CRITICAL(&mux);
|
|
}
|
|
#endif
|
|
|
|
} // namespace EspNowCrsf
|