From 8677e73d75021f5264f89255006c1a8b7c8dfca8 Mon Sep 17 00:00:00 2001
From: pascallanger <planger@mathworks.com>
Date: Tue, 3 Jan 2017 19:56:20 +0100
Subject: [PATCH] WK2x01: subprotocols addition

WK2801 number 0, 8 channels, fixed id is not supported
WK2401 number 1, 4 channels
W6_5_1 number 2, 6 channels
W6_6_1 number 3, 7 channels
W6_HEL number 4, 6 channels, option is used to limit COL
W6_HEL_I number 5, 6 channels, COL inverted, option is used to limit COL
---
 Multiprotocol/Multi.txt           |  2 +-
 Multiprotocol/Multiprotocol.h     | 14 ++++++++++----
 Multiprotocol/WK2x01_cyrf6936.ino | 18 +++++++++---------
 Multiprotocol/_Config.h           |  6 ++++--
 4 files changed, 24 insertions(+), 16 deletions(-)

diff --git a/Multiprotocol/Multi.txt b/Multiprotocol/Multi.txt
index a18b8df..7c5bd88 100644
--- a/Multiprotocol/Multi.txt
+++ b/Multiprotocol/Multi.txt
@@ -27,4 +27,4 @@
 27,OpnLrs
 28,AFHD2SA,PWM_IBUS,PPM_IBUS,PWM_SBUS,PPM_SBUS
 29,Q2X2,Q222,Q242,Q282
-30,WK2X01,WK2801,WK2601,WK2401
+30,WK2x01,WK2801,WK2401,W6_5_1,W6_6_1,W6_HEL,W6_HEL_I
diff --git a/Multiprotocol/Multiprotocol.h b/Multiprotocol/Multiprotocol.h
index 3c9c8ef..f0c4bb7 100644
--- a/Multiprotocol/Multiprotocol.h
+++ b/Multiprotocol/Multiprotocol.h
@@ -178,8 +178,11 @@ enum FY326
 enum WK2x01
 {
 	WK2801	= 0,
-	WK2601	= 1,
-	WK2401	= 2,
+	WK2401	= 1,
+	W6_5_1	= 2,
+	W6_6_1	= 3,
+	W6_HEL	= 4,
+	W6_HEL_I= 5,
 };
 
 #define NONE 		0
@@ -545,8 +548,11 @@ Serial: 100000 Baud 8e2      _ xxxx xxxx p --
 			FY319		1
 		sub_protocol==WK2x01
 			WK2801		0
-			WK2601		1
-			WK2401		2
+			WK2401		1
+			W6_5_1		2
+			W6_6_1		3
+			W6_HEL		4
+			W6_HEL_I	5
 
    Power value => 0x80	0=High/1=Low
   Stream[3]   = option_protocol;
diff --git a/Multiprotocol/WK2x01_cyrf6936.ino b/Multiprotocol/WK2x01_cyrf6936.ino
index 1136bb5..f443bbf 100644
--- a/Multiprotocol/WK2x01_cyrf6936.ino
+++ b/Multiprotocol/WK2x01_cyrf6936.ino
@@ -201,7 +201,7 @@ static void __attribute__((unused)) WK_channels_heli_2601(uint8_t frame, int16_t
 	//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)
+	if (sub_protocol==W6_HEL_I)
 		pit_rev = 1;
 	int16_t pit_rate = WK_get_channel(5, 0x400, 0, 0x400);
 	uint8_t fmode = 1;
@@ -214,7 +214,7 @@ static void __attribute__((unused)) WK_channels_heli_2601(uint8_t frame, int16_t
 	{
 		//Pitch curve and range
 		*v1 = pit_rate;
-		*v2 = ((option/100) ? -100 : 100) * 0x400 / 100 + 0x400;
+		*v2 = (int16_t)(option) * 0x400 / 100 + 0x400;
 	}
 	packet[7] = (pit_rev << 2);		//reverse bits
 	packet[8] = fmode ? 0x02 : 0x00;
@@ -239,12 +239,12 @@ static void __attribute__((unused)) WK_build_data_pkt_2601()
 		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)
+	if (sub_protocol == W6_5_1)
+		WK_channels_5plus1_2601(frame, &v1, &v2);
+	else if (sub_protocol == W6_6_1)
 		WK_channels_6plus1_2601(frame, &v1, &v2);
 	else
-		WK_channels_5plus1_2601(frame, &v1, &v2);
+		WK_channels_heli_2601(frame, &v1, &v2);
 	if (v1 > 1023)
 		v1 = 1023;
 	if (v2 > 1023)
@@ -404,10 +404,10 @@ uint16_t WK_cb()
 		packet_sent = 1;
 		if(sub_protocol == WK2801)
 			WK_BuildPacket_2801();
-		else if(sub_protocol == WK2601)
-			WK_BuildPacket_2601();
-		else
+		else if(sub_protocol == WK2401)
 			WK_BuildPacket_2401();
+		else
+			WK_BuildPacket_2601();
 		packet_count = (packet_count + 1) % 12;
 		CYRF_WriteDataPacket(packet);
 		return 1600;
diff --git a/Multiprotocol/_Config.h b/Multiprotocol/_Config.h
index 0486d2b..8951480 100644
--- a/Multiprotocol/_Config.h
+++ b/Multiprotocol/_Config.h
@@ -300,9 +300,11 @@ const PPM_Parameters PPM_prot[15]=	{
 		PPM_SBUS
 	MODE_WK2X01
 		WK2801
-		WK2601
 		WK2401
-		
+		W6_5_1
+		W6_6_1
+		W6_HEL
+		W6_HEL_I
 */
 
 // 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...