Skip to content

Commit

Permalink
Fport support
Browse files Browse the repository at this point in the history
  • Loading branch information
@Cleric-K
Cleric-K committed Jan 10, 2020
1 parent 9ffb202 commit 6205c41
Show file tree
Hide file tree
Showing 8 changed files with 226 additions and 33 deletions.
1 change: 1 addition & 0 deletions vJoySerialFeeder/MainForm.Designer.cs
View file

Some generated files are not rendered by default. Learn more about how customized files appear on GitHub.

1 change: 1 addition & 0 deletions vJoySerialFeeder/MainForm.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -55,6 +55,7 @@ public partial class MainForm : Form
typeof(KissReader),
typeof(MultiWiiReader),
typeof(SbusReader),
typeof(FportReader),
typeof(DsmReader),
typeof(DummyReader)
};
Expand Down
2 changes: 1 addition & 1 deletion vJoySerialFeeder/MainFormWorker.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;

Expand Down
182 changes: 182 additions & 0 deletions vJoySerialFeeder/SerialProtocols/FportReader.cs
View file
Original file line number Diff line number Diff line change
@@ -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());
}
}
}
67 changes: 35 additions & 32 deletions vJoySerialFeeder/SerialProtocols/SbusReader.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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

Expand Down Expand Up @@ -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]
Expand Down
4 changes: 4 additions & 0 deletions vJoySerialFeeder/SerialProtocols/SerialReader.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -66,6 +66,10 @@ public class SerialBuffer {
}
return buf[index];
}

set {
buf[index] = value;
}
}

/// <summary>
Expand Down
1 change: 1 addition & 0 deletions vJoySerialFeeder/vJoySerialFeeder.csproj
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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">
Expand Down
1 change: 1 addition & 0 deletions vJoySerialFeeder/vJoySerialFeederLinux.csproj
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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">
Expand Down

0 comments on commit 6205c41

Please sign in to comment.