diff --git a/Multiprotocol/A7105_SPI.ino b/Multiprotocol/A7105_SPI.ino index ae58415..51b1847 100644 --- a/Multiprotocol/A7105_SPI.ino +++ b/Multiprotocol/A7105_SPI.ino @@ -198,6 +198,7 @@ void A7105_AdjustLOBaseFreq(uint8_t cmd) #endif break; case PROTO_AFHDS2A: + case PROTO_AFHDS2A_RX: #ifdef FORCE_AFHDS2A_TUNING offset=(int16_t)FORCE_AFHDS2A_TUNING; #endif @@ -252,7 +253,7 @@ static void __attribute__((unused)) A7105_SetVCOBand(uint8_t vb1, uint8_t vb2) A7105_WriteReg(A7105_25_VCO_SBCAL_I, vb2 | 0x08); } -#ifdef AFHDS2A_A7105_INO +#if defined(AFHDS2A_A7105_INO) || defined(AFHDS2A_RX_A7105_INO) const uint8_t PROGMEM AFHDS2A_A7105_regs[] = { 0xFF, 0x42 | (1<<5), 0x00, 0x25, 0x00, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x01, 0x3c, 0x05, 0x00, 0x50, // 00 - 0f 0x9e, 0x4b, 0x00, 0x02, 0x16, 0x2b, 0x12, 0x4f, 0x62, 0x80, 0xFF, 0xFF, 0x2a, 0x32, 0xc3, 0x1f, // 10 - 1f @@ -329,7 +330,7 @@ void A7105_Init(void) else #endif { - #ifdef AFHDS2A_A7105_INO + #if defined(AFHDS2A_A7105_INO) || defined(AFHDS2A_RX_A7105_INO) A7105_Regs=(uint8_t*)AFHDS2A_A7105_regs; #endif } diff --git a/Multiprotocol/AFHDS2A_Rx_a7105.ino b/Multiprotocol/AFHDS2A_Rx_a7105.ino new file mode 100644 index 0000000..fcf9413 --- /dev/null +++ b/Multiprotocol/AFHDS2A_Rx_a7105.ino @@ -0,0 +1,207 @@ +/* + This project is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + Multiprotocol is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with Multiprotocol. If not, see . + */ + +#if defined(AFHDS2A_RX_A7105_INO) + +#include "iface_a7105.h" + +#define AFHDS2A_RX_TXPACKET_SIZE 38 +#define AFHDS2A_RX_RXPACKET_SIZE 37 +#define AFHDS2A_RX_NUMFREQ 16 + +static uint8_t afhds2a_rx_data_started; +static uint8_t afhds2a_rx_disable_lna; + +enum { + AFHDS2A_RX_BIND1, + AFHDS2A_RX_BIND2, + AFHDS2A_RX_DATA +}; + +static void __attribute__((unused)) AFHDS2A_Rx_build_telemetry_packet() +{ + uint32_t bits = 0; + uint8_t bitsavailable = 0; + uint8_t idx = 0; + + pkt[idx++] = RX_LQI; // 0 - 130 + pkt[idx++] = RX_RSSI; + pkt[idx++] = 0; // start channel + pkt[idx++] = 14; // number of channels in packet + // pack channels + for (uint8_t i = 0; i < 14; i++) { + uint16_t val = packet[9+i*2] | (packet[10+i*2] << 8); + if (val < 860) + val = 860; + else if (val > 2140) + val = 2140; + val -= 860; + + bits |= val << bitsavailable; + bitsavailable += 11; + while (bitsavailable >= 8) { + pkt[idx++] = bits & 0xff; + bits >>= 8; + bitsavailable -= 8; + } + } +} + +static uint8_t __attribute__((unused)) AFHDS2A_Rx_data_ready() +{ + // check if FECF+CRCF Ok + return !(A7105_ReadReg(A7105_00_MODE) & (1 << 5 | 1 << 6 | 1 << 0)); +} + +uint16_t initAFHDS2A_Rx() +{ + uint8_t i; + A7105_Init(); + hopping_frequency_no = 0; + packet_count = 0; + afhds2a_rx_data_started = 0; + afhds2a_rx_disable_lna = IS_POWER_FLAG_on; + CC2500_SetTxRxMode(afhds2a_rx_disable_lna ? TXRX_OFF : RX_EN); + A7105_Strobe(A7105_RX); + + if (IS_BIND_IN_PROGRESS) { + phase = AFHDS2A_RX_BIND1; + } + else { + uint16_t temp = AFHDS2A_RX_EEPROM_OFFSET; + for (i = 0; i < 4; i++) + rx_id[i] = eeprom_read_byte((EE_ADDR)temp++); + for (i = 0; i < AFHDS2A_RX_NUMFREQ; i++) + hopping_frequency[i] = eeprom_read_byte((EE_ADDR)temp++); + phase = AFHDS2A_RX_DATA; + } + return 1000; +} + +#define AFHDS2A_RX_WAIT_WRITE 0x80 + +uint16_t AFHDS2A_Rx_callback() +{ + static uint32_t pps_timer = 0; + static uint16_t pps_counter = 0; + static int8_t read_retry; + int16_t temp; + uint8_t i; + +#ifndef FORCE_AFHDS2A_TUNING + A7105_AdjustLOBaseFreq(1); +#endif + if (afhds2a_rx_disable_lna != IS_POWER_FLAG_on) { + afhds2a_rx_disable_lna = IS_POWER_FLAG_on; + CC2500_SetTxRxMode(afhds2a_rx_disable_lna ? TXRX_OFF : RX_EN); + } + + switch(phase) { + case AFHDS2A_RX_BIND1: + if (AFHDS2A_Rx_data_ready()) { + A7105_ReadData(AFHDS2A_RX_TXPACKET_SIZE); + if ((packet[0] == 0xbb && packet[9] == 0x01) || (packet[0] == 0xbc && packet[9] <= 0x02)) { + memcpy(rx_id, &packet[1], 4); // TX id actually + memcpy(hopping_frequency, &packet[11], AFHDS2A_RX_NUMFREQ); + phase = AFHDS2A_RX_BIND2; + } + } + A7105_WriteReg(A7105_0F_PLL_I, (packet_count++ & 1) ? 0x0D : 0x8C); // bind channels + A7105_SetTxRxMode(RX_EN); + A7105_Strobe(A7105_RX); + return 10000; + + case AFHDS2A_RX_BIND2: + // got 2nd bind packet from tx ? + if (AFHDS2A_Rx_data_ready()) { + A7105_ReadData(AFHDS2A_RX_TXPACKET_SIZE); + if ((packet[0] == 0xBC && packet[9] == 0x02 && packet[10] == 0x00) && + (memcmp(rx_id, &packet[1], 4) == 0) && + (memcmp(rx_tx_addr, &packet[5], 4) == 0)) { + // save tx info to eeprom + temp = AFHDS2A_RX_EEPROM_OFFSET; + for (i = 0; i < 4; i++) + eeprom_write_byte((EE_ADDR)temp++, rx_id[i]); + for (i = 0; i < AFHDS2A_RX_NUMFREQ; i++) + eeprom_write_byte((EE_ADDR)temp++, hopping_frequency[i]); + BIND_DONE; + phase = AFHDS2A_RX_DATA; + return 3850; + } + } + + // transmit response packet + packet[0] = 0xBC; + memcpy(&packet[1], rx_id, 4); + memcpy(&packet[5], rx_tx_addr, 4); + packet[9] = 0x01; + packet[10] = 0x00; + memset(&packet[11], 0xFF, 26); + A7105_WriteData(AFHDS2A_RX_RXPACKET_SIZE, packet_count++ & 1 ? 0x0D : 0x8C); + phase |= AFHDS2A_RX_WAIT_WRITE; + return 1700; + + case AFHDS2A_RX_BIND2 | AFHDS2A_RX_WAIT_WRITE: + //Wait for TX completion + pps_timer = micros(); + while (micros() - pps_timer < 700) // Wait max 700µs, using serial+telemetry exit in about 120µs + if (!(A7105_ReadReg(A7105_00_MODE) & 0x01)) + break; + A7105_Strobe(A7105_RX); + phase &= ~AFHDS2A_RX_WAIT_WRITE; + return 10000; + + case AFHDS2A_RX_DATA: + if (AFHDS2A_Rx_data_ready()) { + A7105_ReadData(AFHDS2A_RX_TXPACKET_SIZE); + if (memcmp(&packet[1], rx_id, 4) == 0 && memcmp(&packet[5], rx_tx_addr, 4) == 0) { + if (packet[0] = 0x58 && packet[37] == 0x00 && telemetry_link == 0) { // standard packet, send channels to TX + int rssi = min(A7105_ReadReg(A7105_1D_RSSI_THOLD),160); + RX_RSSI = map(rssi, 160, 8, 0, 100); + AFHDS2A_Rx_build_telemetry_packet(); + telemetry_link = 1; + } + afhds2a_rx_data_started = 1; + read_retry = 10; // hop to next channel + pps_counter++; + } + } + + // packets per second + if (millis() - pps_timer >= 1000) { + pps_timer = millis(); + debugln("%ld pps", pps_counter); + RX_LQI = pps_counter / 2; + pps_counter = 0; + } + + // frequency hopping + if (read_retry++ >= 10) { + hopping_frequency_no++; + if(hopping_frequency_no >= AFHDS2A_RX_NUMFREQ) + hopping_frequency_no = 0; + A7105_WriteReg(A7105_0F_PLL_I, hopping_frequency[hopping_frequency_no]); + A7105_Strobe(A7105_RX); + if (afhds2a_rx_data_started) + read_retry = 0; + else + read_retry = -127; // retry longer until first packet is catched + } + return 385; + } + return 3850; // never reached +} + +#endif diff --git a/Multiprotocol/Multi.txt b/Multiprotocol/Multi.txt index bd9d6c3..b866d0b 100644 --- a/Multiprotocol/Multi.txt +++ b/Multiprotocol/Multi.txt @@ -53,4 +53,5 @@ 53,Flyzone,FZ-410 54,Scanner 55,FrskyX_RX,FCC,EU_LBT +56,AFHDS2A_RX 63,XN_DUMP,250K,1M,2M diff --git a/Multiprotocol/Multiprotocol.h b/Multiprotocol/Multiprotocol.h index 5b7c5a0..9f47a6f 100644 --- a/Multiprotocol/Multiprotocol.h +++ b/Multiprotocol/Multiprotocol.h @@ -82,6 +82,7 @@ enum PROTOCOLS PROTO_FLYZONE = 53, // =>A7105 PROTO_SCANNER = 54, // =>CC2500 PROTO_FRSKYX_RX = 55, // =>CC2500 + PROTO_AFHDS2A_RX= 56, // =>A7105 PROTO_XN297DUMP = 63, // =>NRF24L01 }; @@ -575,7 +576,8 @@ enum { #define AFHDS2A_EEPROM_OFFSET 50 // RX ID, 4 bytes per model id, end is 50+64=114 #define BUGS_EEPROM_OFFSET 114 // RX ID, 2 bytes per model id, end is 114+32=146 #define BUGSMINI_EEPROM_OFFSET 146 // RX ID, 2 bytes per model id, end is 146+32=178 -#define FRSKYX_RX_EEPROM_OFFSET 178 // (3) TX ID + (1) freq_tune + (47) channels, 51 bytes per model, end is 178+51=229 +#define FRSKYX_RX_EEPROM_OFFSET 178 // (3) TX ID + (1) freq_tune + (47) channels, 51 bytes, end is 178+51=229 +#define AFHDS2A_RX_EEPROM_OFFSET 229 // (4) TX ID + (16) channels, 20 bytes, end is 229+20=249 //#define CONFIG_EEPROM_OFFSET 210 // Current configuration of the multimodule //**************************************** diff --git a/Multiprotocol/Multiprotocol.ino b/Multiprotocol/Multiprotocol.ino index 433c824..5775b5b 100644 --- a/Multiprotocol/Multiprotocol.ino +++ b/Multiprotocol/Multiprotocol.ino @@ -1003,6 +1003,13 @@ static void protocol_init() remote_callback = ReadFlyzone; break; #endif + #if defined(AFHDS2A_RX_A7105_INO) + case PROTO_AFHDS2A_RX: + PE1_off; //antenna RF1 + next_callback = initAFHDS2A_Rx(); + remote_callback = AFHDS2A_Rx_callback; + break; + #endif #endif #ifdef CC2500_INSTALLED #if defined(FRSKYD_CC2500_INO) diff --git a/Multiprotocol/Telemetry.ino b/Multiprotocol/Telemetry.ino index da0b08c..40e83d7 100644 --- a/Multiprotocol/Telemetry.ino +++ b/Multiprotocol/Telemetry.ino @@ -184,16 +184,21 @@ static void multi_send_status() } #endif -#ifdef FRSKYX_RX_TELEMETRY - void frskyx_rx_channels_frame() +#if defined (FRSKYX_RX_TELEMETRY) || defined (AFHDS2A_RX_TELEMETRY) + void receiver_channels_frame() { + uint16_t len = pkt[3] * 11; // 11 bit per channel + if (len % 8 == 0) + len = 4 + (len / 8); + else + len = 5 + (len / 8); #if defined MULTI_TELEMETRY - multi_send_header(MULTI_TELEMETRY_RX_CHANNELS, 26); + multi_send_header(MULTI_TELEMETRY_RX_CHANNELS, len); #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 + for (uint8_t i = 0; i < len; i++) + Serial_write(pkt[i]); // pps, rssi, ch start, ch count, packed ch data } #endif @@ -1032,10 +1037,10 @@ void TelemetryUpdate() } #endif - #if defined FRSKYX_RX_TELEMETRY - if (telemetry_link && protocol == PROTO_FRSKYX_RX) + #if defined (FRSKYX_RX_TELEMETRY) || defined(AFHDS2A_RX_TELEMETRY) + if (telemetry_link && (protocol == PROTO_FRSKYX_RX || protocol == PROTO_AFHDS2A_RX)) { - frskyx_rx_channels_frame(); + receiver_channels_frame(); telemetry_link = 0; return; } diff --git a/Multiprotocol/Validate.h b/Multiprotocol/Validate.h index 4479c9b..7ced7b1 100644 --- a/Multiprotocol/Validate.h +++ b/Multiprotocol/Validate.h @@ -174,6 +174,7 @@ #undef AFHDS2A_A7105_INO #undef BUGS_A7105_INO #undef FLYZONE_A7105_INO + #undef AFHDS2A_RX_A7105_INO #endif #ifndef CYRF6936_INSTALLED #undef DEVO_CYRF6936_INO @@ -254,6 +255,8 @@ #undef SCANNER_CC2500_INO #undef FRSKYX_RX_TELEMETRY #undef FRSKYX_RX_CC2500_INO + #undef AFHDS2A_RX_TELEMETRY + #undef AFHDS2A_RX_A7105_INO #else #if defined(MULTI_TELEMETRY) && defined(MULTI_STATUS) #error You should choose either MULTI_TELEMETRY or MULTI_STATUS but not both. @@ -266,6 +269,10 @@ #undef FRSKYX_RX_TELEMETRY #undef FRSKYX_RX_CC2500_INO #endif + #if not defined(AFHDS2A_RX_A7105_INO) || not defined(AFHDS2A_RX_TELEMETRY) + #undef AFHDS2A_RX_TELEMETRY + #undef AFHDS2A_RX_A7105_INO + #endif #if not defined(BAYANG_NRF24L01_INO) #undef BAYANG_HUB_TELEMETRY #endif diff --git a/Multiprotocol/_Config.h b/Multiprotocol/_Config.h index 6f71e8c..8c4b757 100644 --- a/Multiprotocol/_Config.h +++ b/Multiprotocol/_Config.h @@ -157,6 +157,7 @@ //The protocols below need an A7105 to be installed #define AFHDS2A_A7105_INO +#define AFHDS2A_RX_A7105_INO #define BUGS_A7105_INO #define FLYSKY_A7105_INO #define FLYZONE_A7105_INO @@ -286,6 +287,7 @@ #define HITEC_FW_TELEMETRY // Under development: Forward received telemetry packets to be decoded by ersky9x and OpenTX #define SCANNER_TELEMETRY // Forward spectrum scanner data to TX #define FRSKYX_RX_TELEMETRY // Forward channels data to TX +#define AFHDS2A_RX_TELEMETRY // Forward channels data to TX //SPORT_POLLING is an implementation of the same polling routine as XJT module for sport telemetry bidirectional communication. //This is useful for passing sport control frames from TX to RX(ex: changing Betaflight PID or VTX channels on the fly using LUA scripts with OpentX). @@ -476,6 +478,8 @@ const PPM_Parameters PPM_prot[14*NBR_BANKS]= { PPM_IBUS PWM_SBUS PPM_SBUS + PROTO_AFHDS2A_RX + NONE PROTO_ASSAN NONE PROTO_BAYANG