alejandrogs73/G26-Telemetry-Software
1
0
Fork 0
mirror of https://github.com/adrigongv23/G26---Telemetry-Software.git synced 2026-08-25 11:33:17 +02:00
Code Issues Projects Releases Packages Wiki Activity Actions
G26-Telemetry-Software/Radio/Radio_intermediario/bridge.cpp
Álvaro Alcántara Ramírez f0db3a8aeb Radio subida
2026-08-03 21:38:32 +02:00

726 lines
20 KiB
C++
Raw Permalink Blame History

#include "bridge.hpp"
#include "config.hpp"
#include <Arduino.h>
#include <HardwareSerial.h>
#include <WiFi.h>
#include <esp_now.h>
#include <esp_wifi.h>
#include <string.h>
#include "freertos/FreeRTOS.h"
#include "freertos/queue.h"
struct EspNowRxItem {
uint32_t epoch;
uint16_t len;
uint8_t data[RADIO_PACKET_MAX_LEN];
};
static HardwareSerial PilotUart(2);
static QueueHandle_t box_audio_queue = nullptr;
static portMUX_TYPE control_mux = portMUX_INITIALIZER_UNLOCKED;
static volatile bool box_ptt_active = false;
static volatile bool box_stop_pending = false;
static volatile uint32_t box_stream_epoch = 0;
static volatile uint32_t box_control_version = 0;
static volatile bool espnow_send_busy = false;
static volatile bool espnow_last_send_ok = false;
static volatile uint32_t espnow_rx_dropped = 0;
/* Diagnóstico permanente: no modifica el protocolo ni las MAC. */
static volatile uint32_t debug_uart_rx_bytes = 0;
static volatile uint32_t debug_pilot_audio_packets = 0;
static volatile uint32_t debug_pilot_invalid_packets = 0;
static volatile uint32_t debug_espnow_api_ok = 0;
static volatile uint32_t debug_espnow_api_fail = 0;
static volatile uint32_t debug_espnow_ack_ok = 0;
static volatile uint32_t debug_espnow_ack_fail = 0;
static volatile uint32_t debug_espnow_forward_fail = 0;
static volatile uint32_t debug_espnow_rx_raw = 0;
static volatile uint32_t debug_espnow_rx_from_box = 0;
static volatile uint32_t debug_espnow_rx_foreign_mac = 0;
static volatile uint32_t debug_box_audio_packets = 0;
static volatile uint32_t debug_box_control_packets = 0;
static volatile uint32_t debug_box_invalid_packets = 0;
static volatile uint32_t debug_uart_tx_bytes = 0;
static volatile uint32_t debug_uart_tx_fail = 0;
static uint32_t debug_last_report_ms = 0;
static uint32_t last_control_version_sent = 0;
static uint32_t last_control_epoch_sent = 0;
static bool last_control_state_sent = false;
static uint32_t last_control_sent_ms = 0;
static volatile uint32_t debug_control_heartbeats = 0;
static uint16_t read_le_u16(const uint8_t *p)
{
return (uint16_t)p[0] | ((uint16_t)p[1] << 8);
}
static bool mac_is_zero(const uint8_t *mac)
{
static const uint8_t zero[6] = {};
return mac == nullptr || memcmp(mac, zero, sizeof(zero)) == 0;
}
static bool audio_packet_valid(const uint8_t *data, int len)
{
if (data == nullptr || len < RADIO_PACKET_HEADER_LEN || len > RADIO_PACKET_MAX_LEN) {
return false;
}
if (data[0] != RADIO_AUDIO_MAGIC_1 || data[1] != RADIO_AUDIO_MAGIC_2) {
return false;
}
const uint8_t chunk_index = data[3];
const uint8_t chunk_total = data[4];
const uint16_t frame_len = read_le_u16(&data[5]);
const uint16_t offset = read_le_u16(&data[7]);
const uint16_t chunk_len = read_le_u16(&data[9]);
if (chunk_total == 0 || chunk_total > 16 || chunk_index >= chunk_total) {
return false;
}
if (frame_len == 0 || frame_len > 2000) {
return false;
}
if (chunk_len == 0 || chunk_len > RADIO_AUDIO_CHUNK) {
return false;
}
if ((uint32_t)offset + chunk_len > frame_len) {
return false;
}
return len == RADIO_PACKET_HEADER_LEN + chunk_len;
}
static bool control_packet_valid(const uint8_t *data, int len)
{
return data != nullptr &&
len == RADIO_CONTROL_PACKET_LEN &&
data[0] == RADIO_CONTROL_MAGIC_1 &&
data[1] == RADIO_CONTROL_MAGIC_2 &&
data[2] == RADIO_CONTROL_COMMAND_BOX_PTT &&
(data[3] == 0 || data[3] == 1);
}
static void snapshot_control(
bool *active,
bool *stop_pending,
uint32_t *epoch,
uint32_t *version)
{
portENTER_CRITICAL(&control_mux);
if (active != nullptr) {
*active = box_ptt_active;
}
if (stop_pending != nullptr) {
*stop_pending = box_stop_pending;
}
if (epoch != nullptr) {
*epoch = box_stream_epoch;
}
if (version != nullptr) {
*version = box_control_version;
}
portEXIT_CRITICAL(&control_mux);
}
static bool ensure_peer_registered(const uint8_t *mac)
{
if (mac_is_zero(mac)) {
return false;
}
if (esp_now_is_peer_exist(mac)) {
return true;
}
esp_now_peer_info_t peer = {};
memcpy(peer.peer_addr, mac, 6);
peer.channel = ESPNOW_CHANNEL;
peer.ifidx = WIFI_IF_STA;
peer.encrypt = false;
return esp_now_add_peer(&peer) == ESP_OK;
}
static void on_espnow_sent(const wifi_tx_info_t *tx_info, esp_now_send_status_t status)
{
(void)tx_info;
espnow_last_send_ok = (status == ESP_NOW_SEND_SUCCESS);
if (espnow_last_send_ok) {
++debug_espnow_ack_ok;
} else {
++debug_espnow_ack_fail;
}
espnow_send_busy = false;
}
static void on_espnow_recv(
const esp_now_recv_info_t *info,
const uint8_t *data,
int len)
{
++debug_espnow_rx_raw;
if (info == nullptr || info->src_addr == nullptr || data == nullptr) {
++debug_box_invalid_packets;
return;
}
if (memcmp(info->src_addr, BOX_RECEIVER_MAC, 6) != 0) {
++debug_espnow_rx_foreign_mac;
return;
}
++debug_espnow_rx_from_box;
if (control_packet_valid(data, len)) {
++debug_box_control_packets;
const bool active = data[3] != 0;
portENTER_CRITICAL(&control_mux);
if (active) {
// Un START nuevo cancela un cierre pendiente. Si el stream ya estaba
// activo no se cambia el epoch, por lo que la cola anterior termina
// y la nueva pulsación continúa sin cortes.
if (!box_ptt_active) {
box_ptt_active = true;
++box_stream_epoch;
++box_control_version;
}
box_stop_pending = false;
} else if (box_ptt_active) {
// STOP no se reenvía todavía: primero se vacía la cola de audio que
// ya llegó por ESP-NOW para no cortar el final del mensaje.
box_stop_pending = true;
}
portEXIT_CRITICAL(&control_mux);
return;
}
if (!audio_packet_valid(data, len) || box_audio_queue == nullptr) {
++debug_box_invalid_packets;
return;
}
++debug_box_audio_packets;
bool active = false;
bool stop_pending = false;
uint32_t epoch = 0;
snapshot_control(&active, &stop_pending, &epoch, nullptr);
if (!active || stop_pending) {
return;
}
EspNowRxItem item = {};
item.epoch = epoch;
item.len = (uint16_t)len;
memcpy(item.data, data, len);
if (xQueueSend(box_audio_queue, &item, 0) != pdTRUE) {
EspNowRxItem discarded = {};
xQueueReceive(box_audio_queue, &discarded, 0);
if (xQueueSend(box_audio_queue, &item, 0) != pdTRUE) {
++espnow_rx_dropped;
}
}
}
static bool uart_write_all(const uint8_t *data, size_t len)
{
if (data == nullptr || len == 0) {
return false;
}
size_t sent = 0;
const uint32_t started = millis();
while (sent < len) {
sent += PilotUart.write(data + sent, len - sent);
if (sent >= len) {
debug_uart_tx_bytes += (uint32_t)len;
return true;
}
if ((uint32_t)(millis() - started) >= PILOT_UART_WRITE_TIMEOUT_MS) {
++debug_uart_tx_fail;
return false;
}
delay(0);
}
return true;
}
static bool send_pending_control_to_pilot()
{
bool active = false;
uint32_t epoch = 0;
uint32_t version = 0;
snapshot_control(&active, nullptr, &epoch, &version);
const bool heartbeat_due =
active &&
(uint32_t)(millis() - last_control_sent_ms) >= BOX_PTT_HEARTBEAT_MS;
if (version == last_control_version_sent && !heartbeat_due) {
return true;
}
const uint8_t packet[RADIO_CONTROL_PACKET_LEN] = {
RADIO_CONTROL_MAGIC_1,
RADIO_CONTROL_MAGIC_2,
RADIO_CONTROL_COMMAND_BOX_PTT,
active ? (uint8_t)1 : (uint8_t)0
};
if (!uart_write_all(packet, sizeof(packet))) {
return false;
}
if (version == last_control_version_sent && heartbeat_due) {
++debug_control_heartbeats;
}
last_control_version_sent = version;
last_control_epoch_sent = epoch;
last_control_state_sent = active;
last_control_sent_ms = millis();
return true;
}
static bool finalize_box_stop_if_drained()
{
if (box_audio_queue == nullptr || uxQueueMessagesWaiting(box_audio_queue) != 0) {
return false;
}
bool changed = false;
portENTER_CRITICAL(&control_mux);
if (box_stop_pending && box_ptt_active) {
box_stop_pending = false;
box_ptt_active = false;
++box_stream_epoch;
++box_control_version;
changed = true;
}
portEXIT_CRITICAL(&control_mux);
return changed;
}
static void forward_box_audio_to_pilot()
{
if (!send_pending_control_to_pilot() || box_audio_queue == nullptr) {
return;
}
EspNowRxItem item = {};
uint8_t processed = 0;
// Limitar cada pasada evita que una cola grande bloquee el sentido contrario.
while (processed < MAX_BOX_TO_PILOT_PACKETS_PER_PASS &&
xQueueReceive(box_audio_queue, &item, 0) == pdTRUE) {
++processed;
bool active = false;
uint32_t epoch = 0;
uint32_t version = 0;
snapshot_control(&active, nullptr, &epoch, &version);
if (!active || item.epoch != epoch) {
continue;
}
if (last_control_version_sent != version ||
last_control_epoch_sent != epoch ||
!last_control_state_sent) {
if (!send_pending_control_to_pilot()) {
return;
}
}
if (!uart_write_all(item.data, item.len)) {
Serial.println("ERROR: no se pudo enviar audio al Audio Kit por UART");
return;
}
}
// El STOP solo viaja después del último paquete que ya estaba en la cola.
if (finalize_box_stop_if_drained()) {
send_pending_control_to_pilot();
}
}
bool bridge_send_espnow(const byte *data, size_t len, byte intentos)
{
if (data == nullptr || len == 0 || len > 250 || intentos == 0) {
return false;
}
if (!ensure_peer_registered(BOX_RECEIVER_MAC)) {
return false;
}
for (byte attempt = 0; attempt < intentos; ++attempt) {
const uint32_t wait_start = millis();
while (espnow_send_busy &&
(uint32_t)(millis() - wait_start) < ESPNOW_SEND_TIMEOUT_MS) {
delay(1);
}
if (espnow_send_busy) {
espnow_send_busy = false;
}
espnow_last_send_ok = false;
espnow_send_busy = true;
const esp_err_t err = esp_now_send(BOX_RECEIVER_MAC, data, len);
if (err != ESP_OK) {
++debug_espnow_api_fail;
espnow_send_busy = false;
delay(1);
continue;
}
++debug_espnow_api_ok;
const uint32_t ack_start = millis();
while (espnow_send_busy &&
(uint32_t)(millis() - ack_start) < ESPNOW_SEND_TIMEOUT_MS) {
delay(1);
}
if (!espnow_send_busy && espnow_last_send_ok) {
return true;
}
delay(1);
}
++debug_espnow_forward_fail;
return false;
}
static bool send_audio_packet(const byte *data, size_t len)
{
if (!audio_packet_valid(data, (int)len)) {
return false;
}
return bridge_send_espnow(data, len, ESPNOW_SEND_RETRIES);
}
static void process_pilot_uart_to_espnow()
{
enum ParserState {
WAIT_MAGIC_1,
WAIT_MAGIC_2,
READ_AUDIO_HEADER,
READ_AUDIO_BODY,
SKIP_CONTROL
};
static ParserState state = WAIT_MAGIC_1;
static uint8_t packet[RADIO_PACKET_MAX_LEN] = {};
static size_t index = 0;
static uint16_t expected_audio_len = 0;
static uint8_t control_bytes = 0;
static uint32_t last_byte_ms = 0;
uint8_t processed_packets = 0;
auto reset_parser = [&]() {
state = WAIT_MAGIC_1;
index = 0;
expected_audio_len = 0;
control_bytes = 0;
};
while (PilotUart.available() > 0) {
const int value = PilotUart.read();
if (value < 0) {
break;
}
const uint8_t b = (uint8_t)value;
++debug_uart_rx_bytes;
const uint32_t now = millis();
if (state != WAIT_MAGIC_1 && (uint32_t)(now - last_byte_ms) > 120) {
++debug_pilot_invalid_packets;
reset_parser();
}
last_byte_ms = now;
switch (state) {
case WAIT_MAGIC_1:
if (b == RADIO_AUDIO_MAGIC_1) {
packet[0] = b;
index = 1;
state = WAIT_MAGIC_2;
}
break;
case WAIT_MAGIC_2:
if (b == RADIO_AUDIO_MAGIC_2) {
packet[1] = b;
index = 2;
state = READ_AUDIO_HEADER;
} else if (b == RADIO_CONTROL_MAGIC_2) {
// El Audio Kit no necesita mandar controles al box, pero se
// consume un posible paquete para mantener sincronía.
control_bytes = 2;
state = SKIP_CONTROL;
} else if (b == RADIO_AUDIO_MAGIC_1) {
packet[0] = b;
index = 1;
} else {
reset_parser();
}
break;
case SKIP_CONTROL:
++control_bytes;
if (control_bytes >= RADIO_CONTROL_PACKET_LEN) {
reset_parser();
}
break;
case READ_AUDIO_HEADER:
if (index >= sizeof(packet)) {
reset_parser();
break;
}
packet[index++] = b;
if (index >= RADIO_PACKET_HEADER_LEN) {
const uint16_t frame_len = read_le_u16(&packet[5]);
const uint16_t offset = read_le_u16(&packet[7]);
expected_audio_len = read_le_u16(&packet[9]);
if (packet[4] == 0 || packet[4] > 16 ||
packet[3] >= packet[4] ||
frame_len == 0 || frame_len > 2000 ||
expected_audio_len == 0 || expected_audio_len > RADIO_AUDIO_CHUNK ||
(uint32_t)offset + expected_audio_len > frame_len) {
++debug_pilot_invalid_packets;
reset_parser();
break;
}
state = READ_AUDIO_BODY;
}
break;
case READ_AUDIO_BODY:
if (index >= sizeof(packet)) {
reset_parser();
break;
}
packet[index++] = b;
if (index >= RADIO_PACKET_HEADER_LEN + expected_audio_len) {
++debug_pilot_audio_packets;
send_audio_packet(packet, index);
reset_parser();
++processed_packets;
if (processed_packets >= MAX_PILOT_TO_BOX_PACKETS_PER_PASS) {
return;
}
}
break;
}
}
}
static void print_permanent_debug()
{
const uint32_t now = millis();
if ((uint32_t)(now - debug_last_report_ms) < INTERMEDIARY_DEBUG_INTERVAL_MS) {
return;
}
debug_last_report_ms = now;
bool ptt_active = false;
bool ptt_stop_pending = false;
uint32_t ptt_epoch = 0;
uint32_t ptt_version = 0;
snapshot_control(&ptt_active, &ptt_stop_pending, &ptt_epoch, &ptt_version);
const UBaseType_t queue_items =
box_audio_queue != nullptr ? uxQueueMessagesWaiting(box_audio_queue) : 0;
Serial.println();
Serial.println("========== DEBUG INTERMEDIARIO ==========");
Serial.printf(
"PILOTO -> UART: bytes=%lu | paquetes_audio=%lu | invalidos=%lu\n",
(unsigned long)debug_uart_rx_bytes,
(unsigned long)debug_pilot_audio_packets,
(unsigned long)debug_pilot_invalid_packets
);
Serial.printf(
"INTERMEDIARIO -> BOX: API_OK=%lu | API_FALLO=%lu | ACK_OK=%lu | ACK_FALLO=%lu | paquetes_fallidos=%lu\n",
(unsigned long)debug_espnow_api_ok,
(unsigned long)debug_espnow_api_fail,
(unsigned long)debug_espnow_ack_ok,
(unsigned long)debug_espnow_ack_fail,
(unsigned long)debug_espnow_forward_fail
);
Serial.printf(
"BOX -> ESP-NOW: RAW=%lu | desde_MAC_box=%lu | audio=%lu | controles=%lu | invalidos=%lu | MAC_ajena=%lu\n",
(unsigned long)debug_espnow_rx_raw,
(unsigned long)debug_espnow_rx_from_box,
(unsigned long)debug_box_audio_packets,
(unsigned long)debug_box_control_packets,
(unsigned long)debug_box_invalid_packets,
(unsigned long)debug_espnow_rx_foreign_mac
);
Serial.printf(
"INTERMEDIARIO -> PILOTO: bytes_UART=%lu | fallos_UART=%lu | cola=%u | descartados=%lu\n",
(unsigned long)debug_uart_tx_bytes,
(unsigned long)debug_uart_tx_fail,
(unsigned int)queue_items,
(unsigned long)espnow_rx_dropped
);
Serial.printf(
"PTT BOX: %s | cierre_pendiente=%s | epoch=%lu | version=%lu | latidos_START=%lu\n",
ptt_active ? "ACTIVO" : "INACTIVO",
ptt_stop_pending ? "SI" : "NO",
(unsigned long)ptt_epoch,
(unsigned long)ptt_version,
(unsigned long)debug_control_heartbeats
);
Serial.println("=========================================");
}
bool bridge_init()
{
box_audio_queue = xQueueCreate(ESPNOW_RX_QUEUE_SIZE, sizeof(EspNowRxItem));
if (box_audio_queue == nullptr) {
Serial.println("ERROR creando cola ESP-NOW");
return false;
}
PilotUart.setRxBufferSize(PILOT_UART_RX_BUFFER_SIZE);
PilotUart.begin(
PILOT_UART_BAUD,
SERIAL_8N1,
PILOT_UART_RX_GPIO,
PILOT_UART_TX_GPIO
);
WiFi.mode(WIFI_STA);
WiFi.disconnect();
WiFi.setSleep(false);
delay(50);
if (esp_wifi_set_ps(WIFI_PS_NONE) != ESP_OK) {
return false;
}
if (esp_wifi_set_protocol(
WIFI_IF_STA,
WIFI_PROTOCOL_11B |
WIFI_PROTOCOL_11G |
WIFI_PROTOCOL_11N |
WIFI_PROTOCOL_LR) != ESP_OK) {
return false;
}
if (esp_wifi_set_channel(ESPNOW_CHANNEL, WIFI_SECOND_CHAN_NONE) != ESP_OK) {
return false;
}
// El driver puede limitar el valor según chip, alimentación y normativa.
esp_wifi_set_max_tx_power(ESPNOW_TX_POWER_QDBM);
if (esp_now_init() != ESP_OK) {
return false;
}
if (esp_now_register_send_cb(on_espnow_sent) != ESP_OK ||
esp_now_register_recv_cb(on_espnow_recv) != ESP_OK) {
return false;
}
if (!ensure_peer_registered(BOX_RECEIVER_MAC)) {
return false;
}
int8_t tx_power = 0;
esp_wifi_get_max_tx_power(&tx_power);
Serial.println("Intermediario UART <-> ESP-NOW LR iniciado");
Serial.print("MAC WiFi STA del intermediario: ");
Serial.println(WiFi.macAddress());
Serial.printf(
"MAC fija del receptor BOX: %02X:%02X:%02X:%02X:%02X:%02X\n",
BOX_RECEIVER_MAC[0],
BOX_RECEIVER_MAC[1],
BOX_RECEIVER_MAC[2],
BOX_RECEIVER_MAC[3],
BOX_RECEIVER_MAC[4],
BOX_RECEIVER_MAC[5]
);
Serial.printf("UART: RX GPIO%d / TX GPIO%d / %d baudios\n",
PILOT_UART_RX_GPIO,
PILOT_UART_TX_GPIO,
PILOT_UART_BAUD);
Serial.printf("Canal ESP-NOW: %d\n", ESPNOW_CHANNEL);
Serial.print("Potencia configurada aprox.: ");
Serial.print(tx_power * 0.25f);
Serial.println(" dBm");
return true;
}
void bridge_process()
{
static bool box_first = true;
// El control START/STOP siempre tiene prioridad sobre los paquetes de audio.
send_pending_control_to_pilot();
// Alternar la prioridad en cada pasada evita que un flujo continuo pueda
// monopolizar el loop y cortar el audio del sentido opuesto.
if (box_first) {
forward_box_audio_to_pilot();
process_pilot_uart_to_espnow();
} else {
process_pilot_uart_to_espnow();
forward_box_audio_to_pilot();
}
box_first = !box_first;
// Por si STOP llegó mientras se enviaba un paquete hacia boxes.
send_pending_control_to_pilot();
// Diagnóstico siempre activo, una vez por segundo.
print_permanent_debug();
delay(0);
}
Reference in a new issue View git blame Copy permalink