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

Snapshot iniziale: bridge telemetria CRSF->LTM/MAVLink (ESP-NOW)

Browse Source 
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
2026-06-23 19:32:11 +02:00
commit e22a47fce1
284 changed files with 132664 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

158
02_telemetry_bridge/src/mode_mavlink.cpp Normal file
View File

@@ -0,0 +1,158 @@
// ============================================================
// MODO MAVLINK - traduce la telemetria CRSF (ricevuta dal backpack)
// in MAVLink (dialetto common) emesso sulla USB seriale.
// mwptools lo legge passivamente (downlink/monitoraggio).
//
// Differenza chiave rispetto a LTM: MAVLink trasporta la CORRENTE
// ISTANTANEA in Ampere (SYS_STATUS.current_battery, in cA), che LTM
// non ha. Per il resto e' equivalente (posizione, assetto, batteria).
//
// NB: la libreria mavlink ufficiale (c_library_v2) emette frame v2
// (STX 0xFD) di default; mwp e INAV usano MAVLink 2, quindi va bene.
// Stream binario: come per LTM, niente testo sulla seriale qui.
// ============================================================
#include "modes.h"
#include <terseCRSF.h>
#include <common/mavlink.h>
static CRSF crsf;
// Identita' del "sistema" che pubblichiamo verso il GCS
static const uint8_t SYSID = 1;
static const uint8_t COMPID = MAV_COMP_ID_AUTOPILOT1;
// Cache dello stato corrente (popolata dai frame CRSF)
static int32_t c_lat = 0, c_lon = 0; // deg*1e7
static int32_t c_alt_m = 0; // metri
static float c_gs_mps = 0; // m/s
static float c_head_deg = 0; // gradi
static uint8_t c_sats = 0;
static float c_pitch = 0, c_roll = 0, c_yaw = 0; // gradi
static float c_vbat = 0; // volt
static float c_curr = 0; // ampere
static uint32_t c_mah = 0; // mAh consumati
static uint8_t c_rem = 0; // batteria rimanente %
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 uint8_t mbuf[MAVLINK_MAX_PACKET_LEN];
static mavlink_message_t mmsg;
static inline void sendMsg() {
uint16_t n = mavlink_msg_to_send_buffer(mbuf, &mmsg);
Serial.write(mbuf, n);
}
static void mav_begin() {
Serial.println(F("[MODO MAVLINK] stream MAVLink v2 per mwptools @115200"));
delay(50);
}
static void mav_onFrame(const uint8_t* buf, uint16_t len) {
uint8_t id = crsf.decodeTelemetry((uint8_t*)buf, (uint8_t)len);
switch (id) {
case GPS_ID: // 0x02
c_lat = crsf.gps_lat; // gia' deg*1e7
c_lon = crsf.gps_lon;
c_alt_m = crsf.gps_altitude; // metri
c_gs_mps = crsf.gpsF_groundspeed / 3.6f; // km/h -> m/s
c_head_deg = crsf.gpsF_heading;
c_sats = crsf.gps_sats;
break;
case ATTITUDE_ID: // 0x1E
c_pitch = crsf.attiF_pitch;
c_roll = crsf.attiF_roll;
c_yaw = crsf.attiF_yaw;
break;
case BATTERY_ID: // 0x08
c_vbat = crsf.batF_voltage;
c_curr = crsf.batF_current; // ampere (cio' che mancava in LTM)
c_mah = crsf.bat_fuel_drawn; // mAh consumati
c_rem = crsf.bat_remaining; // percentuale rimanente
break;
case LINK_ID: // 0x14
c_lq = crsf.link_up_quality;
break;
default: break;
}
}
static void mav_tick() {
uint32_t now = millis();
if (now - tATT >= 100) { // ATTITUDE 10 Hz
tATT = now;
const float D2R = (float)M_PI / 180.0f;
mavlink_msg_attitude_pack(SYSID, COMPID, &mmsg,
now, c_roll * D2R, c_pitch * D2R, c_yaw * D2R, 0, 0, 0);
sendMsg();
}
if (now - tGPS >= 200) { // GPS_RAW_INT 5 Hz
tGPS = now;
bool hasPos = (c_lat != 0) || (c_lon != 0);
uint8_t fix = (c_sats >= 5 && hasPos) ? GPS_FIX_TYPE_3D_FIX
: (c_sats > 0 ? GPS_FIX_TYPE_2D_FIX : GPS_FIX_TYPE_NO_FIX);
mavlink_msg_gps_raw_int_pack(SYSID, COMPID, &mmsg,
(uint64_t)now * 1000ULL, fix, c_lat, c_lon, c_alt_m * 1000,
UINT16_MAX, UINT16_MAX, // eph, epv sconosciuti
(uint16_t)(c_gs_mps * 100.0f), // vel cm/s
(uint16_t)(c_head_deg * 100.0f), // cog cdeg
c_sats,
0, 0, 0, 0, 0, 0); // campi extension -> 0
sendMsg();
}
if (now - tHUD >= 200) { // VFR_HUD 5 Hz
tHUD = now;
int16_t hdg = (int16_t)c_head_deg;
mavlink_msg_vfr_hud_pack(SYSID, COMPID, &mmsg,
c_gs_mps, c_gs_mps, hdg, 0, (float)c_alt_m, 0);
sendMsg();
}
if (now - tSYS >= 500) { // SYS_STATUS 2 Hz (corrente!)
tSYS = now;
uint16_t vbat_mv = (uint16_t)(c_vbat * 1000.0f);
int16_t curr_ca = (int16_t)(c_curr * 100.0f); // A -> cA (spec MAVLink)
mavlink_msg_sys_status_pack(SYSID, COMPID, &mmsg,
0, 0, 0, // sensors present/enabled/health
0, // load
vbat_mv, curr_ca, (int8_t)c_rem, // batteria: V, A, % rimanente
0, 0, 0, 0, 0, 0, // drop_rate, errors...
0, 0, 0); // campi extended -> 0
sendMsg();
}
if (now - tBAT >= 1000) { // BATTERY_STATUS 1 Hz (msg dedicato batteria)
tBAT = now;
uint16_t volts[10];
volts[0] = (uint16_t)(c_vbat * 1000.0f); // cella 0 = tensione pacco (mV)
for (int k = 1; k < 10; k++) volts[k] = UINT16_MAX; // celle non note
uint16_t volts_ext[4] = { 0, 0, 0, 0 };
mavlink_msg_battery_status_pack(SYSID, COMPID, &mmsg,
0, MAV_BATTERY_FUNCTION_ALL, MAV_BATTERY_TYPE_LIPO,
INT16_MAX, // temperatura non nota
volts,
(int16_t)(c_curr * 100.0f), // corrente in cA
(int32_t)c_mah, // consumo in mAh
-1, // energia consumata non nota
(int8_t)c_rem, // % rimanente
0, // time_remaining non noto
MAV_BATTERY_CHARGE_STATE_UNDEFINED,
volts_ext, 0, 0);
sendMsg();
}
if (now - tRADIO >= 1000) { // RADIO_STATUS 1 Hz (RSSI / link quality)
tRADIO = now;
uint8_t r = c_lq; // link quality % (0..100)
mavlink_msg_radio_status_pack(SYSID, COMPID, &mmsg,
r, r, 100, 0, 0, 0, 0); // rssi, remrssi, txbuf, noise...
sendMsg();
}
if (now - tHB >= 1000) { // HEARTBEAT 1 Hz
tHB = now;
mavlink_msg_heartbeat_pack(SYSID, COMPID, &mmsg,
MAV_TYPE_FIXED_WING, MAV_AUTOPILOT_GENERIC, 0, 0, MAV_STATE_ACTIVE);
sendMsg();
}
}
const Mode MODE_MAVLINK = { "MAVLINK", true, mav_begin, mav_onFrame, mav_tick };