67933bcfc1
RADIO_STATUS.rssi/remrssi vanno su scala 0..254 (i GCS riscalano a %: mwp fa raw*1023/255). Si mandava link_quality grezzo 0..100, quindi con LQ=100 mwp mostrava 100/2.55 = 39% fisso. Riscalato a 0..254 (come gia' fa il modo LTM). Root cause confermata dal sorgente mwp (mwp-mavlink_msg.vala). Co-Authored-By: Claude Opus 4.8 <noreply@anthropic.com>
165 lines
7.4 KiB
C++
165 lines
7.4 KiB
C++
// ============================================================
|
|
// 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;
|
|
// 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, MAV_STATE_ACTIVE);
|
|
sendMsg();
|
|
}
|
|
}
|
|
|
|
const Mode MODE_MAVLINK = { "MAVLINK", true, mav_begin, mav_onFrame, mav_tick };
|