fzanin/Crossfire-to-Multi-telemetry-converter-ESP-NOW
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

MAVLINK: timeout staleness (no aereo congelato)

Browse Source 
Se non arrivano frame validi da >2s, l'HEARTBEAT passa a MAV_STATE_CRITICAL e
GPS_RAW_INT a NO_FIX, invece di continuare a emettere gli ultimi valori in
cache come "vivi" (MAV_STATE_ACTIVE). L'HEARTBEAT continua (il link verso il
GCS resta), cosi' il GCS segnala "telemetria persa" anziche' un aereo fermo.

Flight mode->custom_mode NON implementato di proposito: mwp decodifica
custom_mode con mav2inav()/xmav2inav() in formato INAV-specifico e dipendente
dalla versione FC (fc_vers>=hasPOI) -> troppo fragile per il valore offerto.

Co-Authored-By: Claude Opus 4.8 <noreply@anthropic.com>
This commit is contained in:
Francesco Zanin
2026-06-23 19:48:12 +02:00
parent bb378f16d0
commit f8bd99b89d
1 changed files with 13 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

15
02_telemetry_bridge/src/mode_mavlink.cpp
View File

@@ -36,6 +36,13 @@ static uint8_t c_lq = 0; // link quality %
static uint32_t tHB = 0, tSYS = 0, tGPS = 0, tATT = 0, tHUD = 0, tBAT = 0, tRADIO = 0; static uint32_t tHB = 0, tSYS = 0, tGPS = 0, tATT = 0, tHUD = 0, tBAT = 0, tRADIO = 0;
// Staleness: se non arrivano frame validi entro TELEM_TIMEOUT_MS, smettiamo
// di spacciare per "vivi" gli ultimi valori in cache. Continuiamo a emettere
// l'HEARTBEAT (il link verso il GCS resta), ma con stato CRITICAL e GPS senza
// fix, cosi' il GCS segnala "telemetria persa" invece di un aereo congelato.
static uint32_t lastFrameMs = 0;
static const uint32_t TELEM_TIMEOUT_MS = 2000;
static uint8_t mbuf[MAVLINK_MAX_PACKET_LEN]; static uint8_t mbuf[MAVLINK_MAX_PACKET_LEN];
static mavlink_message_t mmsg; static mavlink_message_t mmsg;
@@ -76,10 +83,12 @@ static void mav_onFrame(const uint8_t* buf, uint16_t len) {
break; break;
default: break; default: break;
} }
if (id) lastFrameMs = millis(); // frame valido ricevuto
} }
static void mav_tick() { static void mav_tick() {
uint32_t now = millis(); uint32_t now = millis();
bool stale = (lastFrameMs == 0) || (now - lastFrameMs > TELEM_TIMEOUT_MS);
if (now - tATT >= 100) { // ATTITUDE 10 Hz if (now - tATT >= 100) { // ATTITUDE 10 Hz
tATT = now; tATT = now;
@@ -91,7 +100,8 @@ static void mav_tick() {
if (now - tGPS >= 200) { // GPS_RAW_INT 5 Hz if (now - tGPS >= 200) { // GPS_RAW_INT 5 Hz
tGPS = now; tGPS = now;
bool hasPos = (c_lat != 0) || (c_lon != 0); bool hasPos = (c_lat != 0) || (c_lon != 0);
uint8_t fix = (c_sats >= 5 && hasPos) ? GPS_FIX_TYPE_3D_FIX uint8_t fix = stale ? GPS_FIX_TYPE_NO_FIX
: (c_sats >= 5 && hasPos) ? GPS_FIX_TYPE_3D_FIX
: (c_sats > 0 ? GPS_FIX_TYPE_2D_FIX : GPS_FIX_TYPE_NO_FIX); : (c_sats > 0 ? GPS_FIX_TYPE_2D_FIX : GPS_FIX_TYPE_NO_FIX);
mavlink_msg_gps_raw_int_pack(SYSID, COMPID, &mmsg, mavlink_msg_gps_raw_int_pack(SYSID, COMPID, &mmsg,
(uint64_t)now * 1000ULL, fix, c_lat, c_lon, c_alt_m * 1000, (uint64_t)now * 1000ULL, fix, c_lat, c_lon, c_alt_m * 1000,
@@ -156,7 +166,8 @@ static void mav_tick() {
if (now - tHB >= 1000) { // HEARTBEAT 1 Hz if (now - tHB >= 1000) { // HEARTBEAT 1 Hz
tHB = now; tHB = now;
mavlink_msg_heartbeat_pack(SYSID, COMPID, &mmsg, mavlink_msg_heartbeat_pack(SYSID, COMPID, &mmsg,
MAV_TYPE_FIXED_WING, MAV_AUTOPILOT_GENERIC, 0, 0, MAV_STATE_ACTIVE); MAV_TYPE_FIXED_WING, MAV_AUTOPILOT_GENERIC, 0, 0,
stale ? MAV_STATE_CRITICAL : MAV_STATE_ACTIVE);
sendMsg(); sendMsg();
} }
} }