From 41d75da33fa327c894df525571fa52eabf0a544b Mon Sep 17 00:00:00 2001 From: pascallanger Date: Wed, 1 Feb 2017 09:28:35 +0100 Subject: [PATCH] FrSkyD telemetry - rewritten the handling of the incoming over the air telemetry packet - rewritten the TX_RSSI calculation (link frame[4]) - added RX LQI and TX_LQI information in the link frame (frame[5] and frame[6]) --- Multiprotocol/AFHDS2A_a7105.ino | 10 +-- Multiprotocol/Bayang_nrf24l01.ino | 4 +- Multiprotocol/FrSkyD_cc2500.ino | 1 + Multiprotocol/Hubsan_a7105.ino | 2 +- Multiprotocol/Multiprotocol.h | 2 +- Multiprotocol/Multiprotocol.ino | 4 +- Multiprotocol/Telemetry.ino | 136 ++++++++++++------------------ 7 files changed, 68 insertions(+), 91 deletions(-) diff --git a/Multiprotocol/AFHDS2A_a7105.ino b/Multiprotocol/AFHDS2A_a7105.ino index 709f326..7de1da0 100644 --- a/Multiprotocol/AFHDS2A_a7105.ino +++ b/Multiprotocol/AFHDS2A_a7105.ino @@ -110,11 +110,11 @@ static void AFHDS2A_update_telemetry() v_lipo1 = packet[index+2]; telemetry_link=1; break; - /*case AFHDS2A_SENSOR_RX_ERR_RATE: - // packet[index+2]; - break;*/ + case AFHDS2A_SENSOR_RX_ERR_RATE: + RX_LQI=packet[index+2]; + break; case AFHDS2A_SENSOR_RX_RSSI: - RSSI_dBm = -packet[index+2]; + RX_RSSI = -packet[index+2]; break; case 0xff: return; @@ -352,7 +352,7 @@ uint16_t initAFHDS2A() } hopping_frequency_no = 0; #if defined(AFHDS2A_FW_TELEMETRY) || defined(AFHDS2A_HUB_TELEMETRY) - init_hub_telemetry(); + init_frskyd_link_telemetry(); #endif return 50000; } diff --git a/Multiprotocol/Bayang_nrf24l01.ino b/Multiprotocol/Bayang_nrf24l01.ino index 8ba3fff..00acd97 100644 --- a/Multiprotocol/Bayang_nrf24l01.ino +++ b/Multiprotocol/Bayang_nrf24l01.ino @@ -150,7 +150,7 @@ static void __attribute__((unused)) check_rx(void) // compensated battery volts*100/2 v_lipo2 = (packet[5]<<7) + (packet[6]>>2); // reception in packets / sec - RSSI_dBm = packet[7]; + RX_RSSI = packet[7]; //Flags //uint8_t flags = packet[3] >> 3; // battery low: flags & 1 @@ -252,7 +252,7 @@ uint16_t initBAYANG(void) BAYANG_init(); packet_count=0; #ifdef BAYANG_HUB_TELEMETRY - init_hub_telemetry(); + init_frskyd_link_telemetry(); telemetry_lost=1; // do not send telemetry to TX right away until we have a TX_RSSI value to prevent warning message... #endif return BAYANG_INITIAL_WAIT+BAYANG_PACKET_PERIOD; diff --git a/Multiprotocol/FrSkyD_cc2500.ino b/Multiprotocol/FrSkyD_cc2500.ino index 5c8bb96..8abe463 100644 --- a/Multiprotocol/FrSkyD_cc2500.ino +++ b/Multiprotocol/FrSkyD_cc2500.ino @@ -97,6 +97,7 @@ uint16_t initFrSky_2way() { Frsky_init_hop(); packet_count=0; + init_frskyd_link_telemetry(); if(IS_AUTOBIND_FLAG_on) { frsky2way_init(1); diff --git a/Multiprotocol/Hubsan_a7105.ino b/Multiprotocol/Hubsan_a7105.ino index f8e4ed5..755e817 100644 --- a/Multiprotocol/Hubsan_a7105.ino +++ b/Multiprotocol/Hubsan_a7105.ino @@ -352,7 +352,7 @@ uint16_t initHubsan() { packet_count=0; id_data=ID_NORMAL; #ifdef HUBSAN_HUB_TELEMETRY - init_hub_telemetry(); + init_frskyd_link_telemetry(); #endif return 10000; } diff --git a/Multiprotocol/Multiprotocol.h b/Multiprotocol/Multiprotocol.h index 1a0d523..b75bff5 100644 --- a/Multiprotocol/Multiprotocol.h +++ b/Multiprotocol/Multiprotocol.h @@ -19,7 +19,7 @@ #define VERSION_MAJOR 1 #define VERSION_MINOR 1 #define VERSION_REVISION 6 -#define VERSION_PATCH_LEVEL 8 +#define VERSION_PATCH_LEVEL 9 //****************** // Protocols //****************** diff --git a/Multiprotocol/Multiprotocol.ino b/Multiprotocol/Multiprotocol.ino index 9f44658..ac48735 100644 --- a/Multiprotocol/Multiprotocol.ino +++ b/Multiprotocol/Multiprotocol.ino @@ -148,8 +148,10 @@ uint8_t pkt[MAX_PKT];//telemetry receiving packets #endif // BASH_SERIAL uint8_t v_lipo1; uint8_t v_lipo2; - int16_t RSSI_dBm; + uint8_t RX_RSSI; uint8_t TX_RSSI; + uint8_t RX_LQI; + uint8_t TX_LQI; uint8_t telemetry_link=0; uint8_t telemetry_counter=0; uint8_t telemetry_lost; diff --git a/Multiprotocol/Telemetry.ino b/Multiprotocol/Telemetry.ino index 8789ffb..08ee462 100644 --- a/Multiprotocol/Telemetry.ino +++ b/Multiprotocol/Telemetry.ino @@ -28,7 +28,6 @@ uint32_t last = 0; uint8_t sport_counter=0; uint8_t RxBt = 0; - uint8_t rssi; uint8_t sport = 0; #endif #if defined HUB_TELEMETRY @@ -159,25 +158,25 @@ void frskySendStuffed() Serial_write(START_STOP); } -void compute_RSSIdbm() -{ - - RSSI_dBm = (((uint16_t)(pktt[len-2])*18)>>4); - if(pktt[len-2] >=128) - RSSI_dBm -= 164; - else - RSSI_dBm += 130; -} - void frsky_check_telemetry(uint8_t *pkt,uint8_t len) { if(pkt[1] == rx_tx_addr[3] && pkt[2] == rx_tx_addr[2] && len ==(pkt[0] + 3)) { - for (uint8_t i=3;i0 && pktt[6]<=10) //increment only for valid hub frames + telemetry_link=1; // telemetry data is available + /*preious version + RSSI_dBm = (((uint16_t)(pktt[len-2])*18)>>4); + if(pktt[len-2] >=128) RSSI_dBm -= 164; + else RSSI_dBm += 130;*/ + TX_RSSI = pkt[len-2]; + if(TX_RSSI >=128) + TX_RSSI -= 128; + else + TX_RSSI += 128; + TX_LQI = pkt[len-1]&0x7F; + if(pkt[6]>0 && pkt[6]<=10) // increment only for valid size hub frames telemetry_counter=(telemetry_counter+1)%32; + for (uint8_t i=3;i0 && pktt[6]<=10) - {//only valid hub frames - frame[0] = 0xFD; - frame[2] = pktt[7]; - switch(pass) + {//only send valid hub frames + frame[0] = 0xFD; // user frame + if(pktt[6]>USER_MAX_BYTES) { - case 0: - indexx=pktt[6]; - for(i=0;i 1 && protocol == MODE_FRSKYD) + { // FrSkyD frsky_user_frame(); return; }