// ============================================================ // 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 #include 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; // 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 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; } if (id) lastFrameMs = millis(); // frame valido ricevuto } static void mav_tick() { uint32_t now = millis(); bool stale = (lastFrameMs == 0) || (now - lastFrameMs > TELEM_TIMEOUT_MS); if (now - tATT >= 100) { // ATTITUDE 10 Hz tATT = now; const float D2R = (float)M_PI / 180.0f; // Pitch e roll invertiti: con naso giu' / ala abbassata mwp mostrava // l'indicatore di assetto nel verso opposto (segno opposto alla // convenzione attesa dal GCS). Nego pitch e roll per allineare // l'orizzonte artificiale a mwp. Yaw ok. 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 = 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); 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); // NB: NON spec-compliant di proposito. Lo spec MAVLink vuole cA (A*100), // ma mwp interpreta current_battery come mA e divide per 1000, mostrando // quindi A/10 (1.2A -> 0.12A). Mandiamo A*1000 per compensare il bug di // mwp: 1.2A -> 1200 -> mwp mostra 1.2A. Limite: overflow int16 oltre ~32A. int16_t curr_ca = (int16_t)(c_curr * 1000.0f); // A*1000 (workaround mwp) 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 * 1000.0f), // A*1000 (workaround mwp, vedi SYS_STATUS) (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; // RADIO_STATUS.rssi/remrssi sono attesi su scala 0..254 (i GCS la // riscalano a percentuale: mwp fa raw*1023/255, cioe' % = raw/2.55). // c_lq e' 0..100: va riscalato a 0..254, altrimenti con LQ=100 mwp // mostra 100/2.55 = 39% fisso (bug "RSSI bloccato al 39%"). Stesso // scaling gia' usato dal modo LTM. 254 e non 255: alcuni reader // trattano 255 come "invalido". uint8_t r = (uint8_t)((uint16_t)c_lq * 254 / 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, stale ? MAV_STATE_CRITICAL : MAV_STATE_ACTIVE); sendMsg(); } } const Mode MODE_MAVLINK = { "MAVLINK", true, mav_begin, mav_onFrame, mav_tick };