From dfd8d0fa3d3d763c8b212a3098678447cb992836 Mon Sep 17 00:00:00 2001 From: pascallanger Date: Tue, 6 Dec 2016 22:30:48 +0100 Subject: [PATCH] Added: Bayang telemetry Changed telemetry configuration and validation for AFHDS2A and HUBSAN Added default Bayang telemetry from Silverxxx: - Option=1 to activate telemetry (option=0 -> standard Bayang protocol) - Value returned to the TX: RX RSSI, TX RSSI, A1=uncompensated battery voltage, A2=compensated battery voltage --- Multiprotocol/AFHDS2A_a7105.ino | 20 ++++-- Multiprotocol/Bayang_nrf24l01.ino | 110 +++++++++++++++++++++++++++--- Multiprotocol/CC2500_SPI.ino | 2 +- Multiprotocol/Hubsan_a7105.ino | 14 ++-- Multiprotocol/Multiprotocol.ino | 7 +- Multiprotocol/Telemetry.ino | 36 ++++++---- Multiprotocol/Validate.h | 28 +++++--- Multiprotocol/_Config.h | 19 ++++-- 8 files changed, 179 insertions(+), 57 deletions(-) diff --git a/Multiprotocol/AFHDS2A_a7105.ino b/Multiprotocol/AFHDS2A_a7105.ino index 869b5a8..5fc8963 100644 --- a/Multiprotocol/AFHDS2A_a7105.ino +++ b/Multiprotocol/AFHDS2A_a7105.ino @@ -72,7 +72,7 @@ static void AFHDS2A_calc_channels() } } -#if defined(TELEMETRY) +#if defined(AFHDS2A_FW_TELEMETRY) || defined(AFHDS2A_HUB_TELEMETRY) // telemetry sensors ID enum{ AFHDS2A_SENSOR_RX_VOLTAGE = 0x00, @@ -86,8 +86,9 @@ static void AFHDS2A_update_telemetry() { // AA | TXID | rx_id | sensor id | sensor # | value 16 bit big endian | sensor id ...... // max 7 sensors per packet -#if defined AFHDS2A_TELEMETRY - if (option & 0x80) { +#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++) @@ -96,14 +97,17 @@ static void AFHDS2A_update_telemetry() 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_lipo = packet[index+3]<<8 | packet[index+2]; + //v_lipo1 = packet[index+3]<<8 | packet[index+2]; + v_lipo1 = packet[index+2]; telemetry_link=1; break; /*case AFHDS2A_SENSOR_RX_ERR_RATE: @@ -119,6 +123,7 @@ static void AFHDS2A_update_telemetry() break;*/ } } +#endif } #endif @@ -298,7 +303,7 @@ uint16_t ReadAFHDS2A() { if(packet[9] == 0xfc) packet_type=AFHDS2A_PACKET_SETTINGS; // RX is asking for settings - #if defined(TELEMETRY) + #if defined(AFHDS2A_FW_TELEMETRY) || defined(AFHDS2A_HUB_TELEMETRY) else { // Read TX RSSI @@ -346,6 +351,9 @@ uint16_t initAFHDS2A() rx_id[i]=eeprom_read_byte((EE_ADDR)(temp+i)); } hopping_frequency_no = 0; +#if defined(AFHDS2A_FW_TELEMETRY) || defined(AFHDS2A_HUB_TELEMETRY) + init_hub_telemetry(); +#endif return 50000; } #endif diff --git a/Multiprotocol/Bayang_nrf24l01.ino b/Multiprotocol/Bayang_nrf24l01.ino index 3a5bbe4..6d089f1 100644 --- a/Multiprotocol/Bayang_nrf24l01.ino +++ b/Multiprotocol/Bayang_nrf24l01.ino @@ -20,7 +20,7 @@ Multiprotocol is distributed in the hope that it will be useful, #include "iface_nrf24l01.h" #define BAYANG_BIND_COUNT 1000 -#define BAYANG_PACKET_PERIOD 2000 +#define BAYANG_PACKET_PERIOD 1000 #define BAYANG_INITIAL_WAIT 500 #define BAYANG_PACKET_SIZE 15 #define BAYANG_RF_NUM_CHANNELS 4 @@ -43,7 +43,12 @@ static void __attribute__((unused)) BAYANG_send_packet(uint8_t bind) uint8_t i; if (bind) { - packet[0]= 0xA4; + #ifdef BAYANG_HUB_TELEMETRY + if(option) + packet[0]= 0xA3; // telemetry is enabled + else + #endif + packet[0]= 0xA4; for(i=0;i<5;i++) packet[i+1]=rx_tx_addr[i]; for(i=0;i<4;i++) @@ -97,21 +102,65 @@ static void __attribute__((unused)) BAYANG_send_packet(uint8_t bind) for (uint8_t i=0; i < BAYANG_PACKET_SIZE-1; i++) packet[14] += packet[i]; + NRF24L01_WriteReg(NRF24L01_05_RF_CH, bind ? BAYANG_RF_BIND_CHANNEL:hopping_frequency[hopping_frequency_no++]); + hopping_frequency_no%=BAYANG_RF_NUM_CHANNELS; + + // clear packet status bits and TX FIFO + NRF24L01_WriteReg(NRF24L01_07_STATUS, 0x70); + NRF24L01_FlushTx(); + + XN297_WritePayload(packet, BAYANG_PACKET_SIZE); + + NRF24L01_SetTxRxMode(TXRX_OFF); + NRF24L01_SetTxRxMode(TX_EN); + // Power on, TX mode, 2byte CRC // Why CRC0? xn297 does not interpret it - either 16-bit CRC or nothing XN297_Configure(_BV(NRF24L01_00_EN_CRC) | _BV(NRF24L01_00_CRCO) | _BV(NRF24L01_00_PWR_UP)); - NRF24L01_WriteReg(NRF24L01_05_RF_CH, bind ? BAYANG_RF_BIND_CHANNEL:hopping_frequency[hopping_frequency_no++]); - hopping_frequency_no%=BAYANG_RF_NUM_CHANNELS; - - // clear packet status bits and TX FIFO - NRF24L01_WriteReg(NRF24L01_07_STATUS, 0x70); - NRF24L01_FlushTx(); - XN297_WritePayload(packet, BAYANG_PACKET_SIZE); + #ifdef BAYANG_HUB_TELEMETRY + if (option) + { // switch radio to rx as soon as packet is sent + + while (!(NRF24L01_ReadReg(NRF24L01_07_STATUS) & _BV(NRF24L01_07_TX_DS))); + NRF24L01_WriteReg(NRF24L01_00_CONFIG, 0x03); + } + #endif NRF24L01_SetPower(); // Set tx_power } +#ifdef BAYANG_HUB_TELEMETRY +static void __attribute__((unused)) check_rx(void) +{ + if (NRF24L01_ReadReg(NRF24L01_07_STATUS) & _BV(NRF24L01_07_RX_DR)) + { // data received from model + XN297_ReadPayload(packet, BAYANG_PACKET_SIZE); + NRF24L01_WriteReg(NRF24L01_07_STATUS, 255); + + NRF24L01_FlushRx(); + uint8_t check = packet[0]; + for (uint8_t i=1; i < BAYANG_PACKET_SIZE-1; i++) + check += packet[i]; + // decode data , check sum is ok as well, since there is no crc + if (packet[0] == 0x85 && packet[14] == check) + { + // uncompensated battery volts*100/2 + v_lipo1 = (packet[3]<<7) + (packet[4]>>2); + // compensated battery volts*100/2 + v_lipo2 = (packet[5]<<7) + (packet[6]>>2); + // reception in packets / sec + RSSI_dBm = packet[7]; + //Flags + //uint8_t flags = packet[3] >> 3; + // battery low: flags & 1 + telemetry_counter++; + telemetry_link=1; + } + } +} +#endif + static void __attribute__((unused)) BAYANG_init() { NRF24L01_Initialize(); @@ -124,24 +173,59 @@ static void __attribute__((unused)) BAYANG_init() NRF24L01_WriteReg(NRF24L01_07_STATUS, 0x70); // Clear data ready, data sent, and retransmit NRF24L01_WriteReg(NRF24L01_01_EN_AA, 0x00); // No Auto Acknowldgement on all data pipes NRF24L01_WriteReg(NRF24L01_02_EN_RXADDR, 0x01); // Enable data pipe 0 only + NRF24L01_WriteReg(NRF24L01_11_RX_PW_P0, BAYANG_PACKET_SIZE); NRF24L01_SetBitrate(NRF24L01_BR_1M); // 1Mbps NRF24L01_SetPower(); + NRF24L01_Activate(0x73); // Activate feature register + NRF24L01_WriteReg(NRF24L01_1C_DYNPD, 0x00); // Disable dynamic payload length on all pipes + NRF24L01_WriteReg(NRF24L01_1D_FEATURE, 0x01); + NRF24L01_Activate(0x73); } uint16_t BAYANG_callback() { if(IS_BIND_DONE_on) - BAYANG_send_packet(0); + { + if(packet_count==0) + BAYANG_send_packet(0); + packet_count++; + #ifdef BAYANG_HUB_TELEMETRY + if (option) + { // telemetry is enabled + state++; + if (state > 1000) + { + //calculate telemetry reception packet rate - packets per 1000ms + TX_RSSI = telemetry_counter; + telemetry_counter = 0; + state = 0; + } + + if (packet_count > 1) + check_rx(); + + packet_count %= 5; + } + else + #endif + packet_count%=2; + } else { if (bind_counter == 0) { XN297_SetTXAddr(rx_tx_addr, BAYANG_ADDRESS_LENGTH); + #ifdef BAYANG_HUB_TELEMETRY + XN297_SetRXAddr(rx_tx_addr, BAYANG_ADDRESS_LENGTH); + #endif BIND_DONE; } else { - BAYANG_send_packet(1); + if(packet_count==0) + BAYANG_send_packet(1); + packet_count++; + packet_count%=4; bind_counter--; } } @@ -164,6 +248,10 @@ uint16_t initBAYANG(void) bind_counter = BAYANG_BIND_COUNT; BAYANG_initialize_txid(); BAYANG_init(); + packet_count=0; +#ifdef BAYANG_HUB_TELEMETRY + init_hub_telemetry(); +#endif return BAYANG_INITIAL_WAIT+BAYANG_PACKET_PERIOD; } diff --git a/Multiprotocol/CC2500_SPI.ino b/Multiprotocol/CC2500_SPI.ino index ac39670..8e17b46 100644 --- a/Multiprotocol/CC2500_SPI.ino +++ b/Multiprotocol/CC2500_SPI.ino @@ -86,8 +86,8 @@ void CC2500_SetTxRxMode(uint8_t mode) { if(mode == TX_EN) {//from deviation firmware - CC2500_WriteReg(CC2500_02_IOCFG0, 0x2F | 0x40); CC2500_WriteReg(CC2500_00_IOCFG2, 0x2F); + CC2500_WriteReg(CC2500_02_IOCFG0, 0x2F | 0x40); } else if (mode == RX_EN) diff --git a/Multiprotocol/Hubsan_a7105.ino b/Multiprotocol/Hubsan_a7105.ino index 249f220..f8e4ed5 100644 --- a/Multiprotocol/Hubsan_a7105.ino +++ b/Multiprotocol/Hubsan_a7105.ino @@ -180,7 +180,7 @@ static void __attribute__((unused)) hubsan_build_packet() hubsan_update_crc(); } -#if defined(TELEMETRY) +#ifdef HUBSAN_HUB_TELEMETRY static uint8_t __attribute__((unused)) hubsan_check_integrity() { if( (packet[0]&0xFE) != 0xE0 ) @@ -194,7 +194,7 @@ static uint8_t __attribute__((unused)) hubsan_check_integrity() uint16_t ReadHubsan() { -#if defined(TELEMETRY) +#ifdef HUBSAN_HUB_TELEMETRY static uint8_t rfMode=0; #endif static uint8_t txState=0; @@ -279,7 +279,7 @@ uint16_t ReadHubsan() case DATA_4: case DATA_5: if( txState == 0) { // send packet -#if defined(TELEMETRY) +#ifdef HUBSAN_HUB_TELEMETRY rfMode = A7105_TX; #endif if( phase == DATA_1) @@ -293,7 +293,7 @@ uint16_t ReadHubsan() delay=3000; } else { -#if defined(TELEMETRY) +#ifdef HUBSAN_HUB_TELEMETRY if( rfMode == A7105_TX) {// switch to rx mode 3ms after packet sent for( i=0; i<10; i++) @@ -315,7 +315,7 @@ uint16_t ReadHubsan() A7105_ReadData(16); if( hubsan_check_integrity() ) { - v_lipo=packet[13];// hubsan lipo voltage 8bits the real value is h_lipo/10(0x2A=42 -> 4.2V) + v_lipo1=packet[13]*2;// hubsan lipo voltage 8bits the real value is h_lipo/10(0x2A=42 -> 4.2V) telemetry_link=1; } A7105_Strobe(A7105_RX); @@ -351,8 +351,8 @@ uint16_t initHubsan() { phase = BIND_1; packet_count=0; id_data=ID_NORMAL; -#if defined(TELEMETRY) - telemetry_link=0; +#ifdef HUBSAN_HUB_TELEMETRY + init_hub_telemetry(); #endif return 10000; } diff --git a/Multiprotocol/Multiprotocol.ino b/Multiprotocol/Multiprotocol.ino index 023b6be..e77d040 100644 --- a/Multiprotocol/Multiprotocol.ino +++ b/Multiprotocol/Multiprotocol.ino @@ -146,7 +146,8 @@ uint8_t pkt[MAX_PKT];//telemetry receiving packets volatile uint8_t tx_head=0; volatile uint8_t tx_tail=0; #endif // BASH_SERIAL - uint8_t v_lipo; + uint8_t v_lipo1; + uint8_t v_lipo2; int16_t RSSI_dBm; uint8_t TX_RSSI; uint8_t telemetry_link=0; @@ -490,7 +491,7 @@ void Update_All() #endif //ENABLE_PPM update_channels_aux(); #if defined(TELEMETRY) - if((protocol==MODE_FRSKYD) || (protocol==MODE_HUBSAN) || (protocol==MODE_AFHDS2A) || (protocol==MODE_FRSKYX) || (protocol==MODE_DSM) ) + if((protocol==MODE_FRSKYD) || (protocol==MODE_BAYANG) || (protocol==MODE_HUBSAN) || (protocol==MODE_AFHDS2A) || (protocol==MODE_FRSKYX) || (protocol==MODE_DSM) ) TelemetryUpdate(); #endif update_led_status(); @@ -999,7 +1000,7 @@ void Mprotocol_serial_init() #if defined(TELEMETRY) void PPM_Telemetry_serial_init() { - if( (protocol==MODE_FRSKYD) || (protocol==MODE_HUBSAN) || (protocol==MODE_AFHDS2A) ) + if( (protocol==MODE_FRSKYD) || (protocol==MODE_HUBSAN) || (protocol==MODE_AFHDS2A) || (protocol==MODE_BAYANG) ) initTXSerial( SPEED_9600 ) ; if(protocol==MODE_FRSKYX) initTXSerial( SPEED_57600 ) ; diff --git a/Multiprotocol/Telemetry.ino b/Multiprotocol/Telemetry.ino index 9923c41..ac986c6 100644 --- a/Multiprotocol/Telemetry.ino +++ b/Multiprotocol/Telemetry.ino @@ -52,7 +52,7 @@ uint8_t frame[18]; } SerialControl ; #endif -#if defined DSM_TELEMETRY +#ifdef DSM_TELEMETRY void DSM_frame() { Serial_write(0xAA); // Telemetry packet @@ -61,7 +61,7 @@ void DSM_frame() } #endif -#if defined AFHDS2A_TELEMETRY +#ifdef AFHDS2A_FW_TELEMETRY void AFHDSA_short_frame() { Serial_write(0xAA); // Telemetry packet @@ -127,6 +127,16 @@ void frsky_check_telemetry(uint8_t *pkt,uint8_t len) } } +void init_hub_telemetry() +{ + telemetry_link=0; + telemetry_counter=0; + v_lipo1=0; + v_lipo2=0; + RSSI_dBm=0; + TX_RSSI=0; +} + void frsky_link_frame() { frame[0] = 0xFE; @@ -139,14 +149,14 @@ void frsky_link_frame() frame[4] = (uint8_t)RSSI_dBm; } else - if (protocol==MODE_HUBSAN||protocol==MODE_AFHDS2A) + if (protocol==MODE_HUBSAN||protocol==MODE_AFHDS2A||protocol==MODE_BAYANG) { - frame[1] = v_lipo*2; //v_lipo; common 0x2A=42/10=4.2V - frame[2] = frame[1]; - frame[3] = protocol==MODE_HUBSAN?0x00:(uint8_t)RSSI_dBm; + frame[1] = v_lipo1; + frame[2] = v_lipo2; + frame[3] = (uint8_t)RSSI_dBm; frame[4] = TX_RSSI; } - frame[5] = frame[6] = frame[7] = frame[8] = 0; + frame[5] = frame[6] = frame[7] = frame[8] = 0; frskySendStuffed(); } @@ -494,15 +504,15 @@ void TelemetryUpdate() return; } #endif - #if defined AFHDS2A_TELEMETRY + #if defined AFHDS2A_FW_TELEMETRY if(telemetry_link == 2 && protocol == MODE_AFHDS2A) - { - AFHDSA_short_frame(); - telemetry_link=0; - } + { + AFHDSA_short_frame(); + telemetry_link=0; + } #endif if(telemetry_link && protocol != MODE_FRSKYX ) - { // FrSkyD + Hubsan + AFHDS2A + { // FrSkyD + Hubsan + AFHDS2A + Bayang frsky_link_frame(); telemetry_link=0; return; diff --git a/Multiprotocol/Validate.h b/Multiprotocol/Validate.h index 277d5b1..3402a0a 100644 --- a/Multiprotocol/Validate.h +++ b/Multiprotocol/Validate.h @@ -69,24 +69,34 @@ //Make sure telemetry is selected correctly #ifndef TELEMETRY #undef INVERT_TELEMETRY - #undef DSM_TELEMETRY - #undef SPORT_TELEMETRY + #undef AFHDS2A_FW_TELEMETRY + #undef AFHDS2A_HUB_TELEMETRY + #undef BAYANG_HUB_TELEMETRY + #undef HUBSAN_HUB_TELEMETRY #undef HUB_TELEMETRY - #undef AFHDS2A_TELEMETRY + #undef SPORT_TELEMETRY + #undef DSM_TELEMETRY #else - #if not defined(DSM_CYRF6936_INO) - #undef DSM_TELEMETRY + #if not defined(BAYANG_NRF24L01_INO) + #undef BAYANG_HUB_TELEMETRY #endif - #if not defined(FRSKYD_CC2500_INO) && not defined(HUBSAN_A7105_INO) && not defined(AFHDS2A_A7105_INO) + #if not defined(HUBSAN_A7105_INO) + #undef HUBSAN_HUB_TELEMETRY + #endif + #if not defined(AFHDS2A_A7105_INO) + #undef AFHDS2A_HUB_TELEMETRY + #undef AFHDS2A_FW_TELEMETRY + #endif + #if not defined(FRSKYD_CC2500_INO) #undef HUB_TELEMETRY #endif #if not defined(FRSKYX_CC2500_INO) #undef SPORT_TELEMETRY #endif - #if not defined(AFHDS2A_A7105_INO) - #undef AFHDS2A_TELEMETRY + #if not defined(DSM_CYRF6936_INO) + #undef DSM_TELEMETRY #endif - #if not defined(DSM_TELEMETRY) && not defined(HUB_TELEMETRY) && not defined(SPORT_TELEMETRY) && not defined(AFHDS2A_TELEMETRY) + #if not defined(DSM_TELEMETRY) && not defined(SPORT_TELEMETRY) && not defined(HUB_TELEMETRY) && not defined(HUBSAN_HUB_TELEMETRY) && not defined(BAYANG_HUB_TELEMETRY) && not defined(AFHDS2A_HUB_TELEMETRY) && not defined(AFHDS2A_FW_TELEMETRY) #undef TELEMETRY #undef INVERT_TELEMETRY #endif diff --git a/Multiprotocol/_Config.h b/Multiprotocol/_Config.h index dd6a9a0..9e84e4c 100644 --- a/Multiprotocol/_Config.h +++ b/Multiprotocol/_Config.h @@ -62,7 +62,7 @@ //All the protocols will not fit in the Atmega328p module so you need to pick and choose. //Comment the protocols you are not using with "//" to save Flash space. -//The protocols below need an A7105 to be installed +/*//The protocols below need an A7105 to be installed #define FLYSKY_A7105_INO #define HUBSAN_A7105_INO #define AFHDS2A_A7105_INO @@ -77,10 +77,10 @@ #define FRSKYV_CC2500_INO #define FRSKYX_CC2500_INO #define SFHSS_CC2500_INO - +*/ //The protocols below need a NRF24L01 to be installed #define BAYANG_NRF24L01_INO -#define CG023_NRF24L01_INO +/*#define CG023_NRF24L01_INO #define CX10_NRF24L01_INO // Include Q2X2 protocol #define ESKY_NRF24L01_INO #define HISKY_NRF24L01_INO @@ -96,6 +96,7 @@ #define FQ777_NRF24L01_INO #define ASSAN_NRF24L01_INO #define HONTAI_NRF24L01_INO +*/ /**************************/ /*** TELEMETRY SETTINGS ***/ @@ -110,10 +111,14 @@ //#define INVERT_TELEMETRY //Comment a line to disable a protocol telemetry -#define DSM_TELEMETRY -#define SPORT_TELEMETRY -#define HUB_TELEMETRY -#define AFHDS2A_TELEMETRY +#define DSM_TELEMETRY // Forward received telemetry packet directly to TX to be decoded +#define SPORT_TELEMETRY // Use FrSkyX SPORT format to send telemetry to TX +#define AFHDS2A_FW_TELEMETRY // Forward received telemetry packet directly to TX to be decoded +#define HUB_TELEMETRY // Use FrSkyD Hub format to send telemetry to TX +#define AFHDS2A_HUB_TELEMETRY // Use FrSkyD Hub format to send telemetry to TX +#define BAYANG_HUB_TELEMETRY // Use FrSkyD Hub format to send telemetry to TX +#define HUBSAN_HUB_TELEMETRY // Use FrSkyD Hub format to send telemetry to TX + /****************************/ /*** SERIAL MODE SETTINGS ***/