e22a47fce1
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>
58 lines
1.9 KiB
C++
58 lines
1.9 KiB
C++
#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
|