From fbb919d7671c5f3ba028a6365f01678563beca3c Mon Sep 17 00:00:00 2001 From: pascallanger Date: Tue, 3 Jan 2017 19:19:53 +0100 Subject: [PATCH] New protocol WK2x01 Protocol WK2x01 number 30 Sub protocols: - WK2801 number 0, 8 channels, fixed id not supported - WK2601 number 1, 6/7 channels, option is used see doc for details - WK2401 number 2, 4 channels Extended limits supported Autobind protocol Most receivers support WK2801 so always start trying this sub protocol first. --- Multiprotocol/Multi.txt | 1 + Multiprotocol/Multiprotocol.h | 12 + Multiprotocol/Multiprotocol.ino | 7 + Multiprotocol/Validate.h | 1 + Multiprotocol/WK2x01_cyrf6936.ino | 457 ++++++++++++++++++++++++++++++ Multiprotocol/_Config.h | 7 + 6 files changed, 485 insertions(+) create mode 100644 Multiprotocol/WK2x01_cyrf6936.ino diff --git a/Multiprotocol/Multi.txt b/Multiprotocol/Multi.txt index 4ee2b9d..a18b8df 100644 --- a/Multiprotocol/Multi.txt +++ b/Multiprotocol/Multi.txt @@ -27,3 +27,4 @@ 27,OpnLrs 28,AFHD2SA,PWM_IBUS,PPM_IBUS,PWM_SBUS,PPM_SBUS 29,Q2X2,Q222,Q242,Q282 +30,WK2X01,WK2801,WK2601,WK2401 diff --git a/Multiprotocol/Multiprotocol.h b/Multiprotocol/Multiprotocol.h index 2b577e3..3c9c8ef 100644 --- a/Multiprotocol/Multiprotocol.h +++ b/Multiprotocol/Multiprotocol.h @@ -56,6 +56,7 @@ enum PROTOCOLS MODE_OPENLRS = 27, // =>OpenLRS hardware MODE_AFHDS2A = 28, // =>A7105 MODE_Q2X2 = 29, // =>NRF24L01, extension of CX-10 protocol + MODE_WK2x01 = 30, // =>CYRF6936 }; enum Flysky @@ -174,6 +175,12 @@ enum FY326 FY326 = 0, FY319 = 1, }; +enum WK2x01 +{ + WK2801 = 0, + WK2601 = 1, + WK2401 = 2, +}; #define NONE 0 #define P_HIGH 1 @@ -448,6 +455,7 @@ Serial: 100000 Baud 8e2 _ xxxx xxxx p -- OpenLRS 27 AFHDS2A 28 Q2X2 29 + WK2x01 30 BindBit=> 0x80 1=Bind/0=No AutoBindBit=> 0x40 1=Yes /0=No RangeCheck=> 0x20 1=Yes /0=No @@ -535,6 +543,10 @@ Serial: 100000 Baud 8e2 _ xxxx xxxx p -- sub_protocol==FY326 FY326 0 FY319 1 + sub_protocol==WK2x01 + WK2801 0 + WK2601 1 + WK2401 2 Power value => 0x80 0=High/1=Low Stream[3] = option_protocol; diff --git a/Multiprotocol/Multiprotocol.ino b/Multiprotocol/Multiprotocol.ino index 32921a7..19de9a0 100644 --- a/Multiprotocol/Multiprotocol.ino +++ b/Multiprotocol/Multiprotocol.ino @@ -756,6 +756,13 @@ static void protocol_init() remote_callback = ReadJ6Pro; break; #endif + #if defined(WK2x01_CYRF6936_INO) + case MODE_WK2x01: + PE2_on; //antenna RF4 + next_callback = WK_setup(); + remote_callback = WK_cb; + break; + #endif #endif #ifdef NRF24L01_INSTALLED #if defined(HISKY_NRF24L01_INO) diff --git a/Multiprotocol/Validate.h b/Multiprotocol/Validate.h index ba094f3..27bf510 100644 --- a/Multiprotocol/Validate.h +++ b/Multiprotocol/Validate.h @@ -39,6 +39,7 @@ #undef DEVO_CYRF6936_INO #undef DSM_CYRF6936_INO #undef J6PRO_CYRF6936_INO + #undef WK2x01_CYRF6936_INO #endif #ifndef CC2500_INSTALLED #undef FRSKYD_CC2500_INO diff --git a/Multiprotocol/WK2x01_cyrf6936.ino b/Multiprotocol/WK2x01_cyrf6936.ino new file mode 100644 index 0000000..1136bb5 --- /dev/null +++ b/Multiprotocol/WK2x01_cyrf6936.ino @@ -0,0 +1,457 @@ +/* + This project is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + Multiprotocol is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with Multiprotocol. If not, see . + */ + +#if defined(WK2x01_CYRF6936_INO) + +#include "iface_cyrf6936.h" + +#define WK_BIND_COUNT 2980 +#define WK_NUM_WAIT_LOOPS (100 / 5) //each loop is ~5us. Do not wait more than 100us + +enum { + WK_BIND=0, + WK_BOUND_1, + WK_BOUND_2, + WK_BOUND_3, + WK_BOUND_4, + WK_BOUND_5, + WK_BOUND_6, + WK_BOUND_7, + WK_BOUND_8, +}; + +static const uint8_t WK_sopcodes[8] = { + /* Note these are in order transmitted (LSB 1st) */ + 0xDF,0xB1,0xC0,0x49,0x62,0xDF,0xC1,0x49 //0x49C1DF6249C0B1DF +}; +static const uint8_t init_2801[] = {0xc5, 0x34, 0x60, 0x00, 0x25}; +static const uint8_t init_2601[] = {0xb9, 0x45, 0xb0, 0xf1, 0x3a}; +static const uint8_t init_2401[] = {0xa5, 0x23, 0xd0, 0xf0, 0x00}; + +uint8_t WK_last_beacon; + +static void __attribute__((unused)) WK_add_pkt_crc(uint8_t init) +{ + uint8_t add = init; + uint8_t xou = init; + for (uint8_t i = 0; i < 14; i++) + { + add += packet[i]; + xou ^= packet[i]; + } + packet[14] = xou; + packet[15] = add; +} + +static void __attribute__((unused)) WK_build_bind_pkt(const uint8_t *init) +{ + packet[0] = init[0]; + packet[1] = init[1]; + packet[2] = hopping_frequency[0]; + packet[3] = hopping_frequency[1]; + packet[4] = init[2]; + packet[5] = hopping_frequency[2]; + packet[6] = 0xff; + packet[7] = 0x00; + packet[8] = 0x00; + packet[9] = 0x32; + if (sub_protocol == WK2401) + packet[10] = 0x10 | (rx_tx_addr[0] & 0x0e); + else + packet[10] = rx_tx_addr[0]; + packet[11] = rx_tx_addr[1]; + packet[12] = rx_tx_addr[2] | packet_count; + packet[13] = init[3]; + WK_add_pkt_crc(init[4]); +} + +static int16_t __attribute__((unused)) WK_map_16(int32_t x, int32_t in_min, int32_t in_max, int32_t out_min, int32_t out_max) +{ + return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; +} +static int16_t __attribute__((unused)) WK_get_channel(uint8_t ch, int32_t scale, int16_t center, int16_t range) +{ + int16_t value = WK_map_16(Servo_data[CH_AETR[ch]],servo_min_100,servo_max_100,-scale,scale)+center; + if (value < center - range) value = center - range; + if (value > center + range) value = center + range; + return value; +} + +static void __attribute__((unused)) WK_build_data_pkt_2401() +{ + uint16_t msb = 0; + uint8_t offset = 0; + for (uint8_t i = 0; i < 4; i++) + { + if (i == 2) + offset = 1; + int16_t value = WK_get_channel(i, 0x800, 0, 0xA00); //12 bits, allow value to go to 125% + uint16_t base = abs(value) >> 2; //10 bits is the base value + uint16_t trim = abs(value) & 0x03; //lowest 2 bits represent trim + if (base >= 0x200) + { //if value is > 100%, remainder goes to trim + trim = 4 *(base - 0x200); + base = 0x1ff; + } + base = (value >= 0) ? 0x200 + base : 0x200 - base; + trim = (value >= 0) ? 0x200 + trim : 0x200 - trim; + + packet[2*i+offset] = base & 0xff; + packet[2*i+offset+1] = trim & 0xff; + msb = (msb << 4) | ((base >> 6) & 0x0c) | ((trim >> 8) & 0x03); + } + packet[4] = msb >> 8; //Ele/Ail MSB + packet[9] = msb & 0xff; //Thr/Rud MSB + packet[10] = 0xe0 | (rx_tx_addr[0] & 0x0e); + packet[11] = rx_tx_addr[1]; + packet[12] = rx_tx_addr[2] | packet_count; + packet[13] = 0xf0; //FIXME - What is this? + WK_add_pkt_crc(0x00); +} + +#define PCT(pct, max) (((int32_t)(max) * (int32_t)(pct) + 1L) / 1000L) +#define MAXTHR 426 //Measured to provide equal value at +/-0 +static void __attribute__((unused)) WK_channels_6plus1_2601(uint8_t frame, int16_t *_v1, int16_t *_v2) +{ + int16_t thr = WK_get_channel(2, 1000, 0, 1000); + int16_t v1; + uint8_t thr_rev = 0, pitch_rev = 0; + if(thr > 0) + { + if(thr >= 780) + { //78% + v1 = 0; //thr = 60% * (x - 78%) / 22% + 40% + thr = PCT(1000-MAXTHR,512) * (thr-PCT(780,1000)) / PCT(220,1000) + PCT(MAXTHR,512); + } + else + { + v1 = 1023 - 1023 * thr / 780; + thr = PCT(MAXTHR, 512); //40% + } + } + else + { + thr = -thr; + thr_rev = 1; + if(thr >= 780) + { //78% + v1 = 1023; //thr = 60% * (x - 78%) / 22% + 40% + thr = PCT(1000-MAXTHR,512) * (thr-PCT(780,1000)) / PCT(220,1000) + PCT(MAXTHR,512); + } + else + { + v1 = 1023 * thr / 780; + thr = PCT(MAXTHR, 512); //40% + } + } + if (thr >= 512) + thr = 511; + packet[2] = thr & 0xff; + packet[4] = (packet[4] & 0xF3) | ((thr >> 6) & 0x04); + + int16_t pitch= WK_get_channel(5, 0x400, 0, 0x400); + if (pitch < 0) + { + pitch_rev = 1; + pitch = -pitch; + } + if (frame == 1) + { + //Pitch curve and range + if (thr > PCT(MAXTHR, 512)) + *_v2 = pitch - pitch * 16 * (thr - PCT(MAXTHR, 512)) / PCT(1000 - MAXTHR, 512) / 100; + else + *_v2 = pitch; + *_v1 = 0; + } + else + if (frame == 2) + { + //Throttle curve & Expo + *_v1 = v1; + *_v2 = 512; + } + packet[7] = (thr_rev << 5) | (pitch_rev << 2); //reverse bits + packet[8] = 0; +} + +static void __attribute__((unused)) WK_channels_5plus1_2601(uint8_t frame, int16_t *v1, int16_t *v2) +{ + (void)v1; + //Zero out pitch, provide ail, ele, thr, rud, gyr + gear + if (frame == 1) + *v2 = 0; //Pitch curve and range + packet[7] = 0; + packet[8] = 0; +} +static void __attribute__((unused)) WK_channels_heli_2601(uint8_t frame, int16_t *v1, int16_t *v2) +{ + //pitch is controlled by rx + //we can only control fmode, pit-reverse and pit/thr rate + uint8_t pit_rev = 0; + if ((option/10)%10) + pit_rev = 1; + int16_t pit_rate = WK_get_channel(5, 0x400, 0, 0x400); + uint8_t fmode = 1; + if (pit_rate < 0) + { + pit_rate = -pit_rate; + fmode = 0; + } + if (frame == 1) + { + //Pitch curve and range + *v1 = pit_rate; + *v2 = ((option/100) ? -100 : 100) * 0x400 / 100 + 0x400; + } + packet[7] = (pit_rev << 2); //reverse bits + packet[8] = fmode ? 0x02 : 0x00; +} + +static void __attribute__((unused)) WK_build_data_pkt_2601() +{ + uint8_t msb = 0; + uint8_t frame = (packet_count % 3); + for (uint8_t i = 0; i < 4; i++) + { + int16_t value = WK_get_channel(i, 0x190, 0, 0x1FF); + uint16_t mag = value < 0 ? -value : value; + packet[i] = mag & 0xff; + msb = (msb << 2) | ((mag >> 8) & 0x01) | (value < 0 ? 0x02 : 0x00); + } + packet[4] = msb; + int16_t v1 = 0x200, v2 = 0x200; + if (frame == 0) + { + //Gyro & Rudder mix + v1 = WK_get_channel(6, 0x200, 0x200, 0x200); + v2 = 0; + } + if (option%10 == 1) + WK_channels_heli_2601(frame, &v1, &v2); + else if (option%10 == 2) + WK_channels_6plus1_2601(frame, &v1, &v2); + else + WK_channels_5plus1_2601(frame, &v1, &v2); + if (v1 > 1023) + v1 = 1023; + if (v2 > 1023) + v2 = 1023; + packet[5] = v2 & 0xff; + packet[6] = v1 & 0xff; + //packet[7] handled by channel code + packet[8] |= (WK_get_channel(4, 0x190, 0, 0x1FF) > 0 ? 1 : 0); + packet[9] = ((v1 >> 4) & 0x30) | ((v2 >> 2) & 0xc0) | 0x04 | frame; + packet[10] = rx_tx_addr[0]; + packet[11] = rx_tx_addr[1]; + packet[12] = rx_tx_addr[2] | packet_count; + packet[13] = 0xff; + + WK_add_pkt_crc(0x3A); +} + +static void __attribute__((unused)) WK_build_data_pkt_2801() +{ + uint16_t msb = 0; + uint8_t offset = 0; + uint8_t sign = 0; + for (uint8_t i = 0; i < 8; i++) + { + if (i == 4) { offset = 1; } + int16_t value = WK_get_channel(i, 0x190, 0, 0x3FF); + uint16_t mag = value < 0 ? -value : value; + packet[i+offset] = mag & 0xff; + msb = (msb << 2) | ((mag >> 8) & 0x03); + if (value < 0) { sign |= 1 << i; } + } + packet[4] = msb >> 8; + packet[9] = msb & 0xff; + packet[10] = rx_tx_addr[0]; + packet[11] = rx_tx_addr[1]; + packet[12] = rx_tx_addr[2] | packet_count; + packet[13] = sign; + WK_add_pkt_crc(0x25); +} + +static void __attribute__((unused)) WK_build_beacon_pkt_2801() +{ + WK_last_beacon ^= 1; + uint8_t en = 0; + uint8_t bind_state=0x99; //Ignoring WK2801 bind process which is the same as Devo + + for (uint8_t i = 0; i < 4; i++) + { // failsafe info: WARNING All channels are set to 0 instead of midstick and 0 for throttle + packet[i+1] = 0; + } + packet[0] = en; + packet[5] = packet[4]; + packet[4] = WK_last_beacon << 6; + packet[6] = hopping_frequency[0]; + packet[7] = hopping_frequency[1]; + packet[8] = hopping_frequency[2]; + packet[9] = bind_state; + packet[10] = rx_tx_addr[0]; + packet[11] = rx_tx_addr[1]; + packet[12] = rx_tx_addr[2] | packet_count; + packet[13] = 0x00; //Does this matter? in the docs it is the same as the data packet + WK_add_pkt_crc(0x1C); +} + +static void __attribute__((unused)) wk2x01_cyrf_init() { + /* Initialize CYRF chip */ + CYRF_SetPower(0x28); + CYRF_WriteRegister(CYRF_06_RX_CFG, 0x4A); + CYRF_WriteRegister(CYRF_0B_PWR_CTRL, 0x00); + CYRF_WriteRegister(CYRF_0C_XTAL_CTRL, 0xC0); + CYRF_WriteRegister(CYRF_0D_IO_CFG, 0x04); + CYRF_WriteRegister(CYRF_0F_XACT_CFG, 0x2C); + CYRF_WriteRegister(CYRF_10_FRAMING_CFG, 0xEE); + CYRF_WriteRegister(CYRF_1B_TX_OFFSET_LSB, 0x55); + CYRF_WriteRegister(CYRF_1C_TX_OFFSET_MSB, 0x05); + CYRF_WriteRegister(CYRF_1D_MODE_OVERRIDE, 0x18); + CYRF_WriteRegister(CYRF_32_AUTO_CAL_TIME, 0x3C); + CYRF_WriteRegister(CYRF_35_AUTOCAL_OFFSET, 0x14); + CYRF_WriteRegister(CYRF_1E_RX_OVERRIDE, 0x90); + CYRF_WriteRegister(CYRF_1F_TX_OVERRIDE, 0x00); + CYRF_WriteRegister(CYRF_01_TX_LENGTH, 0x10); + CYRF_WriteRegister(CYRF_0F_XACT_CFG, 0x2C); + CYRF_WriteRegister(CYRF_28_CLK_EN, 0x02); + CYRF_WriteRegister(CYRF_27_CLK_OVERRIDE, 0x02); + CYRF_ConfigSOPCode(WK_sopcodes); + CYRF_WriteRegister(CYRF_0F_XACT_CFG, 0x28); + CYRF_WriteRegister(CYRF_1E_RX_OVERRIDE, 0x10); + CYRF_WriteRegister(CYRF_0E_GPIO_CTRL, 0x20); + CYRF_WriteRegister(CYRF_0F_XACT_CFG, 0x2C); +} + +static void __attribute__((unused)) WK_BuildPacket_2801() +{ + switch(phase) { + case WK_BIND: + bind_counter--; + WK_build_bind_pkt(init_2801); + if (bind_counter == 0) + { + BIND_DONE; + phase++; + } + break; + case WK_BOUND_1: + case WK_BOUND_2: + case WK_BOUND_3: + case WK_BOUND_4: + case WK_BOUND_5: + case WK_BOUND_6: + case WK_BOUND_7: + WK_build_data_pkt_2801(); + phase++; + break; + case WK_BOUND_8: + WK_build_beacon_pkt_2801(); + phase = WK_BOUND_1; + if (bind_counter) + { + bind_counter--; + if (bind_counter == 0) + BIND_DONE; + } + break; + } +} + +static void __attribute__((unused)) WK_BuildPacket_2601() +{ + if (bind_counter) + { + bind_counter--; + WK_build_bind_pkt(init_2601); + if (bind_counter == 0) + BIND_DONE; + } + else + WK_build_data_pkt_2601(); +} + +static void __attribute__((unused)) WK_BuildPacket_2401() +{ + if (bind_counter) + { + bind_counter--; + WK_build_bind_pkt(init_2401); + if(bind_counter == 0) + BIND_DONE; + } + else + WK_build_data_pkt_2401(); +} + +uint16_t WK_cb() +{ + if (packet_sent == 0) + { + packet_sent = 1; + if(sub_protocol == WK2801) + WK_BuildPacket_2801(); + else if(sub_protocol == WK2601) + WK_BuildPacket_2601(); + else + WK_BuildPacket_2401(); + packet_count = (packet_count + 1) % 12; + CYRF_WriteDataPacket(packet); + return 1600; + } + packet_sent = 0; + uint8_t start=micros(); + while ((uint8_t)micros()-start < 100) // Wait max 100µs + if(CYRF_ReadRegister(CYRF_04_TX_IRQ_STATUS) & 0x02) + break; + if((packet_count & 0x03) == 0) + { + hopping_frequency_no++; + hopping_frequency_no%=3; + CYRF_ConfigRFChannel(hopping_frequency[hopping_frequency_no]); + //Keep transmit power updated + CYRF_SetPower(0x28); + } + return 1200; +} + +uint16_t WK_setup() { + wk2x01_cyrf_init(); + CYRF_SetTxRxMode(TX_EN); + + hopping_frequency_no=0; + CYRF_FindBestChannels(hopping_frequency, 3, 4, 4, 80); + CYRF_ConfigRFChannel(hopping_frequency[0]); + + packet_count = 0; + packet_sent = 0; + WK_last_beacon = 0; + + CYRF_GetMfgData(cyrfmfg_id); + rx_tx_addr[2]=(hopping_frequency[0] ^ cyrfmfg_id[0] ^ cyrfmfg_id[3])<<4; + rx_tx_addr[1]=hopping_frequency[1] ^ cyrfmfg_id[1] ^ cyrfmfg_id[4]; + rx_tx_addr[0]=hopping_frequency[2] ^ cyrfmfg_id[2] ^ cyrfmfg_id[5]; + if(sub_protocol == WK2401) + rx_tx_addr[0] |= 0x01; //ID must be odd for 2401 + + bind_counter = WK_BIND_COUNT; + phase = WK_BIND; + BIND_IN_PROGRESS; + //Ignoring WK2801 bind process which is the same as Devo + return 2800; +} + +#endif diff --git a/Multiprotocol/_Config.h b/Multiprotocol/_Config.h index 7b0e61a..0486d2b 100644 --- a/Multiprotocol/_Config.h +++ b/Multiprotocol/_Config.h @@ -76,6 +76,7 @@ #define DEVO_CYRF6936_INO #define DSM_CYRF6936_INO #define J6PRO_CYRF6936_INO +#define WK2x01_CYRF6936_INO //The protocols below need a CC2500 to be installed #define FRSKYV_CC2500_INO @@ -297,6 +298,11 @@ const PPM_Parameters PPM_prot[15]= { PPM_IBUS PWM_SBUS PPM_SBUS + MODE_WK2X01 + WK2801 + WK2601 + WK2401 + */ // 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... @@ -308,6 +314,7 @@ const PPM_Parameters PPM_prot[15]= { // 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 -127 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