From bd78739217338d1016c4220785625d535c8febe5 Mon Sep 17 00:00:00 2001 From: Pascal Langer Date: Tue, 20 Nov 2018 21:04:29 +0100 Subject: [PATCH] AFHDS2A: LQI to RX channel --- Multiprotocol/AFHDS2A_a7105.ino | 119 ++++++++++++++++++-------------- Multiprotocol/Multiprotocol.h | 2 +- Multiprotocol/Validate.h | 9 +++ Multiprotocol/_Config.h | 32 +++++---- 4 files changed, 94 insertions(+), 68 deletions(-) diff --git a/Multiprotocol/AFHDS2A_a7105.ino b/Multiprotocol/AFHDS2A_a7105.ino index 407045b..46a3838 100644 --- a/Multiprotocol/AFHDS2A_a7105.ino +++ b/Multiprotocol/AFHDS2A_a7105.ino @@ -86,50 +86,55 @@ enum{ static void AFHDS2A_update_telemetry() { + // Read TX RSSI + int16_t temp=256-(A7105_ReadReg(A7105_1D_RSSI_THOLD)*8)/5; // value from A7105 is between 8 for maximum signal strength to 160 or less + if(temp<0) temp=0; + else if(temp>255) temp=255; + TX_RSSI=temp; // AA | TXID | rx_id | sensor id | sensor # | value 16 bit big endian | sensor id ...... // max 7 sensors per packet -#ifdef AFHDS2A_FW_TELEMETRY - if (option & 0x80) - { - // forward telemetry to TX, skip rx and tx id to save space - pkt[0]= TX_RSSI; - for(int i=9;i < AFHDS2A_RXPACKET_SIZE; i++) - pkt[i-8]=packet[i]; - - telemetry_link=2; - return; - } -#endif -#ifdef AFHDS2A_HUB_TELEMETRY - for(uint8_t sensor=0; sensor<7; sensor++) - { - // Send FrSkyD telemetry to TX - uint8_t index = 9+(4*sensor); - switch(packet[index]) + #ifdef AFHDS2A_FW_TELEMETRY + if (option & 0x80) { - case AFHDS2A_SENSOR_RX_VOLTAGE: - //v_lipo1 = packet[index+3]<<8 | packet[index+2]; - v_lipo1 = packet[index+2]; - telemetry_link=1; - break; - case AFHDS2A_SENSOR_A3_VOLTAGE: - v_lipo2 = (packet[index+3]<<5) | (packet[index+2]>>3); // allows to read voltage up to 4S - telemetry_link=1; - break; - case AFHDS2A_SENSOR_RX_ERR_RATE: - RX_LQI=packet[index+2]; - break; - case AFHDS2A_SENSOR_RX_RSSI: - RX_RSSI = -packet[index+2]; - break; - case 0xff: - return; - /*default: - // unknown sensor ID - break;*/ + // forward telemetry to TX, skip rx and tx id to save space + pkt[0]= TX_RSSI; + for(int i=9;i < AFHDS2A_RXPACKET_SIZE; i++) + pkt[i-8]=packet[i]; + + telemetry_link=2; + return; } - } -#endif + #endif + #ifdef AFHDS2A_HUB_TELEMETRY + for(uint8_t sensor=0; sensor<7; sensor++) + { + // Send FrSkyD telemetry to TX + uint8_t index = 9+(4*sensor); + switch(packet[index]) + { + case AFHDS2A_SENSOR_RX_VOLTAGE: + //v_lipo1 = packet[index+3]<<8 | packet[index+2]; + v_lipo1 = packet[index+2]; + telemetry_link=1; + break; + case AFHDS2A_SENSOR_A3_VOLTAGE: + v_lipo2 = (packet[index+3]<<5) | (packet[index+2]>>3); // allows to read voltage up to 4S + telemetry_link=1; + break; + case AFHDS2A_SENSOR_RX_ERR_RATE: + RX_LQI=packet[index+2]; + break; + case AFHDS2A_SENSOR_RX_RSSI: + RX_RSSI = -packet[index+2]; + break; + case 0xff: + return; + /*default: + // unknown sensor ID + break;*/ + } + } + #endif } #endif @@ -169,6 +174,7 @@ static void AFHDS2A_build_bind_packet() static void AFHDS2A_build_packet(uint8_t type) { + uint16_t val; memcpy( &packet[1], rx_tx_addr, 4); memcpy( &packet[5], rx_id, 4); switch(type) @@ -181,6 +187,12 @@ static void AFHDS2A_build_packet(uint8_t type) packet[9 + ch*2] = channelMicros&0xFF; packet[10 + ch*2] = (channelMicros>>8)&0xFF; } + #ifdef AFHDS2A_LQI_CH + // override channel with LQI + val = 2000 - 10*RX_LQI; + packet[9+((AFHDS2A_LQI_CH-1)*2)] = val & 0xff; + packet[10+((AFHDS2A_LQI_CH-1)*2)] = (val >> 8) & 0xff; + #endif break; case AFHDS2A_PACKET_FAILSAFE: packet[0] = 0x56; @@ -206,10 +218,10 @@ static void AFHDS2A_build_packet(uint8_t type) packet[0] = 0xaa; packet[9] = 0xfd; packet[10]= 0xff; - uint16_t val_hz=5*(option & 0x7f)+50; // option value should be between 0 and 70 which gives a value between 50 and 400Hz - if(val_hz<50 || val_hz>400) val_hz=50; // default is 50Hz - packet[11]= val_hz; - packet[12]= val_hz >> 8; + val=5*(option & 0x7f)+50; // option value should be between 0 and 70 which gives a value between 50 and 400Hz + if(val<50 || val>400) val=50; // default is 50Hz + packet[11]= val; + packet[12]= val >> 8; if(sub_protocol == PPM_IBUS || sub_protocol == PPM_SBUS) packet[13] = 0x01; // PPM output enabled else @@ -324,17 +336,20 @@ uint16_t ReadAFHDS2A() { if(packet[9] == 0xfc) packet_type=AFHDS2A_PACKET_SETTINGS; // RX is asking for settings - #if defined(AFHDS2A_FW_TELEMETRY) || defined(AFHDS2A_HUB_TELEMETRY) else { - // Read TX RSSI - int16_t temp=256-(A7105_ReadReg(A7105_1D_RSSI_THOLD)*8)/5; // value from A7105 is between 8 for maximum signal strength to 160 or less - if(temp<0) temp=0; - else if(temp>255) temp=255; - TX_RSSI=temp; - AFHDS2A_update_telemetry(); + #ifdef AFHDS2A_LQI_CH + for(uint8_t sensor=0; sensor<7; sensor++) + {//read LQI value for RX output + uint8_t index = 9+(4*sensor); + if(packet[index]==AFHDS2A_SENSOR_RX_ERR_RATE) + RX_LQI=packet[index+2]; + } + #endif + #if defined(AFHDS2A_FW_TELEMETRY) || defined(AFHDS2A_HUB_TELEMETRY) + AFHDS2A_update_telemetry(); + #endif } - #endif } } packet_counter++; diff --git a/Multiprotocol/Multiprotocol.h b/Multiprotocol/Multiprotocol.h index 91c1f0b..ee4a7e0 100644 --- a/Multiprotocol/Multiprotocol.h +++ b/Multiprotocol/Multiprotocol.h @@ -19,7 +19,7 @@ #define VERSION_MAJOR 1 #define VERSION_MINOR 2 #define VERSION_REVISION 1 -#define VERSION_PATCH_LEVEL 17 +#define VERSION_PATCH_LEVEL 18 //****************** // Protocols diff --git a/Multiprotocol/Validate.h b/Multiprotocol/Validate.h index 55e026a..2dfc763 100644 --- a/Multiprotocol/Validate.h +++ b/Multiprotocol/Validate.h @@ -282,6 +282,15 @@ #endif #endif +#if defined(AFHDS2A_LQI_CH) + #if AFHDS2A_LQI_CH<4 + #error AFHDS2A_LQI_CH must be above 4. + #endif + #if AFHDS2A_LQI_CH>14 + #error AFHDS2A_LQI_CH must be below or equal to 14. + #endif +#endif + #if MIN_PPM_CHANNELS>16 #error MIN_PPM_CHANNELS must be below or equal to 16. The default for this value is 4. #endif diff --git a/Multiprotocol/_Config.h b/Multiprotocol/_Config.h index ba90828..ab7893e 100644 --- a/Multiprotocol/_Config.h +++ b/Multiprotocol/_Config.h @@ -213,7 +213,9 @@ //AFHDS2A specific settings //------------------------- -//TODO: make LQI available on RX channel +//When enabled (remove the "//"), the below setting makes LQI (Link Quality Indicator) available on one of the RX ouput channel (5-14). +//#define AFHDS2A_LQI_CH 14 + /**************************/ /*** FAILSAFE SETTINGS ***/ @@ -435,6 +437,20 @@ const PPM_Parameters PPM_prot[14*NBR_BANKS]= { /* 14 */ {PROTO_MT99XX, MT99 , 0 , P_HIGH , NO_AUTOBIND , 0 }, #endif }; +// RX_Num is used for TX & RX match. Using different RX_Num values for each receiver will prevent starting a model with the false config loaded... +// RX_Num value is between 0 and 15. + +// Power P_HIGH or P_LOW: High or low power setting for the transmission. +// For indoor P_LOW is more than enough. + +// Auto Bind AUTOBIND or NO_AUTOBIND +// For protocols which does not require binding at each power up (like Flysky, FrSky...), you might still want a bind to be initiated each time you power up the TX. +// As an example, it's usefull for the WLTOYS F929/F939/F949/F959 (all using the Flysky protocol) which requires a bind at each power up. +// It also enables the Bind from channel feature, allowing to execute a bind by toggling a designated channel. + +// Option: the value is between -128 and +127. +// The option value is only valid for some protocols, read this page for more information: https://github.com/pascallanger/DIY-Multiprotocol-TX-Module/blob/master/Protocols_Details.md + /* Available protocols and associated sub protocols to pick and choose from PROTO_FLYSKY Flysky @@ -603,17 +619,3 @@ const PPM_Parameters PPM_prot[14*NBR_BANKS]= { PROTO_GD00X NONE */ - -// RX_Num is used for TX & RX match. Using different RX_Num values for each receiver will prevent starting a model with the false config loaded... -// RX_Num value is between 0 and 15. - -// Power P_HIGH or P_LOW: High or low power setting for the transmission. -// For indoor P_LOW is more than enough. - -// Auto Bind AUTOBIND or NO_AUTOBIND -// For protocols which does not require binding at each power up (like Flysky, FrSky...), you might still want a bind to be initiated each time you power up the TX. -// As an example, it's usefull for the WLTOYS F929/F939/F949/F959 (all using the Flysky protocol) which requires a bind at each power up. -// It also enables the Bind from channel feature, allowing to execute a bind by toggling a designated channel. - -// Option: the value is between -128 and +127. -// The option value is only valid for some protocols, read this page for more information: https://github.com/pascallanger/DIY-Multiprotocol-TX-Module/blob/master/Protocols_Details.md