From d5f819dd590bf5d928e27b62d3aac61f98b734ce Mon Sep 17 00:00:00 2001 From: Pascal Langer Date: Fri, 3 Jul 2020 19:51:11 +0200 Subject: [PATCH] FrSkyR9: initial telemetry support --- Multiprotocol/FrSkyDVX_common.ino | 2 +- Multiprotocol/FrSkyR9_sx1276.ino | 160 +++++++++++++++++++++--------- Multiprotocol/Multiprotocol.h | 2 +- Multiprotocol/Multiprotocol.ino | 8 +- Multiprotocol/SX1276_SPI.ino | 14 ++- Multiprotocol/Telemetry.ino | 88 ++++++++-------- Multiprotocol/Validate.h | 2 +- Protocols_Details.md | 10 +- 8 files changed, 186 insertions(+), 100 deletions(-) diff --git a/Multiprotocol/FrSkyDVX_common.ino b/Multiprotocol/FrSkyDVX_common.ino index f4c3234..775aa0f 100644 --- a/Multiprotocol/FrSkyDVX_common.ino +++ b/Multiprotocol/FrSkyDVX_common.ino @@ -33,7 +33,7 @@ static uint16_t __attribute__((unused)) FrSkyX_CRCTable(uint8_t val) val /= 16 ; return word ^ (0x1081 * val) ; } -uint16_t FrSkyX_crc(uint8_t *data, uint8_t len, uint8_t init=0) +uint16_t FrSkyX_crc(uint8_t *data, uint8_t len, uint16_t init=0) { uint16_t crc = init; for(uint8_t i=0; i < len; i++) diff --git a/Multiprotocol/FrSkyR9_sx1276.ino b/Multiprotocol/FrSkyR9_sx1276.ino index 93d4146..dc5a00a 100644 --- a/Multiprotocol/FrSkyR9_sx1276.ino +++ b/Multiprotocol/FrSkyR9_sx1276.ino @@ -6,6 +6,13 @@ uint8_t FrSkyR9_step = 1; uint32_t FrSkyR9_freq_map[FREQ_MAP_SIZE]; +enum { + FRSKYR9_FREQ=0, + FRSKYR9_DATA, + FRSKYR9_RX1, + FRSKYR9_RX2, +}; + static void __attribute__((unused)) FrSkyR9_build_freq() { uint32_t start_freq=914472960; // 915 @@ -55,6 +62,10 @@ static void __attribute__((unused)) FrSkyR9_build_packet() } //SPort + packet[20] = FrSkyX_RX_Seq << 4; + packet[20] |= FrSkyX_TX_Seq ; //TX=8 at startup + if ( !(FrSkyX_TX_IN_Seq & 0xF8) ) + FrSkyX_TX_Seq = ( FrSkyX_TX_Seq + 1 ) & 0x03 ; // Next iteration send next packet packet[20] = 0x08; //FrSkyX_TX_Seq=8 at startup packet[21] = 0x00; // length? packet[22] = 0x00; // data1? @@ -104,57 +115,116 @@ uint16_t initFrSkyR9() SX1276_SetPaDac(true); SX1276_SetTxRxMode(TX_EN); // Set RF switch to TX + FrSkyX_TX_Seq = 0x08 ; // Request init + FrSkyX_TX_IN_Seq = 0xFF ; // No sequence received yet + #ifdef SPORT_SEND + for(uint8_t i=0;i<4;i++) + FrSkyX_TX_Frames[i].count=0; // discard frames in current output buffer + SportHead=SportTail=0; // empty data buffer + #endif + FrSkyX_RX_Seq = 0 ; // Seq 0 to start with + + phase=FRSKYR9_FREQ; + return 20000; // start calling FrSkyR9_callback in 20 milliseconds } uint16_t FrSkyR9_callback() { static boolean bind=false; - if(bind && IS_BIND_DONE) - FrSkyR9_build_freq(); // 868 is binding at 915, need to switch back to 868 once bind is completed - bind=IS_BIND_IN_PROGRESS; - - //Force standby - SX1276_SetMode(true, false, SX1276_OPMODE_STDBY); - - //SX1276_WriteReg(SX1276_11_IRQFLAGSMASK, 0xbf); // use only RxDone interrupt - - // uint8_t buffer[2]; - // buffer[0] = 0x00; - // buffer[1] = 0x00; - // SX1276_WriteRegisterMulti(SX1276_40_DIOMAPPING1, buffer, 2); // RxDone interrupt mapped to DIO0 (the rest are not used because of the REG_IRQ_FLAGS_MASK) - - // SX1276_WriteReg(REG_PAYLOAD_LENGTH, 13); - - // SX1276_WriteReg(REG_FIFO_ADDR_PTR, 0x00); - - // SX1276_WriteReg(SX1276_01_OPMODE, 0x85); // RXCONTINUOUS - // delay(10); // 10 ms - - // SX1276_WriteReg(SX1276_01_OPMODE, 0x81); // STDBY - - // SX1276_WriteReg(SX1276_09_PACONFIG, 0xF0); - - //Set power - // max power: 15dBm (10.8 + 0.6 * MaxPower [dBm]) - // output_power: 2 dBm (17-(15-OutputPower) (if pa_boost_pin == true)) - SX1276_SetPaConfig(true, 7, 0); - - //Set frequency - hopping_frequency_no = (hopping_frequency_no + FrSkyR9_step) % FREQ_MAP_SIZE; - SX1276_SetFrequency(FrSkyR9_freq_map[hopping_frequency_no]); // set current center frequency - delayMicroseconds(500); //Frequency settle time - - //Build packet - FrSkyR9_build_packet(); - - //Send - SX1276_WritePayloadToFifo(packet, 26); - SX1276_SetMode(true, false, SX1276_OPMODE_TX); - - // need to clear RegIrqFlags? - - return 20000; + switch (phase) + { + case FRSKYR9_FREQ: + //Force standby + SX1276_SetMode(true, false, SX1276_OPMODE_STDBY); + //Set frequency + //868 is binding at 915, need to switch back to 868 once bind is completed + if(bind && IS_BIND_DONE) + FrSkyR9_build_freq(); + bind=IS_BIND_IN_PROGRESS; + hopping_frequency_no = (hopping_frequency_no + FrSkyR9_step) % FREQ_MAP_SIZE; + SX1276_SetFrequency(FrSkyR9_freq_map[hopping_frequency_no]); // set current center frequency + //Set power + // max power: 15dBm (10.8 + 0.6 * MaxPower [dBm]) + // output_power: 2 dBm (17-(15-OutputPower) (if pa_boost_pin == true)) + SX1276_SetPaConfig(true, 7, 0); + //Build packet + FrSkyR9_build_packet(); + phase++; + return 460; // Frequency settle time + case FRSKYR9_DATA: + //Set RF switch to TX + SX1276_SetTxRxMode(TX_EN); + //Send packet + SX1276_WritePayloadToFifo(packet, 26); + SX1276_SetMode(true, false, SX1276_OPMODE_TX); +#if not defined TELEMETRY + phase=FRSKYR9_FREQ; + return 20000-460; +#else + phase++; + return 11140; // Packet send time + case FRSKYR9_RX1: + //Force standby + SX1276_SetMode(true, false, SX1276_OPMODE_STDBY); + //RX packet size is 13 + SX1276_WriteReg(SX1276_22_PAYLOAD_LENGTH, 13); + //Reset pointer + SX1276_WriteReg(SX1276_0D_FIFOADDRPTR, 0x00); + //Set RF switch to RX + SX1276_SetTxRxMode(RX_EN); + //Switch to RX + SX1276_WriteReg(SX1276_01_OPMODE, 0x85); + phase++; + return 7400; + case FRSKYR9_RX2: + if( (SX1276_ReadReg(SX1276_12_REGIRQFLAGS)&0xF0) == (_BV(SX1276_REGIRQFLAGS_RXDONE) | _BV(SX1276_REGIRQFLAGS_VALIDHEADER)) ) + { + if(SX1276_ReadReg(SX1276_13_REGRXNBBYTES)==13) + { + SX1276_ReadRegisterMulti(SX1276_00_FIFO,packet_in,13); + if( packet_in[9]==rx_tx_addr[1] && packet_in[10]==rx_tx_addr[2] && FrSkyX_crc(packet_in, 11, rx_tx_addr[1]+(rx_tx_addr[2]<<8))==(packet_in[11]+(packet_in[12]<<8)) ) + { + if(packet_in[0]&0x80) + RX_RSSI=packet_in[0]<<1; + else + v_lipo1=(packet_in[0]<<1)+1; + TX_LQI=~(SX1276_ReadReg(SX1276_19_PACKETSNR)>>2)+1; + TX_RSSI=SX1276_ReadReg(SX1276_1A_PACKETRSSI)-157; + for(uint8_t i=0;i<9;i++) + packet[4+i]=packet_in[i]; //Adjust buffer to match FrSkyX + frsky_check_telemetry(packet,len); //Check and parse telemetry packets + } + } + } + if(!telemetry_link) + { + packet_count++; + //debugln("M %d",packet_count); + // restart sequence on missed packet - might need count or timeout instead of one missed + if(packet_count>100) + {//~1sec + FrSkyX_TX_Seq = 0x08 ; //Request init + FrSkyX_TX_IN_Seq = 0xFF ; //No sequence received yet + #ifdef SPORT_SEND + for(uint8_t i=0;i<4;i++) + FrSkyX_TX_Frames[i].count=0; //Discard frames in current output buffer + #endif + packet_count=0; + telemetry_lost=1; + telemetry_link=0; //Stop sending telemetry + } + CC2500_Strobe(CC2500_SFRX); //Flush the RXFIFO + } + if(!telemetry_lost) + telemetry_link=1; // Send telemetry out anyway + //Clear all flags + SX1276_WriteReg(SX1276_12_REGIRQFLAGS,0xFF); + phase=FRSKYR9_FREQ; + break; +#endif + } + return 1000; } #endif \ No newline at end of file diff --git a/Multiprotocol/Multiprotocol.h b/Multiprotocol/Multiprotocol.h index 869bdde..7a022fa 100644 --- a/Multiprotocol/Multiprotocol.h +++ b/Multiprotocol/Multiprotocol.h @@ -19,7 +19,7 @@ #define VERSION_MAJOR 1 #define VERSION_MINOR 3 #define VERSION_REVISION 1 -#define VERSION_PATCH_LEVEL 30 +#define VERSION_PATCH_LEVEL 31 //****************** // Protocols diff --git a/Multiprotocol/Multiprotocol.ino b/Multiprotocol/Multiprotocol.ino index cbfbc93..f27afe8 100644 --- a/Multiprotocol/Multiprotocol.ino +++ b/Multiprotocol/Multiprotocol.ino @@ -766,7 +766,7 @@ bool Update_All() update_led_status(); #if defined(TELEMETRY) #if ( !( defined(MULTI_TELEMETRY) || defined(MULTI_STATUS) ) ) - if((protocol == PROTO_BAYANG_RX) || (protocol == PROTO_AFHDS2A_RX) || (protocol == PROTO_FRSKY_RX) || (protocol == PROTO_SCANNER) || (protocol==PROTO_FRSKYD) || (protocol==PROTO_BAYANG) || (protocol==PROTO_NCC1701) || (protocol==PROTO_BUGS) || (protocol==PROTO_BUGSMINI) || (protocol==PROTO_HUBSAN) || (protocol==PROTO_AFHDS2A) || (protocol==PROTO_FRSKYX) || (protocol==PROTO_FRSKYX2) || (protocol==PROTO_DSM) || (protocol==PROTO_CABELL) || (protocol==PROTO_HITEC) || (protocol==PROTO_HOTT) || (protocol==PROTO_PROPEL) || (protocol==PROTO_DEVO) || (protocol==PROTO_DSM_RX)) + if((protocol == PROTO_BAYANG_RX) || (protocol == PROTO_AFHDS2A_RX) || (protocol == PROTO_FRSKY_RX) || (protocol == PROTO_SCANNER) || (protocol==PROTO_FRSKYD) || (protocol==PROTO_BAYANG) || (protocol==PROTO_NCC1701) || (protocol==PROTO_BUGS) || (protocol==PROTO_BUGSMINI) || (protocol==PROTO_HUBSAN) || (protocol==PROTO_AFHDS2A) || (protocol==PROTO_FRSKYX) || (protocol==PROTO_FRSKYX2) || (protocol==PROTO_DSM) || (protocol==PROTO_CABELL) || (protocol==PROTO_HITEC) || (protocol==PROTO_HOTT) || (protocol==PROTO_PROPEL) || (protocol==PROTO_DEVO) || (protocol==PROTO_DSM_RX) || (protocol==PROTO_FRSKY_R9)) #endif if(IS_DISABLE_TELEM_off) TelemetryUpdate(); @@ -782,8 +782,8 @@ bool Update_All() { // Autobind is on and BIND_CH went down BIND_CH_PREV_off; //Request protocol to terminate bind - #if defined(FRSKYD_CC2500_INO) || defined(FRSKYL_CC2500_INO) || defined(FRSKYX_CC2500_INO) || defined(FRSKYV_CC2500_INO) || defined(AFHDS2A_A7105_INO) - if(protocol==PROTO_FRSKYD || protocol==PROTO_FRSKYL || protocol==PROTO_FRSKYX || protocol==PROTO_FRSKYX2 || protocol==PROTO_FRSKYV || protocol==PROTO_AFHDS2A ) + #if defined(FRSKYD_CC2500_INO) || defined(FRSKYL_CC2500_INO) || defined(FRSKYX_CC2500_INO) || defined(FRSKYV_CC2500_INO) || defined(AFHDS2A_A7105_INO) || defined(FRSKYR9_SX1276_INO) + if(protocol==PROTO_FRSKYD || protocol==PROTO_FRSKYL || protocol==PROTO_FRSKYX || protocol==PROTO_FRSKYX2 || protocol==PROTO_FRSKYV || protocol==PROTO_AFHDS2A || protocol==PROTO_FRSKY_R9) BIND_DONE; else #endif @@ -1886,7 +1886,7 @@ void update_serial_data() } #endif #ifdef SPORT_SEND - if((protocol==PROTO_FRSKYX || protocol==PROTO_FRSKYX2) && rx_len==35) + if((protocol==PROTO_FRSKYX || protocol==PROTO_FRSKYX2 || PROTO_FRSKY_R9) && rx_len==35) {//Protocol waiting for 8 bytes #define BYTE_STUFF 0x7D #define STUFF_MASK 0x20 diff --git a/Multiprotocol/SX1276_SPI.ino b/Multiprotocol/SX1276_SPI.ino index 0e8e5d0..b0fbf2b 100644 --- a/Multiprotocol/SX1276_SPI.ino +++ b/Multiprotocol/SX1276_SPI.ino @@ -16,9 +16,8 @@ uint8_t SX1276_ReadReg(uint8_t address) { SPI_CSN_off; SPI_Write(address & 0x7F); - uint8_t result = SPI_Read(); + uint8_t result = SPI_Read(); SPI_CSN_on; - return result; } @@ -33,6 +32,17 @@ void SX1276_WriteRegisterMulti(uint8_t address, const uint8_t* data, uint8_t len SPI_CSN_on; } +void SX1276_ReadRegisterMulti(uint8_t address, uint8_t* data, uint8_t length) +{ + SPI_CSN_off; + SPI_Write(address & 0x7F); + + for(uint8_t i = 0; i < length; i++) + data[i]=SPI_Read(); + + SPI_CSN_on; +} + uint8_t SX1276_Reset() { //TODO when pin is not wired diff --git a/Multiprotocol/Telemetry.ino b/Multiprotocol/Telemetry.ino index 66381a5..22ca498 100644 --- a/Multiprotocol/Telemetry.ino +++ b/Multiprotocol/Telemetry.ino @@ -33,7 +33,6 @@ uint8_t RetrySequence ; #if defined SPORT_TELEMETRY #define FRSKY_SPORT_PACKET_SIZE 8 #define FX_BUFFERS 4 - uint8_t RxBt = 0; uint8_t Sport_Data = 0; uint8_t pktx1[FRSKY_SPORT_PACKET_SIZE*FX_BUFFERS]; @@ -348,26 +347,28 @@ void frskySendStuffed() Serial_write(START_STOP); } -void frsky_check_telemetry(uint8_t *packet_in,uint8_t len) +void frsky_check_telemetry(uint8_t *buffer,uint8_t len) { - if(packet_in[1] != rx_tx_addr[3] || packet_in[2] != rx_tx_addr[2] || len != packet_in[0] + 3 ) - return; // Bad address or length... - + if(protocol!=PROTO_FRSKY_R9) + { + if(buffer[1] != rx_tx_addr[3] || buffer[2] != rx_tx_addr[2] || len != buffer[0] + 3 ) + return; // Bad address or length... + // RSSI and LQI are the 2 last bytes + TX_RSSI = buffer[len-2]; + if(TX_RSSI >=128) + TX_RSSI -= 128; + else + TX_RSSI += 128; + TX_LQI = buffer[len-1]&0x7F; + } telemetry_link|=1; // Telemetry data is available - // RSSI and LQI are the 2 last bytes - TX_RSSI = packet_in[len-2]; - if(TX_RSSI >=128) - TX_RSSI -= 128; - else - TX_RSSI += 128; - TX_LQI = packet_in[len-1]&0x7F; - + #if defined FRSKYD_CC2500_INO if (protocol==PROTO_FRSKYD) { //Save current buffer for (uint8_t i=3;i0 && telemetry_in_buffer[6]<=10) @@ -392,7 +393,7 @@ void frsky_check_telemetry(uint8_t *packet_in,uint8_t len) } #endif -#if defined SPORT_TELEMETRY && defined FRSKYX_CC2500_INO +#if defined SPORT_TELEMETRY && (defined FRSKYX_CC2500_INO || defined FRSKYR9_SX1276_INO) if (protocol==PROTO_FRSKYX||protocol==PROTO_FRSKYX2) { /*Telemetry frames(RF) SPORT info @@ -413,29 +414,30 @@ void frsky_check_telemetry(uint8_t *packet_in,uint8_t len) [12] STRM6 D1 D1 D0 D0 [13] CHKSUM1 --|2 CRC bytes sent by RX (calculated on RX side crc16/table) [14] CHKSUM2 --|*/ - telemetry_lost=0; - - uint16_t lcrc = FrSkyX_crc(&packet_in[3], len-7 ) ; - if ( ( (lcrc >> 8) != packet_in[len-4]) || ( (lcrc & 0x00FF ) != packet_in[len-3]) ) + //len=17 -> len-7=10 -> 3..12 + uint16_t lcrc = FrSkyX_crc(&buffer[3], len-7 ) ; + if ( ( (lcrc >> 8) != buffer[len-4]) || ( (lcrc & 0x00FF ) != buffer[len-3]) ) return; // Bad CRC - if(packet_in[4] & 0x80) - RX_RSSI=packet_in[4] & 0x7F ; + if(buffer[4] & 0x80) + RX_RSSI=buffer[4] & 0x7F ; else - RxBt = (packet_in[4]<<1) + 1 ; + v_lipo1 = (buffer[4]<<1) + 1 ; #if defined(TELEMETRY_FRSKYX_TO_FRSKYD) && defined(ENABLE_PPM) if(mode_select != MODE_SERIAL) - {//PPM - v_lipo1=RxBt; return; - } #endif + } + if (protocol==PROTO_FRSKYX||protocol==PROTO_FRSKYX2||protocol==PROTO_FRSKY_R9) + { + telemetry_lost=0; + //Save outgoing telemetry sequence - FrSkyX_TX_IN_Seq=packet_in[5] >> 4; + FrSkyX_TX_IN_Seq=buffer[5] >> 4; //Check incoming telemetry sequence - uint8_t packet_seq=packet_in[5] & 0x03; - if ( packet_in[5] & 0x08 ) + uint8_t packet_seq=buffer[5] & 0x03; + if ( buffer[5] & 0x08 ) {//Request init FrSkyX_RX_Seq = 0x08 ; FrSkyX_RX_NextFrame = 0x00 ; @@ -448,17 +450,17 @@ void frsky_check_telemetry(uint8_t *packet_in,uint8_t len) {//In sequence struct t_FrSkyX_RX_Frame *p ; uint8_t count ; - // packet_in[4] RSSI - // packet_in[5] sequence control - // packet_in[6] payload count - // packet_in[7-12] payload + // buffer[4] RSSI + // buffer[5] sequence control + // buffer[6] payload count + // buffer[7-12] payload p = &FrSkyX_RX_Frames[packet_seq] ; - count = packet_in[6]; // Payload length + count = buffer[6]; // Payload length if ( count <= 6 ) {//Store payload p->count = count ; for ( uint8_t i = 0 ; i < count ; i++ ) - p->payload[i] = packet_in[i+7] ; + p->payload[i] = buffer[i+7] ; } else p->count = 0 ; // Discard @@ -477,19 +479,19 @@ void frsky_check_telemetry(uint8_t *packet_in,uint8_t len) {//Not in sequence struct t_FrSkyX_RX_Frame *q ; uint8_t count ; - // packet_in[4] RSSI - // packet_in[5] sequence control - // packet_in[6] payload count - // packet_in[7-12] payload + // buffer[4] RSSI + // buffer[5] sequence control + // buffer[6] payload count + // buffer[7-12] payload if ( packet_seq == ( ( FrSkyX_RX_Seq +1 ) & 3 ) ) {//Received next sequence -> save it q = &FrSkyX_RX_Frames[packet_seq] ; - count = packet_in[6]; // Payload length + count = buffer[6]; // Payload length if ( count <= 6 ) {//Store payload q->count = count ; for ( uint8_t i = 0 ; i < count ; i++ ) - q->payload[i] = packet_in[i+7] ; + q->payload[i] = buffer[i+7] ; } else q->count = 0 ; @@ -725,7 +727,7 @@ void sportSendFrame() case 0: frame[2] = 0x05; frame[3] = 0xf1; - frame[4] = 0x02 ;//dummy values if swr 20230f00 + frame[4] = 0x02; //dummy values if swr 20230f00 frame[5] = 0x23; frame[6] = 0x0F; break; @@ -740,7 +742,7 @@ void sportSendFrame() case 4: //BATT frame[2] = 0x04; frame[3] = 0xf1; - frame[4] = RxBt;//a1; + frame[4] = v_lipo1; //a1; break; default: if(Sport_Data) @@ -870,7 +872,7 @@ void TelemetryUpdate() #endif #endif #if defined SPORT_TELEMETRY - if ((protocol==PROTO_FRSKYX || protocol==PROTO_FRSKYX2) && telemetry_link + if ((protocol==PROTO_FRSKYX || protocol==PROTO_FRSKYX2||protocol==PROTO_FRSKY_R9) && telemetry_link #ifdef TELEMETRY_FRSKYX_TO_FRSKYD && mode_select==MODE_SERIAL #endif diff --git a/Multiprotocol/Validate.h b/Multiprotocol/Validate.h index 27efe68..07a0177 100644 --- a/Multiprotocol/Validate.h +++ b/Multiprotocol/Validate.h @@ -375,7 +375,7 @@ #if not defined(FRSKYD_CC2500_INO) #undef HUB_TELEMETRY #endif - #if not defined(FRSKYX_CC2500_INO) + #if not defined(FRSKYX_CC2500_INO) && not defined(FRSKYR9_SX1276_INO) #undef SPORT_TELEMETRY #undef SPORT_SEND #endif diff --git a/Protocols_Details.md b/Protocols_Details.md index f3fba0b..442b838 100644 --- a/Protocols_Details.md +++ b/Protocols_Details.md @@ -1543,11 +1543,15 @@ CH1|CH2|CH3|CH4|CH5 # SX1276 RF Module ## FRSKYR9 - *65* -Extended limits and failsafe supported. - **R9 RXs must be flashed with ACCST Flex.** -Telemetry and power adjustment not yet supported. +Extended limits and failsafe supported. + +Full telemetry from RX to TX is supported (not yet from TX to RX). + +Notes: +- The choices of CH1-8/CH9-16 and Telem ON/OFF will be available in OpenTX 2.3.10+. The default is CH1-8 Telem ON. +- Power adjustment is not supported on the T18. ### Sub_protocol R9_915 - *0* 915MHz, 16 channels