From 999c630c5a34b0beeaa9eafdaa29f7ffb8b0d259 Mon Sep 17 00:00:00 2001 From: pascallanger Date: Thu, 20 Oct 2016 19:29:46 +0200 Subject: [PATCH] AFHDS2A protocol addition: UNTESTED Protocol number: 28 Sub protocols: PWM_IBUS = 0, PPM_IBUS = 1, PWM_SBUS = 2, PPM_SBUS = 3 Option value =0->50Hz, =1->400Hz, =2->5Hz Extended channel range supported Telemetry supported for voltage and RSSI (RX RSSI) --- Multiprotocol/A7105_SPI.ino | 67 +++++-- Multiprotocol/AFHDS2A_a7105.ino | 336 ++++++++++++++++++++++++++++++++ Multiprotocol/FlySky_a7105.ino | 2 +- Multiprotocol/Hubsan_a7105.ino | 8 +- Multiprotocol/Multiprotocol.h | 9 + Multiprotocol/Multiprotocol.ino | 12 +- Multiprotocol/TX_Def.h | 1 - Multiprotocol/Telemetry.ino | 4 +- Multiprotocol/_Config.h | 3 + Multiprotocol/iface_a7105.h | 5 - 10 files changed, 411 insertions(+), 36 deletions(-) create mode 100644 Multiprotocol/AFHDS2A_a7105.ino diff --git a/Multiprotocol/A7105_SPI.ino b/Multiprotocol/A7105_SPI.ino index 926b594..183444d 100644 --- a/Multiprotocol/A7105_SPI.ino +++ b/Multiprotocol/A7105_SPI.ino @@ -31,13 +31,13 @@ void A7105_WriteData(uint8_t len, uint8_t channel) A7105_Strobe(A7105_TX); } -void A7105_ReadData() +void A7105_ReadData(uint8_t len) { uint8_t i; A7105_Strobe(0xF0); //A7105_RST_RDPTR A7105_CSN_off; SPI_Write(0x45); - for (i=0;i<16;i++) + for (i=0;i. + */ +// Last sync with hexfet new_protocols/flysky_a7105.c dated 2015-09-28 + +#ifdef AFHDS2A_A7105_INO + +#define AFHDS2A_EEPROMadress 0 // rx ID 32bit + +#define AFHDS2A_TXPACKET_SIZE 38 +#define AFHDS2A_RXPACKET_SIZE 37 +#define AFHDS2A_NUMFREQ 16 + +enum{ + AFHDS2A_PACKET_STICKS, + AFHDS2A_PACKET_SETTINGS, + AFHDS2A_PACKET_FAILSAFE, +}; + +enum{ + AFHDS2A_BIND1, + AFHDS2A_BIND2, + AFHDS2A_BIND3, + AFHDS2A_BIND4, + AFHDS2A_DATA, +}; + +static void AFHDS2A_calc_channels() +{ + uint8_t idx = 0; + uint32_t rnd = MProtocol_id; + while (idx < AFHDS2A_NUMFREQ) + { + uint8_t i; + uint8_t count_1_42 = 0, count_43_85 = 0, count_86_128 = 0, count_129_168 = 0; + rnd = rnd * 0x0019660D + 0x3C6EF35F; // Randomization + + uint8_t next_ch = ((rnd >> (idx%32)) % 0xa8) + 1; + // Keep the distance 2 between the channels - either odd or even + if (((next_ch ^ MProtocol_id) & 0x01 )== 0) + continue; + // Check that it's not duplicate and spread uniformly + for (i = 0; i < idx; i++) + { + if(hopping_frequency[i] == next_ch) + break; + if(hopping_frequency[i] <= 42) + count_1_42++; + else if (hopping_frequency[i] <= 85) + count_43_85++; + else if (hopping_frequency[i] <= 128) + count_86_128++; + else + count_129_168++; + } + if (i != idx) + continue; + if ((next_ch <= 42 && count_1_42 < 5) + ||(next_ch >= 43 && next_ch <= 85 && count_43_85 < 5) + ||(next_ch >= 86 && next_ch <=128 && count_86_128 < 5) + ||(next_ch >= 129 && count_129_168 < 5)) + hopping_frequency[idx++] = next_ch; + } +} + +// telemetry sensors ID +enum{ + AFHDS2A_SENSOR_RX_VOLTAGE = 0x00, + AFHDS2A_SENSOR_RX_ERR_RATE = 0xfe, + AFHDS2A_SENSOR_RX_RSSI = 0xfc, + AFHDS2A_SENSOR_RX_NOISE = 0xfb, + AFHDS2A_SENSOR_RX_SNR = 0xfa, +}; + +static void AFHDS2A_update_telemetry() +{ + // AA | TXID | rx_id | sensor id | sensor # | value 16 bit big endian | sensor id ...... + // max 7 sensors per packet + + for(uint8_t sensor=0; sensor<7; sensor++) + { + uint8_t index = 9+(4*sensor); + switch(packet[index]) + { + case AFHDS2A_SENSOR_RX_VOLTAGE: + v_lipo = packet[index+3]<<8 | packet[index+2]; + telemetry_link=1; + break; + /*case AFHDS2A_SENSOR_RX_ERR_RATE: + // packet[index+2]; + break;*/ + case AFHDS2A_SENSOR_RX_RSSI: + RSSI_dBm = -packet[index+2]; + break; + case 0xff: + return; + /*default: + // unknown sensor ID + break;*/ + } + } +} + +static void AFHDS2A_build_bind_packet() +{ + uint8_t ch; + memcpy( &packet[1], rx_tx_addr, 4); + memset( &packet[5], 0xff, 4); + packet[10]= 0x00; + for(ch=0; ch<16; ch++) + packet[11+ch] = hopping_frequency[ch]; + memset( &packet[27], 0xff, 10); + packet[37] = 0x00; + switch(phase) + { + case AFHDS2A_BIND1: + packet[0] = 0xbb; + packet[9] = 0x01; + break; + case AFHDS2A_BIND2: + case AFHDS2A_BIND3: + case AFHDS2A_BIND4: + packet[0] = 0xbc; + if(phase == AFHDS2A_BIND4) + { + memcpy( &packet[5], &rx_id, 4); + memset( &packet[11], 0xff, 16); + } + packet[9] = phase-1; + if(packet[9] > 0x02) + packet[9] = 0x02; + packet[27]= 0x01; + packet[28]= 0x80; + break; + } +} + +static void AFHDS2A_build_packet(uint8_t type) +{ + memcpy( &packet[1], rx_tx_addr, 4); + memcpy( &packet[5], rx_id, 4); + switch(type) + { + case AFHDS2A_PACKET_STICKS: + packet[0] = 0x58; + for(uint8_t ch=0; ch<14; ch++) + { + packet[9 + ch*2] = Servo_data[CH_AETR[ch]]&0xFF; + packet[10 + ch*2] = (Servo_data[CH_AETR[ch]]>>8)&0xFF; + } + packet[37] = 0x00; + break; + case AFHDS2A_PACKET_FAILSAFE: + packet[0] = 0x56; + for(uint8_t ch=0; ch<14; ch++) + { + /*if((Model.limits[ch].flags & CH_FAILSAFE_EN)) + { + packet[9 + ch*2] = Servo_data[CH_AETR[ch]] & 0xff; + packet[10+ ch*2] = (Servo_data[CH_AETR[ch]] >> 8) & 0xff; + } + else*/ + { + packet[9 + ch*2] = 0xff; + packet[10+ ch*2] = 0xff; + } + } + packet[37] = 0x00; + break; + case AFHDS2A_PACKET_SETTINGS: + packet[0] = 0xaa; + packet[9] = 0xfd; + packet[10]= 0xff; + packet[12]= 0x00; + if(option==0) //50Hz + packet[11]= 50; + else + if(option==2) //5Hz + packet[11]= 50; + else //400Hz + { + packet[11]= 400&0xFF; + packet[12]= 400>>8; + } + if(sub_protocol == PPM_IBUS || sub_protocol == PPM_SBUS) + packet[13] = 0x01; // PPM output enabled + else + packet[13] = 0x00; + packet[14]= 0x00; + for(uint8_t i=15; i<37; i++) + packet[i] = 0xff; + packet[18] = 0x05; // ? + packet[19] = 0xdc; // ? + packet[20] = 0x05; // ? + if(sub_protocol == PWM_SBUS || sub_protocol == PPM_SBUS) + packet[21] = 0xdd; // SBUS output enabled + else + packet[21] = 0xde; + packet[37] = 0x00; + break; + } +} + +#define AFHDS2A_WAIT_WRITE 0x80 +uint16_t ReadAFHDS2A() +{ + uint8_t packet_type = AFHDS2A_PACKET_STICKS; + switch(phase) + { + case AFHDS2A_BIND1: + case AFHDS2A_BIND2: + case AFHDS2A_BIND3: + A7105_Strobe(A7105_STANDBY); + AFHDS2A_build_bind_packet(); + A7105_WriteData(AFHDS2A_TXPACKET_SIZE, packet_count%2 ? 0x0d : 0x8c); + if(A7105_ReadReg(A7105_00_MODE) == 0x1b) + { // todo: replace with check crc+fec + A7105_Strobe(A7105_RST_RDPTR); + A7105_ReadData(AFHDS2A_RXPACKET_SIZE); + if(packet[0] == 0xbc) + { + uint8_t temp=50+RX_num*4; + for(uint8_t i=0; i<4; i++) + { + rx_id[i] = packet[5+i]; + eeprom_write_byte((EE_ADDR)(temp+i),rx_id[i]); + } + if(packet[9] == 0x01) + phase = AFHDS2A_BIND4; + } + } + packet_count++; + phase |= AFHDS2A_WAIT_WRITE; + return 1700; + case AFHDS2A_BIND1|AFHDS2A_WAIT_WRITE: + case AFHDS2A_BIND2|AFHDS2A_WAIT_WRITE: + case AFHDS2A_BIND3|AFHDS2A_WAIT_WRITE: + A7105_Strobe(A7105_RX); + phase &= ~AFHDS2A_WAIT_WRITE; + phase++; + if(phase > AFHDS2A_BIND3) + phase = AFHDS2A_BIND1; + return 2150; + case AFHDS2A_BIND4: + A7105_Strobe(A7105_STANDBY); + AFHDS2A_build_bind_packet(); + A7105_WriteData(AFHDS2A_TXPACKET_SIZE, packet_count%2 ? 0x0d : 0x8c); + packet_count++; + bind_phase++; + if(bind_phase>=4) + { + packet_count=0; + hopping_frequency_no=1; + phase = AFHDS2A_DATA; + BIND_DONE; + } + phase |= AFHDS2A_WAIT_WRITE; + return 1700; + case AFHDS2A_BIND4|AFHDS2A_WAIT_WRITE: + A7105_Strobe(A7105_RX); + phase &= ~AFHDS2A_WAIT_WRITE; + return 2150; + case AFHDS2A_DATA: + A7105_Strobe(A7105_STANDBY); + AFHDS2A_build_packet(packet_type); + A7105_WriteData(AFHDS2A_TXPACKET_SIZE, hopping_frequency[hopping_frequency_no++]); + if(hopping_frequency_no >= 16) + hopping_frequency_no = 0; + if(!(packet_count % 1313)) + packet_type = AFHDS2A_PACKET_SETTINGS; + else if(!(packet_count % 1569)) + packet_type = AFHDS2A_PACKET_FAILSAFE; + else + packet_type = AFHDS2A_PACKET_STICKS; // todo : check for settings changes + // got some data from RX ? + // we've no way to know if RX fifo has been filled + // as we can't poll GIO1 or GIO2 to check WTR + // we can't check A7105_MASK_TREN either as we know + // it's currently in transmit mode. + if(!(A7105_ReadReg(A7105_00_MODE) & (1<<5 | 1<<6))) + { // FECF+CRCF Ok + A7105_Strobe(A7105_RST_RDPTR); + A7105_ReadData(1); + if(packet[0] == 0xaa) + { + A7105_Strobe(A7105_RST_RDPTR); + A7105_ReadData(AFHDS2A_RXPACKET_SIZE); + if(packet[9] == 0xfc) + packet_type=AFHDS2A_PACKET_SETTINGS; // RX is asking for settings + else + AFHDS2A_update_telemetry(); + } + } + packet_count++; + phase |= AFHDS2A_WAIT_WRITE; + return 1700; + case AFHDS2A_DATA|AFHDS2A_WAIT_WRITE: + phase &= ~AFHDS2A_WAIT_WRITE; + A7105_Strobe(A7105_RX); + return 2150; + } + return 3850; // never reached, please the compiler +} + +uint16_t initAFHDS2A() +{ + A7105_Init(); + + AFHDS2A_calc_channels(); + packet_count = 0; + bind_phase = 0; + if(IS_AUTOBIND_FLAG_on) + phase = AFHDS2A_BIND1; + else + { + phase = AFHDS2A_DATA; + //Read RX ID from EEPROM based on RX_num, RX_num must be uniq for each RX + uint8_t temp=50+RX_num*4; + for(uint8_t i=0;i<4;i++) + rx_id[i]=eeprom_read_byte((EE_ADDR)(temp+i)); + } + hopping_frequency_no = 0; + return 50000; +} +#endif diff --git a/Multiprotocol/FlySky_a7105.ino b/Multiprotocol/FlySky_a7105.ino index 43ccffd..c1632e7 100644 --- a/Multiprotocol/FlySky_a7105.ino +++ b/Multiprotocol/FlySky_a7105.ino @@ -184,7 +184,7 @@ uint16_t initFlySky() uint8_t chanoffset; uint8_t temp; - A7105_Init(INIT_FLYSKY); //flysky_init(); + A7105_Init(); if ((rx_tx_addr[3]&0xF0) > 0x90) // limit offset to 9 as higher values don't work with some RX (ie V912) rx_tx_addr[3]=rx_tx_addr[3]-0x70; diff --git a/Multiprotocol/Hubsan_a7105.ino b/Multiprotocol/Hubsan_a7105.ino index de0c2bf..e6669ae 100644 --- a/Multiprotocol/Hubsan_a7105.ino +++ b/Multiprotocol/Hubsan_a7105.ino @@ -253,7 +253,7 @@ uint16_t ReadHubsan() phase = BIND_1; return 4500; //No signal, restart binding procedure. 12msec elapsed since last write } - A7105_ReadData(); + A7105_ReadData(16); phase++; if (phase == BIND_5) A7105_WriteID(((uint32_t)packet[2] << 24) | ((uint32_t)packet[3] << 16) | ((uint32_t)packet[4] << 8) | packet[5]); @@ -264,7 +264,7 @@ uint16_t ReadHubsan() phase = BIND_7; return 15000; //22.5msec elapsed since last write } - A7105_ReadData(); + A7105_ReadData(16); if(packet[1] == 9 && id_data == ID_NORMAL) { phase = DATA_1; A7105_WriteReg(A7105_1F_CODE_I, 0x0F); @@ -314,7 +314,7 @@ uint16_t ReadHubsan() { if( !(A7105_ReadReg(A7105_00_MODE) & 0x01)) { // data received - A7105_ReadData(); + 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) @@ -343,7 +343,7 @@ uint16_t ReadHubsan() uint16_t initHubsan() { const uint8_t allowed_ch[] = {0x14, 0x1e, 0x28, 0x32, 0x3c, 0x46, 0x50, 0x5a, 0x64, 0x6e, 0x78, 0x82}; - A7105_Init(INIT_HUBSAN); //hubsan_init(); + A7105_Init(); sessionid = random(0xfefefefe) + ((uint32_t)random(0xfefefefe) << 16); channel = allowed_ch[random(0xfefefefe) % sizeof(allowed_ch)]; diff --git a/Multiprotocol/Multiprotocol.h b/Multiprotocol/Multiprotocol.h index 54e9e55..6f69473 100644 --- a/Multiprotocol/Multiprotocol.h +++ b/Multiprotocol/Multiprotocol.h @@ -46,6 +46,7 @@ enum PROTOCOLS MODE_FRSKYV = 25, // =>CC2500 MODE_HONTAI = 26, // =>NRF24L01 MODE_OPENLRS = 27, // =>OpenLRS hardware + MODE_AFHDS2A = 28, // =>A7105 }; enum Flysky @@ -55,6 +56,13 @@ enum Flysky V6X6 = 2, V912 = 3 }; +enum AFHDS2A +{ + PWM_IBUS = 0, + PPM_IBUS = 1, + PWM_SBUS = 2, + PPM_SBUS = 3, +}; enum Hisky { Hisky = 0, @@ -378,6 +386,7 @@ Serial: 100000 Baud 8e2 _ xxxx xxxx p -- FrskyV 25 HONTAI 26 OpenLRS 27 + AFHDS2A 28 BindBit=> 0x80 1=Bind/0=No AutoBindBit=> 0x40 1=Yes /0=No RangeCheck=> 0x20 1=Yes /0=No diff --git a/Multiprotocol/Multiprotocol.ino b/Multiprotocol/Multiprotocol.ino index 079b842..b6ac0ac 100644 --- a/Multiprotocol/Multiprotocol.ino +++ b/Multiprotocol/Multiprotocol.ino @@ -66,6 +66,7 @@ uint16_t servo_max_100,servo_min_100,servo_max_125,servo_min_125; // Protocol variables uint8_t cyrfmfg_id[6];//for dsm2 and devo uint8_t rx_tx_addr[5]; +uint8_t rx_id[4]; uint8_t phase; uint16_t bind_counter; uint8_t bind_phase; @@ -93,7 +94,7 @@ uint8_t RX_num; #endif //Channel mapping for protocols -const uint8_t CH_AETR[]={AILERON, ELEVATOR, THROTTLE, RUDDER, AUX1, AUX2, AUX3, AUX4, AUX5, AUX6, AUX7, AUX8}; +const uint8_t CH_AETR[]={AILERON, ELEVATOR, THROTTLE, RUDDER, AUX1, AUX2, AUX3, AUX4, AUX5, AUX6, AUX7, AUX8, AUX9, AUX10}; const uint8_t CH_TAER[]={THROTTLE, AILERON, ELEVATOR, RUDDER, AUX1, AUX2, AUX3, AUX4, AUX5, AUX6, AUX7, AUX8}; const uint8_t CH_RETA[]={RUDDER, ELEVATOR, THROTTLE, AILERON, AUX1, AUX2, AUX3, AUX4, AUX5, AUX6, AUX7, AUX8}; const uint8_t CH_EATR[]={ELEVATOR, AILERON, THROTTLE, RUDDER, AUX1, AUX2, AUX3, AUX4, AUX5, AUX6, AUX7, AUX8}; @@ -481,7 +482,7 @@ void Update_All() #endif //ENABLE_PPM update_led_status(); #if defined(TELEMETRY) - if((protocol==MODE_FRSKYD) || (protocol==MODE_HUBSAN) || (protocol==MODE_FRSKYX) || (protocol==MODE_DSM) ) + if((protocol==MODE_FRSKYD) || (protocol==MODE_HUBSAN) || (protocol==MODE_AFHDS2A) || (protocol==MODE_FRSKYX) || (protocol==MODE_DSM) ) TelemetryUpdate(); #endif } @@ -610,6 +611,13 @@ static void protocol_init() remote_callback = ReadFlySky; break; #endif + #if defined(AFHDS2A_A7105_INO) + case MODE_AFHDS2A: + PE1_off; //antenna RF1 + next_callback = initAFHDS2A(); + remote_callback = ReadAFHDS2A; + break; + #endif #if defined(HUBSAN_A7105_INO) case MODE_HUBSAN: PE1_off; //antenna RF1 diff --git a/Multiprotocol/TX_Def.h b/Multiprotocol/TX_Def.h index 603ce76..f59f34b 100644 --- a/Multiprotocol/TX_Def.h +++ b/Multiprotocol/TX_Def.h @@ -210,4 +210,3 @@ #define AUX10 13 #define AUX11 14 #define AUX12 15 -#define AUX13 16 diff --git a/Multiprotocol/Telemetry.ino b/Multiprotocol/Telemetry.ino index c37dddf..12fa8a3 100644 --- a/Multiprotocol/Telemetry.ino +++ b/Multiprotocol/Telemetry.ino @@ -130,7 +130,7 @@ void frsky_link_frame() frame[4] = (uint8_t)RSSI_dBm; } else - if (protocol==MODE_HUBSAN) + if (protocol==MODE_HUBSAN||protocol==MODE_AFHDS2A) { frame[1] = v_lipo*2; //v_lipo; common 0x2A=42/10=4.2V frame[2] = frame[1]; @@ -486,7 +486,7 @@ void TelemetryUpdate() } #endif if(telemetry_link && protocol != MODE_FRSKYX ) - { // FrSky + Hubsan + { // FrSky + Hubsan + AFHDS2A frsky_link_frame(); telemetry_link=0; return; diff --git a/Multiprotocol/_Config.h b/Multiprotocol/_Config.h index 50c0b0f..5421a84 100644 --- a/Multiprotocol/_Config.h +++ b/Multiprotocol/_Config.h @@ -57,6 +57,7 @@ //The protocols below need an A7105 to be installed #define FLYSKY_A7105_INO #define HUBSAN_A7105_INO +#define AFHDS2A_A7105_INO //The protocols below need a CYRF6936 to be installed #define DEVO_CYRF6936_INO @@ -255,6 +256,8 @@ const PPM_Parameters PPM_prot[15]= { FORMAT_HONTAI FORMAT_JJRCX1 FORMAT_X5C1 + MODE_AFHDS2A + NONE */ // RX_Num is used for model match. Using RX_Num values different for each receiver will prevent starting a model with the false config loaded... diff --git a/Multiprotocol/iface_a7105.h b/Multiprotocol/iface_a7105.h index f8792d9..f04121d 100644 --- a/Multiprotocol/iface_a7105.h +++ b/Multiprotocol/iface_a7105.h @@ -87,9 +87,4 @@ enum A7105_MASK { A7105_MASK_VBCF = 1 << 3, }; -enum { - INIT_FLYSKY, - INIT_HUBSAN -}; - #endif \ No newline at end of file