From 6205c4147db2e78b3138b0a9e3f6697fc9ec0c7f Mon Sep 17 00:00:00 2001
From: Cleric-K <9365881+Cleric-K@users.noreply.github.com>
Date: Fri, 10 Jan 2020 22:28:42 +0200
Subject: [PATCH] Fport support

---
 vJoySerialFeeder/MainForm.Designer.cs         |   1 +
 vJoySerialFeeder/MainForm.cs                  |   1 +
 vJoySerialFeeder/MainFormWorker.cs            |   2 +-
 .../SerialProtocols/FportReader.cs            | 182 ++++++++++++++++++
 .../SerialProtocols/SbusReader.cs             |  67 ++++---
 .../SerialProtocols/SerialReader.cs           |   4 +
 vJoySerialFeeder/vJoySerialFeeder.csproj      |   1 +
 vJoySerialFeeder/vJoySerialFeederLinux.csproj |   1 +
 8 files changed, 226 insertions(+), 33 deletions(-)
 create mode 100644 vJoySerialFeeder/SerialProtocols/FportReader.cs

diff --git a/vJoySerialFeeder/MainForm.Designer.cs b/vJoySerialFeeder/MainForm.Designer.cs
index 4db94b3..37cd5da 100644
--- a/vJoySerialFeeder/MainForm.Designer.cs
+++ b/vJoySerialFeeder/MainForm.Designer.cs
@@ -293,6 +293,7 @@ protected override void Dispose(bool disposing)
         	        	        	"KISS",
         	        	        	"MultiWii",
         	        	        	"SBUS",
+        	        	        	"FPort",
         	        	        	"DSM",
         	        	        	"Dummy"});
         	this.comboProtocol.Location = new System.Drawing.Point(360, 64);
diff --git a/vJoySerialFeeder/MainForm.cs b/vJoySerialFeeder/MainForm.cs
index 5afcf6d..c0ead2a 100644
--- a/vJoySerialFeeder/MainForm.cs
+++ b/vJoySerialFeeder/MainForm.cs
@@ -55,6 +55,7 @@ public partial class MainForm : Form
 										typeof(KissReader),
 										typeof(MultiWiiReader),
 										typeof(SbusReader),
+										typeof(FportReader),
 										typeof(DsmReader),
 										typeof(DummyReader)
 									};
diff --git a/vJoySerialFeeder/MainFormWorker.cs b/vJoySerialFeeder/MainFormWorker.cs
index b511aed..2b25d0f 100644
--- a/vJoySerialFeeder/MainFormWorker.cs
+++ b/vJoySerialFeeder/MainFormWorker.cs
@@ -212,7 +212,7 @@ void BackgroundWorkerDoWork(object sender, System.ComponentModel.DoWorkEventArgs
 					if(now >= nextUIUpdateTime) {
 						nextUIUpdateTime = now + 100;
 						
-						// update the Rate on evert 500ms
+						// update the Rate on every 500ms
 						if(now >= nextRateUpdateTime) {
 							nextRateUpdateTime = now + 500;
 							
diff --git a/vJoySerialFeeder/SerialProtocols/FportReader.cs b/vJoySerialFeeder/SerialProtocols/FportReader.cs
new file mode 100644
index 0000000..3a0afb2
--- /dev/null
+++ b/vJoySerialFeeder/SerialProtocols/FportReader.cs
@@ -0,0 +1,182 @@
+/*
+ * Created by SharpDevelop.
+ * User: Cleric
+ * Date: 28.11.2019 г.
+ * Time: 21:14 ч.
+ * 
+ */
+
+using System;
+using System.Collections;
+using System.IO.Ports;
+using System.Windows.Forms;
+
+namespace vJoySerialFeeder
+{
+	/// <summary>
+	/// 
+	/// Info at:
+	/// https://github.com/betaflight/betaflight/files/1491056/F.Port.protocol.betaFlight.V2.1.2017.11.21.pdf
+	/// </summary>
+	/// 
+	public class FportReader : SbusReader
+	{
+		private const byte BORDER_BYTE = 0x7e;
+		private const byte ESCAPE_BYTE = 0x7d;
+		private const byte ESCAPE_MASK = 0x20;
+		private const byte CONTROL_FRAME = 0x00;
+		private const byte CRC_CORRECT = 0xff;
+		private const int FRAME_LENGTH = 25; // sbus frame is always 25 bytes
+		private const int NUM_CHANNELS = 16;
+		private const int SBUS_CH_BITS = 11;
+		private const int SBUS_CH_MASK = (1<<SBUS_CH_BITS)-1;
+		private const int SBUS_FAILSAFE_MASK = 1<<3;
+		
+		private bool useRawInput;
+		private bool ignoreSbusFailsafeFlag;
+	
+		public override void Start()
+		{
+			//Buffer.FrameLength = FRAME_LENGTH;
+			serialPort.ReadTimeout = 500;
+			
+			parseConfig(config);
+		}
+		
+		public override void Stop()
+		{
+		}
+		
+		
+		public override int ReadChannels()
+		{
+			var retries = 0;
+			while(retries++ < 3) {
+				Buffer.FrameLength = 2;
+				
+				if(Buffer[0] != BORDER_BYTE) {
+					// first byte incorrect
+					Buffer.Slide(1);
+					System.Diagnostics.Debug.WriteLine("bad start - resyncing");
+					return 0;
+				}
+				
+				var data_len = Buffer[1];
+				var frame_len = Buffer.FrameLength = 2 + data_len + 2; // start + len + [dataLen] + crc + end byte
+				var full_frame_len = frame_len;
+				
+				// fix escapes
+				var tmp = new byte[frame_len*2];
+				tmp[0] = Buffer[0];
+				tmp[1] = Buffer[1];
+				for(int src = 2, dst = 2; dst < full_frame_len; src++, dst++) {
+					if(Buffer[dst] == ESCAPE_BYTE) {
+						Buffer.FrameLength++;
+						full_frame_len++;
+						tmp[dst] = Buffer[dst];
+						tmp[dst+1] = Buffer[dst+1];
+						Buffer[src] = (byte)(Buffer[++dst] ^ ESCAPE_MASK);
+						
+					}
+					else {
+						tmp[dst] = Buffer[dst];
+						Buffer[src] = Buffer[dst];
+					}
+				}
+				
+				if(Buffer[frame_len - 1] != BORDER_BYTE) {
+					// end byte incorrect
+					Buffer.Slide(1);
+					System.Diagnostics.Debug.WriteLine("bad end - resyncing");
+					return 0;
+				}
+				
+				if(Buffer[2] != CONTROL_FRAME) {
+					// probably downlink frame
+					Buffer.Slide(full_frame_len);
+					continue;
+				}
+				
+				short crc = 0;
+				for(var i=1; i < frame_len - 1; i++) {
+					crc += Buffer[i];
+				}
+				crc = (byte)((crc & 0xff) + (crc >> 8));
+				if(crc != CRC_CORRECT) {
+					System.Diagnostics.Debug.WriteLine("bad crc");
+					Buffer.Slide(full_frame_len);
+					return 0;
+				}
+
+				
+				if(!ignoreSbusFailsafeFlag && (Buffer[25]&SBUS_FAILSAFE_MASK) != 0) {
+					Buffer.Slide(full_frame_len);
+					throw new FailsafeException("SBUS Failsafe active");
+				}
+
+				// Fport uses the same channel format as Sbus				
+				DecodeSbusChannels(3);
+				
+				// do not check the flags byte, we don't really need anything from there	
+				Buffer.Slide(full_frame_len);
+				return NUM_CHANNELS;
+				
+			}
+			return 0;
+		}
+		
+		public override bool Configurable {
+			get { return true; }
+		}
+		
+		public override string Configure(string config)
+		{
+			parseConfig(config);
+			
+			using(var d = new SbusSetupForm(useRawInput, ignoreSbusFailsafeFlag)) {
+				d.ShowDialog();
+				if(d.DialogResult == DialogResult.OK) {
+					useRawInput = d.UseRawInput;
+					ignoreSbusFailsafeFlag = d.IgnoreSbusFailsafeFlag;
+					return buildConfig();
+				}
+					
+				return null;
+			}
+		}
+		
+		public override Configuration.SerialParameters GetDefaultSerialParameters()
+		{
+			return new Configuration.SerialParameters() {
+				BaudRate = 115200,
+				DataBits = 8,
+				Parity = Parity.None,
+				StopBits = StopBits.One
+			};
+		}
+		
+		private void parseConfig(string config) {
+			var tokens = config == null ?
+					new string[0]
+					:
+					config.Split(',');
+			
+			foreach(var s in tokens) {
+				if(s == "raw")
+					useRawInput = true;
+				else if(s == "failsafe")
+					ignoreSbusFailsafeFlag = true;
+			}
+		}
+		
+		private string buildConfig() {
+			var cfg = new ArrayList();
+			if(useRawInput)
+				cfg.Add("raw");
+			if(ignoreSbusFailsafeFlag)
+				cfg.Add("failsafe");
+			
+			return string.Join(",", cfg.ToArray());
+		}
+	}
+}
diff --git a/vJoySerialFeeder/SerialProtocols/SbusReader.cs b/vJoySerialFeeder/SerialProtocols/SbusReader.cs
index 05b14e5..2733459 100644
--- a/vJoySerialFeeder/SerialProtocols/SbusReader.cs
+++ b/vJoySerialFeeder/SerialProtocols/SbusReader.cs
@@ -81,38 +81,7 @@ public override int ReadChannels()
 				throw new FailsafeException("SBUS Failsafe active");
 			}
 			
-			int inputbits = 0;
-			int inputbitsavailable = 0;
-			int bufIdx = 1;
-			
-			// channel parser based on
-			// https://github.com/opentx/opentx/blob/6bd38ce13a89ade70aa8e83914063464a8d9750a/radio/src/sbus.cpp#L50
-			
-			for (var i=0; i<NUM_CHANNELS; i++) {
-			    while (inputbitsavailable < SBUS_CH_BITS) {
-		      		inputbits |= Buffer[bufIdx++] << inputbitsavailable;
-		      		inputbitsavailable += 8;
-			    }
-			
-				var v = inputbits & SBUS_CH_MASK;
-				
-				if(useRawInput) {
-					channelData[i] = v;
-				}
-				else {
-					// OpenTX sends channel data with in its own values. We can prescale them to the standard 1000 - 2000 range.
-					// Thanks to @fape for providing the raw data:
-					// min   mid   max
-					// 172   992   1811
-	
-					// http://www.wolframalpha.com/input/?i=linear+fit+%7B172,+1000%7D,+%7B1811,+2000%7D,+%7B992,+1500%7D
-					// slightly adjusted to give better results in integer math
-					channelData[i] = (610127*v + 895364000)/1000000;
-				}
-
-			    inputbitsavailable -= SBUS_CH_BITS;
-			    inputbits >>= SBUS_CH_BITS;
-			}	
+			DecodeSbusChannels(1);
 			
 			// do not check the flags byte, we don't really need anything from there	
 
@@ -149,6 +118,40 @@ public override Configuration.SerialParameters GetDefaultSerialParameters()
 			};
 		}
 		
+		protected void DecodeSbusChannels(int bufIdx) {
+			int inputbits = 0;
+			int inputbitsavailable = 0;
+			
+			// channel parser based on
+			// https://github.com/opentx/opentx/blob/6bd38ce13a89ade70aa8e83914063464a8d9750a/radio/src/sbus.cpp#L50
+			
+			for (var i=0; i<NUM_CHANNELS; i++) {
+			    while (inputbitsavailable < SBUS_CH_BITS) {
+		      		inputbits |= Buffer[bufIdx++] << inputbitsavailable;
+		      		inputbitsavailable += 8;
+			    }
+			
+				var v = inputbits & SBUS_CH_MASK;
+				
+				if(useRawInput) {
+					channelData[i] = v;
+				}
+				else {
+					// OpenTX sends channel data with in its own values. We can prescale them to the standard 1000 - 2000 range.
+					// Thanks to @fape for providing the raw data:
+					// min   mid   max
+					// 172   992   1811
+	
+					// http://www.wolframalpha.com/input/?i=linear+fit+%7B172,+1000%7D,+%7B1811,+2000%7D,+%7B992,+1500%7D
+					// slightly adjusted to give better results in integer math
+					channelData[i] = (610127*v + 895364000)/1000000;
+				}
+
+			    inputbitsavailable -= SBUS_CH_BITS;
+			    inputbits >>= SBUS_CH_BITS;
+			}	
+		}
+		
 		private void parseConfig(string config) {
 			var tokens = config == null ?
 					new string[0]
diff --git a/vJoySerialFeeder/SerialProtocols/SerialReader.cs b/vJoySerialFeeder/SerialProtocols/SerialReader.cs
index 6fa8252..6fb8c4b 100644
--- a/vJoySerialFeeder/SerialProtocols/SerialReader.cs
+++ b/vJoySerialFeeder/SerialProtocols/SerialReader.cs
@@ -66,6 +66,10 @@ public class SerialBuffer {
 					}
 					return buf[index];
 				}
+				
+				set {
+					buf[index] = value;
+				}
 			}
 			
 			/// <summary>
diff --git a/vJoySerialFeeder/vJoySerialFeeder.csproj b/vJoySerialFeeder/vJoySerialFeeder.csproj
index ff9d273..2fa96a9 100644
--- a/vJoySerialFeeder/vJoySerialFeeder.csproj
+++ b/vJoySerialFeeder/vJoySerialFeeder.csproj
@@ -134,6 +134,7 @@
     <Compile Include="SerialProtocols\DummySetupForm.Designer.cs">
       <DependentUpon>DummySetupForm.cs</DependentUpon>
     </Compile>
+    <Compile Include="SerialProtocols\FportReader.cs" />
     <Compile Include="SerialProtocols\IbusReader.cs" />
     <Compile Include="SerialProtocols\IbusSetupForm.cs" />
     <Compile Include="SerialProtocols\IbusSetupForm.Designer.cs">
diff --git a/vJoySerialFeeder/vJoySerialFeederLinux.csproj b/vJoySerialFeeder/vJoySerialFeederLinux.csproj
index d14ad74..799c08a 100644
--- a/vJoySerialFeeder/vJoySerialFeederLinux.csproj
+++ b/vJoySerialFeeder/vJoySerialFeederLinux.csproj
@@ -129,6 +129,7 @@
     <Compile Include="SerialProtocols\DummySetupForm.Designer.cs">
       <DependentUpon>DummySetupForm.cs</DependentUpon>
     </Compile>
+    <Compile Include="SerialProtocols\FportReader.cs" />
     <Compile Include="SerialProtocols\IbusReader.cs" />
     <Compile Include="SerialProtocols\IbusSetupForm.cs" />
     <Compile Include="SerialProtocols\IbusSetupForm.Designer.cs">