#include /* 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; }