Snapshot iniziale: bridge telemetria CRSF->LTM/MAVLink (ESP-NOW)
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:
57
02_telemetry_bridge/lib/Ltm/Ltm.cpp
Normal file
57
02_telemetry_bridge/lib/Ltm/Ltm.cpp
Normal file
@@ -0,0 +1,57 @@
|
||||
#include "Ltm.h"
|
||||
|
||||
namespace Ltm {
|
||||
|
||||
// Scrive header "$T<code>" in out, ritorna l'indice del primo byte payload (3).
|
||||
static inline size_t header(uint8_t* out, char code) {
|
||||
out[0] = '$';
|
||||
out[1] = 'T';
|
||||
out[2] = (uint8_t)code;
|
||||
return 3;
|
||||
}
|
||||
|
||||
// Calcola CRC (XOR del payload da [3] a [n-1]) e lo appende. Ritorna n+1.
|
||||
static inline size_t finalise(uint8_t* out, size_t n) {
|
||||
uint8_t crc = 0;
|
||||
for (size_t i = 3; i < n; ++i) crc ^= out[i];
|
||||
out[n] = crc;
|
||||
return n + 1;
|
||||
}
|
||||
|
||||
static inline void wU16(uint8_t* p, uint16_t v) { p[0] = v & 0xFF; p[1] = (v >> 8) & 0xFF; }
|
||||
static inline void wU32(uint8_t* p, uint32_t v) {
|
||||
p[0] = v & 0xFF; p[1] = (v >> 8) & 0xFF; p[2] = (v >> 16) & 0xFF; p[3] = (v >> 24) & 0xFF;
|
||||
}
|
||||
|
||||
size_t gframe(uint8_t* out, int32_t lat, int32_t lon,
|
||||
uint8_t gs_mps, int32_t alt_cm, uint8_t sats, uint8_t fix) {
|
||||
size_t n = header(out, 'G');
|
||||
wU32(&out[n], (uint32_t)lat); n += 4;
|
||||
wU32(&out[n], (uint32_t)lon); n += 4;
|
||||
out[n++] = gs_mps;
|
||||
wU32(&out[n], (uint32_t)alt_cm); n += 4;
|
||||
out[n++] = (uint8_t)((sats << 2) | (fix & 0x03));
|
||||
return finalise(out, n); // 18 byte
|
||||
}
|
||||
|
||||
size_t aframe(uint8_t* out, int16_t pitch, int16_t roll, int16_t heading) {
|
||||
size_t n = header(out, 'A');
|
||||
wU16(&out[n], (uint16_t)pitch); n += 2;
|
||||
wU16(&out[n], (uint16_t)roll); n += 2;
|
||||
wU16(&out[n], (uint16_t)heading); n += 2;
|
||||
return finalise(out, n); // 10 byte
|
||||
}
|
||||
|
||||
size_t sframe(uint8_t* out, uint16_t vbat_mv, uint16_t current,
|
||||
uint8_t rssi, uint8_t airspeed,
|
||||
uint8_t flightmode, uint8_t statemode) {
|
||||
size_t n = header(out, 'S');
|
||||
wU16(&out[n], vbat_mv); n += 2;
|
||||
wU16(&out[n], current); n += 2;
|
||||
out[n++] = rssi;
|
||||
out[n++] = airspeed;
|
||||
out[n++] = (uint8_t)((flightmode << 2) | (statemode & 0x03));
|
||||
return finalise(out, n); // 11 byte
|
||||
}
|
||||
|
||||
} // namespace Ltm
|
||||
Reference in New Issue
Block a user