5d6f646a5e
terseCRSF vendorizzata: rimosse initialise/sbus_initialise/readCrsfFrame/ decodeRC/printPWM/prepSBUS/bytesToPWM/pwmToBytes/sendSBUS e i membri/buffer SBUS-PWM (sb_bytes, pwm_val, crsf_port, sbus_port, RC_BUILD/SBUS machinery). Usiamo solo decodeTelemetry: mantenuti i decoder + helper (bytes2*, wrap360, crc8_dvb_s2[_sbuf_accum] usata dalla patch CRC, printByte/printBytes). ~283 righe in meno, nessun riferimento pendente (verificato con grep su 01/02/03). EspNowCrsf: rimosso sendRaw() e il peer broadcast in begin() (uplink sperimentale morto, il backpack e' downlink-only). Ora RX-only pulito. main.cpp: commento "MAVLink v1" -> "v2" (la lib emette v2). DA VERIFICARE A BANCO con un build (qui non compilabile). Co-Authored-By: Claude Opus 4.8 <noreply@anthropic.com>
250 lines
8.7 KiB
C++
250 lines
8.7 KiB
C++
#include <terseCRSF.h>
|
||
/*
|
||
CRSF::CRSF() :
|
||
crsf_crc(0xd5),
|
||
blah(0)
|
||
{
|
||
// constructor for CRSF class
|
||
}
|
||
*/
|
||
//=======================================================
|
||
uint8_t crc8_dvb_s2(uint8_t crc, unsigned char a)
|
||
{
|
||
crc ^= a;
|
||
for (int ii = 0; ii < 8; ++ii) {
|
||
if (crc & 0x80) {
|
||
crc = (crc << 1) ^ 0xD5;
|
||
} else {
|
||
crc = crc << 1;
|
||
}
|
||
}
|
||
return crc;
|
||
}
|
||
//=======================================================
|
||
uint8_t crc8_dvb_s2_sbuf_accum(const void *data, uint8_t frm_lth)
|
||
{
|
||
uint8_t crc = 0;
|
||
const uint8_t *p = (const uint8_t *)data;
|
||
const uint8_t *pend = p + frm_lth;
|
||
|
||
for (; p != pend; p++)
|
||
{
|
||
//log.printf("crc *p 0x%2X\n", *p);
|
||
crc = crc8_dvb_s2(crc, *p);
|
||
}
|
||
return crc;
|
||
}
|
||
//=======================================================
|
||
int32_t CRSF::bytes2int32(uint8_t *byt)
|
||
{
|
||
return ((byt[3] << 0) & 0xFF) + ((byt[2] << 8) & 0xFFFF) + ((byt[1] << 16) & 0xFFFFFF) + ((byt[0] << 24) & 0xFFFFFFFF);
|
||
}
|
||
//=======================================================
|
||
uint16_t CRSF::bytes2uint16(uint8_t *byt)
|
||
{
|
||
return ((byt[1] << 0) & 0xFF) + ((byt[0] << 8) & 0xFFFF);
|
||
}
|
||
//=======================================================
|
||
int16_t CRSF::bytes2int16(uint8_t *byt)
|
||
{
|
||
return ((byt[1] << 0) & 0xFF) + ((byt[0] << 8) & 0xFFFF);
|
||
}
|
||
//=======================================================
|
||
void CRSF::printByte(byte b, char delimiter)
|
||
{
|
||
if (b <= 0xf)
|
||
log.print("0");
|
||
log.print(b, HEX);
|
||
log.write(delimiter);
|
||
}
|
||
//========================================================
|
||
void CRSF::printBytes(uint8_t *buf, uint8_t len)
|
||
{
|
||
//log.printf("len:%2u:", len);
|
||
for (int i = 0; i < len; i++)
|
||
{
|
||
printByte(buf[i], ' ');
|
||
}
|
||
log.println();
|
||
}
|
||
//========================================================
|
||
void CRSF::printLinkStats()
|
||
{
|
||
#if defined SHOW_LINK_STATS
|
||
static uint32_t error_millis = 0;
|
||
if ((millis() - error_millis) > 1.2E5) // 2 minutes
|
||
{
|
||
error_millis = millis();
|
||
log.printf("frames_C:%u good_frames:%u crc_errors:%u frame_errors:%u unknown_ids:%u\n", frames_read, good_frames, crc_errors, frame_errors, unknown_ids);
|
||
}
|
||
#endif
|
||
}
|
||
//===================================================================
|
||
uint16_t CRSF::wrap360(int16_t ang)
|
||
{
|
||
if (ang < 0)
|
||
ang += 360;
|
||
if (ang > 359)
|
||
ang -= 360;
|
||
return ang;
|
||
}
|
||
//========================================================
|
||
uint8_t CRSF::decodeTelemetry(uint8_t *_buf, uint8_t len)
|
||
{
|
||
// PATCH LOCALE: validazione lunghezza + CRC8 prima di decodificare.
|
||
// L'upstream non valida nulla qui (la CRC era controllata solo in
|
||
// readCrsfFrame(), non usato perche' i frame arrivano da ESP-NOW): un
|
||
// frame corto o corrotto veniva decodificato leggendo byte oltre il
|
||
// frame e producendo telemetria spazzatura. 'len' = byte disponibili.
|
||
// _buf: [0]=sync [1]=lth(type+payload+crc) [2]=type ... [lth+1]=crc
|
||
uint8_t crsf_frm_lth = _buf[1];
|
||
if (len < 4) return 0;
|
||
if (crsf_frm_lth < 2) return 0; // almeno type+crc
|
||
if ((uint16_t)crsf_frm_lth + 2 > len) return 0; // frame non completo
|
||
uint8_t calc_crc = crc8_dvb_s2_sbuf_accum(&_buf[2], crsf_frm_lth - 1);
|
||
if (calc_crc != _buf[crsf_frm_lth + 1]) return 0; // CRC errata -> scarta
|
||
uint8_t crsf_id = _buf[2];
|
||
if (crsf_id == 0)
|
||
{
|
||
return 0;
|
||
}
|
||
#if defined SHOW_BUFFER
|
||
log.print("CRSF_BUF:");
|
||
printBytes(&*_buf, len); // plus header and crc bytes
|
||
#endif
|
||
switch (crsf_id)
|
||
{
|
||
case GPS_ID:
|
||
gps_lat = bytes2int32(&_buf[3]); // offset (&*(_buf+3))
|
||
gps_lon = bytes2int32(&_buf[7]);
|
||
gpsF_lat = (float)(gps_lat / 1e7); // degrees+decimals
|
||
gpsF_lon = (float)(gps_lon / 1e7);
|
||
gps_groundspeed = bytes2uint16(&_buf[11]);
|
||
gpsF_groundspeed = (float)(gps_groundspeed * 0.1); // km\hr
|
||
gps_heading = bytes2uint16(&_buf[13]);
|
||
gpsF_heading = (float)(gps_heading * 0.01); // degrees+decimals
|
||
gps_altitude = bytes2uint16(&_buf[15]); // metres, 1000m offset
|
||
gps_altitude = gps_altitude > 100 ? gps_altitude - 1000: gps_altitude;
|
||
gps_sats = (uint8_t)_buf[17];
|
||
break;
|
||
case CF_VARIO_ID:
|
||
#if defined SHOW_CRSF_CF_VARIO
|
||
log.print("CF_VARIO:");
|
||
printBytes(&*_buf, len); // plus header and crc bytes
|
||
#endif
|
||
break;
|
||
case BATTERY_ID:
|
||
bat_voltage = bytes2uint16(&_buf[3]); // mV * 100
|
||
batF_voltage = (float)bat_voltage * 0.1; // volts
|
||
bat_current = bytes2uint16(&_buf[5]); // mA * 100
|
||
batF_current = bat_current * 0.1; // amps
|
||
// PATCH LOCALE: la capacita' consumata e' un uint24 big-endian su
|
||
// _buf[7..9]; l'originale faceva bytes2int32(&_buf[7]) leggendo 4 byte
|
||
// e includendo _buf[10] (percent) come LSB -> es. 350 diventava 89678.
|
||
// Letti esplicitamente solo i 3 byte corretti.
|
||
bat_fuel_drawn = ((uint32_t)_buf[7] << 16) | ((uint32_t)_buf[8] << 8) | (uint32_t)_buf[9]; // mAh drawn
|
||
batF_fuel_drawn = bat_fuel_drawn; // Ah drawn
|
||
bat_remaining = (uint8_t)_buf[10]; // percent
|
||
break;
|
||
case BARO_ALT_ID:
|
||
#if defined SHOW_CRSF_BARO
|
||
log.print("BARO_ALT:");
|
||
printBytes(&*_buf, len); // plus header and crc bytes
|
||
#endif
|
||
break;
|
||
case HEARTBEAT_ID:
|
||
#if defined SHOW_CRSF_HEARTBEAT
|
||
log.print("HEARTBEAT:");
|
||
printBytes(&*_buf, len); // plus header and crc bytes
|
||
#endif
|
||
break;
|
||
case LINK_ID: // 0x14 Link statistics
|
||
link_up_rssi_ant_1 = (uint8_t)_buf[3]; // dBm * -1
|
||
link_up_rssi_ant_2 = (uint8_t)_buf[4]; // dBm * -1
|
||
link_up_quality = (uint8_t)_buf[5]; // packet_success_rate (%)
|
||
link_up_snr = (int8_t)_buf[6]; // db
|
||
link_diversity_active_ant = (uint8_t)_buf[7]; // (enum ant_1 = 0, ant_2)
|
||
link_rf_mode = (uint8_t)_buf[8]; // (enum 4fps = 0, 50fps, 150hz)
|
||
link_up_tx_power = (uint8_t)_buf[9]; // (enum 0mW = 0, 10mW, 25 mW, 100 mW, 500 mW, 1000 mW, 2000mW)
|
||
link_dn_rssi = (uint8_t)_buf[10]; // RSSI(dBm * -1)
|
||
link_dn_quality = (uint8_t)_buf[11]; // packet_success_rate (%)
|
||
link_dn_snr = (int8_t)_buf[12]; // db
|
||
break;
|
||
case CHANNELS_ID:
|
||
#if defined SHOW_CRSF_CHANNELS
|
||
log.print("CHANNELS:");
|
||
printBytes(&*_buf, len); // plus header and crc bytes
|
||
#endif
|
||
break;
|
||
case LINK_RX_ID:
|
||
#if defined SHOW_CRSF_LINK_RX
|
||
log.print("LINK_RX:");
|
||
printBytes(&*_buf, len); // plus header and crc bytes
|
||
#endif
|
||
break;
|
||
case LINK_TX_ID:
|
||
#if defined SHOW_CRSF_LINK_TX
|
||
log.print("LINK_TX:");
|
||
printBytes(&*_buf, len); // plus header and crc bytes
|
||
#endif
|
||
break;
|
||
case ATTITUDE_ID:
|
||
atti_pitch = bytes2int16(&_buf[3]); // rad / 10000
|
||
atti_roll = bytes2int16(&_buf[5]); // rad / 10000
|
||
atti_yaw = bytes2int16(&_buf[7]); // rad / 10000
|
||
attiF_pitch = (float)(atti_pitch * RADS2DEGS * 0.0001); // deg
|
||
attiF_roll = (float)(atti_roll * RADS2DEGS * 0.0001); // deg
|
||
atti_yaw = (int16_t)(atti_yaw * RADS2DEGS * 0.0001); // deg
|
||
atti_yaw = wrap360(atti_yaw);
|
||
attiF_yaw = (float)atti_yaw;
|
||
break;
|
||
case FLIGHT_MODE_ID:
|
||
/* HUH! Flight mode is a string*/
|
||
flight_mode_lth = crsf_frm_lth - 3; // fix 2024-05-17
|
||
flightMode.resize(flight_mode_lth); // fix 2024-09-12
|
||
memcpy(&flightMode[0], &_buf[3], flight_mode_lth); // fix 2024-05-17
|
||
//printBytes(&_buf[3], flight_mode_lth);
|
||
break;
|
||
case PING_DEVICES_ID:
|
||
#if defined SHOW_CRSF_GPS_PING_DEVICES
|
||
log.print("PING_DEVICES:");
|
||
printBytes(&*_buf, len); // plus header and crc bytes
|
||
#endif
|
||
break;
|
||
case DEVICE_INFO_ID:
|
||
#if defined SHOW_CRSF_DEVIDE_INFO
|
||
log.print("DEVICE_INFO:");
|
||
printBytes(&*_buf, len); // plus header and crc bytes
|
||
#endif
|
||
break;
|
||
case REQUEST_SETTINGS_ID:
|
||
#if defined SHOW_CRSF_REQUEST_SETTINGS
|
||
log.print("REQUEST_SETTINGS:");
|
||
printBytes(&*_buf, len); // plus header and crc bytes
|
||
#endif
|
||
break;
|
||
case COMMAND_ID:
|
||
#if defined SHOW_CRSF_COMMAND
|
||
log.print("COMMAND:");
|
||
printBytes(&*_buf, len); // plus header and crc bytes
|
||
#endif
|
||
break;
|
||
case RADIO_ID:
|
||
#if defined SHOW_CRSF_RADIO
|
||
log.print("RADIO id:");
|
||
printBytes(&*_buf, len); // plus header and crc bytes
|
||
#endif
|
||
break;
|
||
default:
|
||
#if defined SHOW_OTHER_FRAME_IDs
|
||
log.print("crsf_id:");
|
||
printByte(crsf_id, ' ');
|
||
log.println();
|
||
//log.print("UNKNOWN ");
|
||
//printBytes(&*_buf, len); // plus header and CRC bytes
|
||
#endif
|
||
unknown_ids++;
|
||
return 0;
|
||
}
|
||
return crsf_id;
|
||
} |