diff --git a/Multiprotocol/FrSkyX_Rx_cc2500.ino b/Multiprotocol/FrSkyX_Rx_cc2500.ino index 5204df0..fc73660 100644 --- a/Multiprotocol/FrSkyX_Rx_cc2500.ino +++ b/Multiprotocol/FrSkyX_Rx_cc2500.ino @@ -23,9 +23,12 @@ static uint16_t frskyx_bind_check; static uint8_t frskyx_rx_txid[3]; + static uint16_t frskyx_rx_rc_chan[16]; static uint8_t frskyx_rx_chanskip; static uint8_t frskyx_rx_disable_lna; static uint8_t frskyx_rx_data_started; + static uint8_t frskyx_rx_rssi; + static uint8_t frskyx_rx_pps; static void __attribute__((unused)) FrSkyX_Rx_initialise() { CC2500_Reset(); @@ -125,6 +128,49 @@ uint8_t __attribute__((unused)) frskyx_rx_check_crc() return lcrc == rcrc; } +void __attribute__((unused)) frskyx_rx_build_telemetry_packet() +{ + uint16_t pxx_channel[8]; + uint32_t bits = 0; + uint8_t bitsavailable = 0; + uint8_t idx = 0; + + // decode PXX channels + pxx_channel[0] = ((packet[10] << 8) & 0xF00) | packet[9]; + pxx_channel[1] = ((packet[11] << 4) & 0xFF0) | (packet[10] >> 4); + pxx_channel[2] = ((packet[13] << 8) & 0xF00) | packet[12]; + pxx_channel[3] = ((packet[14] << 4) & 0xFF0) | (packet[13] >> 4); + pxx_channel[4] = ((packet[16] << 8) & 0xF00) | packet[15]; + pxx_channel[5] = ((packet[17] << 4) & 0xFF0) | (packet[16] >> 4); + pxx_channel[6] = ((packet[19] << 8) & 0xF00) | packet[18]; + pxx_channel[7] = ((packet[20] << 4) & 0xFF0) | (packet[19] >> 4); + for (unsigned i = 0; i < 8; i++) { + uint8_t shifted = (pxx_channel[i] & 0x800)>0; + uint16_t channel_value = pxx_channel[i] & 0x7FF; + if (channel_value < 64) + frskyx_rx_rc_chan[shifted ? i + 8 : i] = 0; + else + frskyx_rx_rc_chan[shifted ? i + 8 : i] = min(((channel_value - 64) << 4) / 15, 2047); + } + + // buid telemetry packet + pkt[idx++] = frskyx_rx_pps; + pkt[idx++] = frskyx_rx_rssi; + pkt[idx++] = 1; // start channel + pkt[idx++] = 16; // number of channels in packet + + // pack channels + for (int i = 0; i < 16; i++) { + bits |= frskyx_rx_rc_chan[i] << bitsavailable; + bitsavailable += 11; + while (bitsavailable >= 8) { + pkt[idx++] = bits & 0xff; + bits >>= 8; + bitsavailable -= 8; + } + } +} + uint16_t initFrSkyX_Rx() { FrSkyX_Rx_initialise(); @@ -211,19 +257,18 @@ uint16_t FrSkyX_Rx_callback() if (len >= packet_length) { CC2500_ReadData(packet, packet_length); if (packet[1] == frskyx_rx_txid[0] && packet[2] == frskyx_rx_txid[1] && packet[6] == frskyx_rx_txid[2] && frskyx_rx_check_crc()) { + frskyx_rx_rssi = CC2500_ReadReg(CC2500_READ_BURST | CC2500_34_RSSI); + if(frskyx_rx_rssi >= 128) + frskyx_rx_rssi -= 128; + else + frskyx_rx_rssi += 128; // hop to next channel frskyx_rx_chanskip = ((packet[4] & 0xC0) >> 6) | ((packet[5] & 0x3F) << 2); hopping_frequency_no = (hopping_frequency_no + frskyx_rx_chanskip) % 47; frskyx_rx_set_channel(hopping_frequency_no); if(packet[7] == 0 && telemetry_link == 0) { // standard packet, send channels to TX - if(telemetry_link == 0) { - memcpy(pkt, &packet[9], 12); + frskyx_rx_build_telemetry_packet(); telemetry_link = 1; - } - //int16_t chan1 = packet[9] | ((packet[10] & 0x0F) << 8); - //int16_t chan2= ((packet[10] & 0xF0) >> 4) | (packet[11] << 4); - //if(chan1 < 2048) - // debugln("Ch1: %d Ch2: %d", chan1, chan2); } frskyx_rx_data_started = 1; read_retry = 0; @@ -231,10 +276,11 @@ uint16_t FrSkyX_Rx_callback() } } - // debug packets per second + // packets per second if (millis() - pps_timer >= 1000) { pps_timer = millis(); debugln("%ld pps", pps_counter); + frskyx_rx_pps = pps_counter; pps_counter = 0; } diff --git a/Multiprotocol/Multiprotocol.h b/Multiprotocol/Multiprotocol.h index 4312fd1..ad67e8b 100644 --- a/Multiprotocol/Multiprotocol.h +++ b/Multiprotocol/Multiprotocol.h @@ -327,7 +327,7 @@ enum MultiPacketTypes MULTI_TELEMETRY_HITEC = 10, MULTI_TELEMETRY_SCANNER = 11, MULTI_TELEMETRY_AFHDS2A_AC = 12, - MULTI_TELEMETRY_CHANNELS = 13, + MULTI_TELEMETRY_RX_CHANNELS = 13, }; // Macros @@ -912,7 +912,11 @@ Serial: 100000 Baud 8e2 _ xxxx xxxx p -- data[1-28] telemetry data Type 0x0D RX channels forwarding - length: 22 - data[0-21] packed channels data, 16 channels, 11 bit per channel, msb first + length: variable + data[0] = received packets per second + data[1] = rssi + data[2] = start channel + data[3] = number of channels to follow + data[4-]= packed channels data, 11 bit per channel */ diff --git a/Multiprotocol/Telemetry.ino b/Multiprotocol/Telemetry.ino index 7a486e2..da0b08c 100644 --- a/Multiprotocol/Telemetry.ino +++ b/Multiprotocol/Telemetry.ino @@ -184,6 +184,19 @@ static void multi_send_status() } #endif +#ifdef FRSKYX_RX_TELEMETRY + void frskyx_rx_channels_frame() + { + #if defined MULTI_TELEMETRY + multi_send_header(MULTI_TELEMETRY_RX_CHANNELS, 26); + #else + Serial_write(0xAA); // Telemetry packet + #endif + for (uint8_t i = 0; i < 26; i++) + Serial_write(pkt[i]); // pps, rssi, ch start, ch count, 16x ch data + } +#endif + #ifdef AFHDS2A_FW_TELEMETRY void AFHDSA_short_frame() { @@ -1022,8 +1035,7 @@ void TelemetryUpdate() #if defined FRSKYX_RX_TELEMETRY if (telemetry_link && protocol == PROTO_FRSKYX_RX) { - // TODO - //channels_frame(); + frskyx_rx_channels_frame(); telemetry_link = 0; return; }