From 1beaf738e435800f701837a6536fd5cf6edb3c1f Mon Sep 17 00:00:00 2001 From: Goebish Date: Tue, 1 Oct 2019 17:18:49 +0200 Subject: [PATCH] Bind & data Ok --- Multiprotocol/A7105_SPI.ino | 5 +- Multiprotocol/AFHDS2A_Rx_a7105.ino | 154 ++++++++++++++++++++++++++++- 2 files changed, 156 insertions(+), 3 deletions(-) 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 index 4ce9b6d..dd2ec1d 100644 --- a/Multiprotocol/AFHDS2A_Rx_a7105.ino +++ b/Multiprotocol/AFHDS2A_Rx_a7105.ino @@ -17,14 +17,166 @@ #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() +{ + +} + +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() { - return 1000; + 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 + // todo: read RSSI + 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; + 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]); // bind channels + A7105_Strobe(A7105_RX); + if (afhds2a_rx_data_started) + read_retry = 0; + else + read_retry = -50; // retry longer until first packet is catched + } + return 385; + } + return 3850; // never reached } #endif