protected virtual void Dispose(bool disposing)
{
if (!disposedValue)
{
if (disposing)
{
readCancelTokenSource?.Cancel();
port?.DiscardInBuffer();
port?.DiscardOutBuffer();
port?.Close();
port?.Dispose();
}
disposedValue = true;
}
}
public string ExecCommand(SerialPort port, string command, int responseTimeout, string errorMessage)
{
try
{
port.DiscardOutBuffer();
port.DiscardInBuffer();
port.Write(command + "\r");
return "Success";
}
catch (Exception ex)
{
throw ex;
}
}
public void request_DoWork(object sender, DoWorkEventArgs e)
{
var worker = (BackgroundWorker)sender;
worker.ReportProgress(0);
ClsComSettingMain clsComSettingMain = (ClsComSettingMain)e.Argument;
string comPort = clsComSettingMain.comport;
int baudRate = clsComSettingMain.baudRate;
if (comPort != null && baudRate != 0)
{
SerialPort serialPort = new SerialPort(comPort, baudRate);
serialPort.DataReceived += new SerialDataReceivedEventHandler(process_DoWorkHandler);
serialPort.Open();
while (true)
{
if (worker.CancellationPending)
{
worker.ReportProgress(100);
e.Cancel = true;
break;
}
else
{
serialPort.DiscardInBuffer(); // Eingangspuffer leeren
serialPort.Write(adv_request, 0, 3); // Write byte array to serial port, with no offset, all 3 bytes
}
}
}
}
internal void ClearBuffers()
{
_port?.DiscardInBuffer();
_port?.DiscardOutBuffer();
}
private void BUT_getcurrent_Click(object sender, EventArgs e)
{
ArdupilotMega.Comms.ICommsSerial comPort = new SerialPort();
try
{
comPort.PortName = MainV2.comPort.BaseStream.PortName;
comPort.BaudRate = MainV2.comPort.BaseStream.BaudRate;
comPort.ReadTimeout = 4000;
comPort.Open();
}
catch { CustomMessageBox.Show("Invalid ComPort or in use"); return; }
lbl_status.Text = "Connecting";
if (doConnect(comPort))
{
// cleanup
doCommand(comPort, "AT&T");
comPort.DiscardInBuffer();
lbl_status.Text = "Doing Command ATI & RTI";
ATI.Text = doCommand(comPort, "ATI");
RTI.Text = doCommand(comPort, "RTI");
uploader.Uploader.Code freq = (uploader.Uploader.Code)Enum.Parse(typeof(uploader.Uploader.Code), doCommand(comPort, "ATI3"));
uploader.Uploader.Code board = (uploader.Uploader.Code)Enum.Parse(typeof(uploader.Uploader.Code), doCommand(comPort, "ATI2"));
ATI3.Text = freq.ToString();
// 8 and 9
if (freq == uploader.Uploader.Code.FREQ_915) {
S8.DataSource = Range(895000, 1000, 935000);
RS8.DataSource = Range(895000, 1000, 935000);
S9.DataSource = Range(895000, 1000, 935000);
RS9.DataSource = Range(895000, 1000, 935000);
}
else if (freq == uploader.Uploader.Code.FREQ_433)
{
S8.DataSource = Range(414000, 100, 454000);
RS8.DataSource = Range(414000, 100, 454000);
S9.DataSource = Range(414000, 100, 454000);
RS9.DataSource = Range(414000, 100, 454000);
}
else if (freq == uploader.Uploader.Code.FREQ_868)
{
S8.DataSource = Range(849000, 1000, 889000);
RS8.DataSource = Range(849000, 1000, 889000);
S9.DataSource = Range(849000, 1000, 889000);
RS9.DataSource = Range(849000, 1000, 889000);
}
if (board == uploader.Uploader.Code.DEVICE_ID_RFD900 || board == uploader.Uploader.Code.DEVICE_ID_RFD900A)
{
S4.DataSource = Range(1, 1, 30);
RS4.DataSource = Range(1, 1, 30);
}
RSSI.Text = doCommand(comPort, "ATI7").Trim();
lbl_status.Text = "Doing Command ATI5";
string answer = doCommand(comPort, "ATI5");
string[] items = answer.Split('\n');
foreach (string item in items)
{
if (item.StartsWith("S"))
{
string[] values = item.Split(':', '=');
if (values.Length == 3)
{
Control[] controls = this.Controls.Find(values[0].Trim(), true);
if (controls.Length > 0)
{
controls[0].Enabled = true;
if (controls[0].GetType() == typeof(CheckBox))
{
((CheckBox)controls[0]).Checked = values[2].Trim() == "1";
}
else
{
controls[0].Text = values[2].Trim();
}
}
}
}
}
// remote
foreach (Control ctl in groupBox2.Controls)
{
if (ctl.Name.StartsWith("RS") && ctl.Name != "RSSI")
ctl.ResetText();
}
comPort.DiscardInBuffer();
lbl_status.Text = "Doing Command RTI5";
answer = doCommand(comPort, "RTI5");
items = answer.Split('\n');
foreach (string item in items)
{
if (item.StartsWith("S"))
{
string[] values = item.Split(':', '=');
if (values.Length == 3)
{
Control[] controls = this.Controls.Find("R" + values[0].Trim(), true);
if (controls.Length == 0)
continue;
controls[0].Enabled = true;
if (controls[0].GetType() == typeof(CheckBox))
{
((CheckBox)controls[0]).Checked = values[2].Trim() == "1";
}
else if (controls[0].GetType() == typeof(TextBox))
{
((TextBox)controls[0]).Text = values[2].Trim();
}
else if (controls[0].GetType() == typeof(ComboBox))
{
((ComboBox)controls[0]).Text = values[2].Trim();
}
}
else
{
log.Info("Odd config line :" + item);
}
}
}
// off hook
doCommand(comPort, "ATO");
lbl_status.Text = "Done";
}
else
{
// off hook
doCommand(comPort, "ATO");
lbl_status.Text = "Fail";
CustomMessageBox.Show("Failed to enter command mode");
}
comPort.Close();
BUT_Syncoptions.Enabled = true;
BUT_savesettings.Enabled = true;
}
public override void DiscardInBuffer()
{
port?.DiscardInBuffer();
}
private void cleanConnection()
{
serialPort.DiscardInBuffer();
}
public static void Main(string[] args)
{
Log("{json:scada} IEC60870-5-101 Driver - Copyright 2020 RLO");
Log("Driver version " + DriverVersion);
Log("Using lib60870.NET version " +
LibraryCommon.GetLibraryVersionString());
if (args.Length > 0) // first argument in number of the driver instance
{
int num;
bool res = int.TryParse(args[0], out num);
if (res)
{
ProtocolDriverInstanceNumber = num;
}
}
if (args.Length > 1) // second argument is logLevel
{
int num;
bool res = int.TryParse(args[1], out num);
if (res)
{
LogLevel = num;
}
}
string fname = JsonConfigFilePath;
if (args.Length > 2) // third argument is config file name
{
if (File.Exists(args[2]))
{
fname = args[2];
}
}
if (!File.Exists(fname))
{
fname = JsonConfigFilePathAlt;
}
if (!File.Exists(fname))
{
Log("Missing config file " + JsonConfigFilePath);
Environment.Exit(-1);
}
Log("Reading config file " + fname);
string json = File.ReadAllText(fname);
JSConfig = JsonSerializer.Deserialize <JSONSCADAConfig>(json);
if (
JSConfig.mongoConnectionString == "" ||
JSConfig.mongoConnectionString == null
)
{
Log("Missing MongoDB connection string in JSON config file " +
fname);
Environment.Exit(-1);
}
// Log("MongoDB connection string: " + JSConfig.mongoConnectionString);
if (
JSConfig.mongoDatabaseName == "" ||
JSConfig.mongoDatabaseName == null
)
{
Log("Missing MongoDB database name in JSON config file " +
fname);
Environment.Exit(-1);
}
Log("MongoDB database name: " + JSConfig.mongoDatabaseName);
if (JSConfig.nodeName == "" || JSConfig.nodeName == null)
{
Log("Missing nodeName parameter in JSON config file " +
fname);
Environment.Exit(-1);
}
Log("Node name: " + JSConfig.nodeName);
// connect to MongoDB Database server
var Client = ConnectMongoClient(JSConfig);
var DB = Client.GetDatabase(JSConfig.mongoDatabaseName);
// read and process instances configuration
var collinsts =
DB
.GetCollection
<protocolDriverInstancesClass
>(ProtocolDriverInstancesCollectionName);
var instances =
collinsts
.Find(inst =>
inst.protocolDriver == ProtocolDriverName &&
inst.protocolDriverInstanceNumber ==
ProtocolDriverInstanceNumber &&
inst.enabled == true)
.ToList();
var foundInstance = false;
foreach (protocolDriverInstancesClass inst in instances)
{
if (
ProtocolDriverName == inst.protocolDriver &&
ProtocolDriverInstanceNumber ==
inst.protocolDriverInstanceNumber
)
{
foundInstance = true;
if (!inst.enabled)
{
Log("Driver instance [" +
ProtocolDriverInstanceNumber.ToString() +
"] disabled!");
Environment.Exit(-1);
}
Log("Instance: " +
inst.protocolDriverInstanceNumber.ToString());
var nodefound = false;
foreach (var name in inst.nodeNames)
{
if (JSConfig.nodeName == name)
{
nodefound = true;
}
}
if (!nodefound)
{
Log("Node '" +
JSConfig.nodeName +
"' not found in instances configuration!");
Environment.Exit(-1);
}
DriverInstance = inst;
break;
}
break; // process just first result
}
if (!foundInstance)
{
Log("Driver instance [" +
ProtocolDriverInstanceNumber +
"] not found in configuration!");
Environment.Exit(-1);
}
// read and process connections configuration for this driver instance
var collconns =
DB
.GetCollection
<IEC10X_connection>(ProtocolConnectionsCollectionName);
var conns =
collconns
.Find(conn =>
conn.protocolDriver == ProtocolDriverName &&
conn.protocolDriverInstanceNumber ==
ProtocolDriverInstanceNumber &&
conn.enabled == true)
.ToList();
foreach (IEC10X_connection isrv in conns)
{
IEC10Xconns.Add(isrv);
Log(isrv.name.ToString() + " - New Connection");
}
if (IEC10Xconns.Count == 0)
{
Log("No connections found!");
Environment.Exit(-1);
}
// start thread to process redundancy control
Thread thrMongoRedundacy =
new Thread(() =>
ProcessRedundancyMongo(JSConfig));
thrMongoRedundacy.Start();
// start thread to update acquired data to database
Thread thrMongo =
new Thread(() =>
ProcessMongo(JSConfig));
thrMongo.Start();
// start thread to watch for commands in the database using a change stream
Thread thrMongoCmd =
new Thread(() =>
ProcessMongoCmd(JSConfig));
thrMongoCmd.Start();
Log("Setting up IEC Connections & ASDU handlers...");
int cntIecSrv = 0;
foreach (IEC10X_connection srv in IEC10Xconns)
{
TcpClientVirtualSerialPort virtualPort = null;
SerialPort port = null;
if (srv.portName.Contains(":"))
{
var hostport = srv.portName.Split(":");
virtualPort = new TcpClientVirtualSerialPort(hostport[0], System.Convert.ToInt32(hostport[1]));
if (LogLevel >= LogLevelDebug)
{
virtualPort.DebugOutput = true;
}
virtualPort.Start();
}
else
{
port = new SerialPort();
port.PortName = srv.portName;
port.BaudRate = srv.baudRate;
switch (srv.parity.ToLower())
{
default: // Even is the starndard parity for 101
case "even":
port.Parity = Parity.Even;
break;
case "none":
port.Parity = Parity.None;
break;
case "odd":
port.Parity = Parity.Odd;
break;
case "mark":
port.Parity = Parity.Mark;
break;
case "space":
port.Parity = Parity.Space;
break;
}
switch (srv.stopBits.ToLower())
{
default:
case "one":
port.StopBits = StopBits.One;
break;
case "one5":
case "onepointfive":
port.StopBits = StopBits.OnePointFive;
break;
case "two":
port.StopBits = StopBits.Two;
break;
}
switch (srv.handshake.ToLower())
{
default:
case "none":
port.Handshake = Handshake.None;
break;
case "xon":
case "xonxoff":
port.Handshake = Handshake.XOnXOff;
break;
case "rts":
case "requesttosend":
port.Handshake = Handshake.RequestToSend;
break;
case "rtsxon":
case "requesttosendxonxoff":
port.Handshake = Handshake.RequestToSendXOnXOff;
break;
}
port.Open();
port.DiscardInBuffer();
}
LinkLayerParameters llParameters = new LinkLayerParameters();
llParameters.AddressLength = srv.sizeOfLinkAddress;
llParameters.TimeoutForACK = srv.timeoutForACK;
llParameters.TimeoutRepeat = srv.timeoutRepeat;
llParameters.UseSingleCharACK = srv.useSingleCharACK;
ApplicationLayerParameters alpars = new ApplicationLayerParameters();
alpars.SizeOfCOT = srv.sizeOfCOT;
alpars.SizeOfCA = srv.sizeOfCA;
alpars.SizeOfIOA = srv.sizeOfIOA;
alpars.OA = srv.localLinkAddress;
CS101Master master;
if (port != null)
{
Log("Serial Port: " + srv.portName);
master =
new CS101Master(port,
LinkLayerMode.UNBALANCED,
llParameters,
alpars);
}
else
{
Log("Virtual Serial Port: " + srv.portName);
master =
new CS101Master(virtualPort,
LinkLayerMode.UNBALANCED,
llParameters,
alpars);
}
// master.OwnAddress = srv.localLinkAddress;
master.SetTimeouts(srv.timeoutMessage, srv.timeoutCharacter);
master.AddSlave(srv.remoteLinkAddress);
master.SlaveAddress = srv.remoteLinkAddress;
srv.master = master;
srv.CntGI = srv.giInterval - 5;
srv.CntTestCommand = srv.testCommandInterval - 2;
srv.CntTimeSync = srv.timeSyncInterval;
if (LogLevel >= LogLevelDebug)
{
master.DebugOutput = true;
}
master.SetASDUReceivedHandler(AsduReceivedHandlerPre, cntIecSrv);
master.SetLinkLayerStateChangedHandler(linkLayerStateChanged, cntIecSrv);
master.SetReceivedRawMessageHandler(rcvdRawMessageHandler, cntIecSrv);
master.Start();
// create timer to increment counters each second
srv.TimerCnt = new System.Timers.Timer();
srv.TimerCnt.Interval = 1000;
srv.TimerCnt.Elapsed += (sender, e) => MyElapsedMethod(sender, e, srv);
private void DataCatch2(object sender, SerialDataReceivedEventArgs e)
{
try
{
mControl.공용함수.timedelay(30);
int Length = SerialPort2.BytesToRead;
AdReadTimeFirst2 = mControl.공용함수.timeGetTimems();
AdReadTimeLast2 = mControl.공용함수.timeGetTimems();
if (Length == 0)
{
SerialPort2.DiscardInBuffer();
return;
}
byte[] buffer = new byte[Length + 10];
SerialPort2.Read(buffer, 0, Length);
if (13 < Length)
{
Length = 13;
}
if ((0 < buffer[0]) && (buffer[1] == READ_COMMAND) && (0 < buffer[2]))
{
float Value;
short Value1;
short Value2;
int Point;
float Point1;
//ushort Data;
ushort crc16;
byte crc16_high, crc16_low;
unchecked //overflow 검출하지 않는다. 여산중 overflow가 발생하더라도 무시한다.
{
crc16 = CRC16(buffer, Length - 2);
crc16_high = (byte)((crc16 >> 0) & 0x00ff);
crc16_low = (byte)((crc16 >> 8) & 0x00ff);
if (crc16_high == buffer[Length - 2] && crc16_low == buffer[Length - 1])
{
Value1 = (short)((buffer[3] << 8) & 0xff00);
Value2 = (short)((buffer[4] << 0) & 0x00ff);
Value = Value1 | Value2;
Point = (int)(((byte)buffer[6] << 0) & 0x00ff);
if (Point <= 7)
{
Point1 = (float)(Math.Pow(10.0, (double)Point));
if ((0 < Point1) && (Point1 < 10000.0))
{
Value = (float)Value / Point1;
}
else
{
Value = (float)0.0;
}
}
else
{
Value = (float)0.0;
}
//if (mControl.GetConfig.CURR_ID == buffer[0])
// Curr = Value;
//else if (mControl.GetConfig.PSEAT_ID == buffer[0])
// PSeat = Value;
PanelMeterReadPos2++;
if (1 < PanelMeterReadPos2)
{
PanelMeterReadPos2 = 0;
}
MessageDisplayFlag2 = false;
}
}
return;
}
SerialPort2.DiscardInBuffer();
}
catch
{
}
finally
{
}
return;
}
public void DiscardInBuffer()
{
serialPort.DiscardInBuffer();
}
public void PurgeQueue()
{
serialPort.DiscardInBuffer();
serialPort.DiscardOutBuffer();
Reply = "";
}
private void VerirfyXOnXOffBufferFull(SerialPort com1, SerialPort com2)
{
int com1BaudRate = com1.BaudRate;
int com2BaudRate = com2.BaudRate;
int com1ReadBufferSize = com1.ReadBufferSize;
bool com2RtsEnable = com2.RtsEnable;
int bufferSize = com1.ReadBufferSize;
int upperLimit = bufferSize - 1024;
int lowerLimit = 1024;
byte[] bytes = new byte[upperLimit];
int byteRead;
try
{
//Set the BaudRate to something faster so that it does not take so long to fill up the buffer
com1.BaudRate = 115200;
com2.BaudRate = 115200;
if (com1.Handshake == Handshake.RequestToSendXOnXOff)
{
com2.RtsEnable = true;
com1.Write("foo");
Thread.Sleep(DEFAULT_WAIT_AFTER_READ_OR_WRITE);
com2.DiscardInBuffer();
}
//Write 1 less byte then when the XOff character would be sent
com2.Write(bytes, 0, upperLimit - 1);
Thread.Sleep(DEFAULT_WAIT_AFTER_READ_OR_WRITE);
if (IsXOnXOff(com1))
{
if (com2.BytesToRead != 0)
{
Fail("Err_81919aniee Did not expect anything to be sent");
}
}
else
{
if (com2.BytesToRead != 0)
{
Fail("Err_5258aieodpo Did not expect anything to be sent com2.BytesToRead={0}", com2.BytesToRead);
}
}
//Write the byte and verify the XOff character was sent as appropriately
com2.Write(bytes, 0, 1);
Thread.Sleep(DEFAULT_WAIT_AFTER_READ_OR_WRITE);
if (IsXOnXOff(com1))
{
if (com2.BytesToRead != 1)
{
Fail("Err_12558aoed Expected XOff to be sent and nothing was sent");
}
else if (XOnOff.XOFF != (byteRead = com2.ReadByte()))
{
Fail("Err_0188598aoepad Expected XOff to be sent actually sent={0}", byteRead);
}
}
else
{
if (com2.BytesToRead != 0)
{
Fail("Err_2258ajoe Did not expect anything to be sent");
}
}
//Read 1 less byte then when the XOn char would be sent
com1.Read(bytes, 0, (upperLimit - lowerLimit) - 1);
Thread.Sleep(DEFAULT_WAIT_AFTER_READ_OR_WRITE);
if (IsXOnXOff(com1))
{
if (com2.BytesToRead != 0)
{
Fail("Err_22808aiuepa Did not expect anything to be sent");
}
}
else
{
if (com2.BytesToRead != 0)
{
Fail("Err_12508aieap Did not expect anything to be sent");
}
}
//Read the byte and verify the XOn char is sent as appropriately
com1.Read(bytes, 0, 1);
Thread.Sleep(DEFAULT_WAIT_AFTER_READ_OR_WRITE);
if (IsXOnXOff(com1))
{
if (com2.BytesToRead != 1)
{
Fail("Err_6887518adizpa Expected XOn to be sent and nothing was sent");
}
else if (XOnOff.XON != (byteRead = com2.ReadByte()))
{
Fail("Err_58145auead Expected XOn to be sent actually sent={0}", byteRead);
}
}
else
{
if (com2.BytesToRead != 0)
{
Fail("Err_256108aipeg Did not expect anything to be sent");
}
}
}
finally
{
//Rollback any changed that were made to the SerialPort
com1.BaudRate = com1BaudRate;
com2.BaudRate = com2BaudRate;
com1.DiscardInBuffer();
com2.DiscardInBuffer();
com2.RtsEnable = com2RtsEnable;
}
}
///<summary>
/// 设置UT351
/// </summary>
/// <param name="ZD">栈号</param>
/// <param name="CMD">命令字符 SV:设定值 BS:误差值 OUT:输出值 MOD:模式(1、手动)</param>
/// <param name="ZValue">设置的值</param>
/// <param name="SDP">小数位数</param>
private void Set(int ZD, string CMD, double ZValue, int SDP)
{
// SH:最大值 SL:最小值 DR:方向 OUT:输入值 MOD:模式(1、手动)
lock (_lock)
{
#region UT351
char STX, ETX, CR, LF;
string ZDD, ZB;
STX = (char)2;
ETX = (char)3;
CR = (char)13;
LF = (char)10;
//ESC = (char)27;
ZDD = string.Format("{0:00}", (ZD % 100));
int temp = Convert.ToInt32(ZValue * Math.Pow(10, SDP));
temp = temp < 0 ? temp + 65536 : temp;
ZB = string.Format("{0:X}", temp);
ZB = ZB.Length > 4 ? ZB.Substring(ZB.Length - 4, 4) : ZB.PadLeft(4, '0');
switch (CMD)
{
case "SV":
ZB = "WWRD2101,01," + ZB;
break;
//case "SH":
// ZB = "WWRD5104,01," + ZB;
// break;
//case "SL":
// ZB = "WWRD5105,01," + ZB;
// break;
case "DR": //方向 0,1
ZB = "WWRD0257,01," + ZB;
break;
case "BS":
ZB = "WWRD2901,01," + ZB;
break;
case "OUT": //1位小数，手动模式下
ZB = "WWRD0217,01," + ZB;
break;
case "MOD": //1手动0自动
ZB = "WWRD0201,01," + ZB;
break;
case "P": //1位小数，和设定值小数位没有关系
ZB = "WWRD0306,01," + ZB;
break;
case "I": //1位小数，和设定值小数位没有关系
ZB = "WWRD0307,01," + ZB;
break;
case "D": //1位小数，和设定值小数位没有关系
ZB = "WWRD0308,01," + ZB;
break;
case "SDP": //仪表显示小数位温度2位，流量1位，设定小数位后再设定最大最小值
ZB = "WWRD1206,01," + ZB;
break;
case "SH": //最大值
ZB = "WWRD1207,01," + ZB;
break;
case "SL": //最小值
ZB = "WWRD1208,01," + ZB;
break;
}
ZB = STX + ZDD + "010" + ZB + ETX + CR + LF;
if (!_port.IsOpen)
{
_port.Open();
}
_port.DiscardInBuffer();
_port.DiscardOutBuffer();
_port.Write(ZB);
int startTick = Environment.TickCount;
while (_port.BytesToRead < 9 && Environment.TickCount - startTick < 150)
{
System.Threading.Thread.Sleep(1);
}
_port.Close();
#endregion
}
}
private void VerirfyRTSBufferFull(SerialPort com1, SerialPort com2)
{
int com1BaudRate = com1.BaudRate;
int com2BaudRate = com2.BaudRate;
int com1ReadBufferSize = com1.ReadBufferSize;
int bufferSize = com1.ReadBufferSize;
int upperLimit = (3 * bufferSize) / 4;
int lowerLimit = bufferSize / 4;
byte[] bytes = new byte[upperLimit];
try
{
//Set the BaudRate to something faster so that it does not take so long to fill up the buffer
com1.BaudRate = 115200;
com2.BaudRate = 115200;
//Write 1 less byte then when the RTS pin would be cleared
com2.Write(bytes, 0, upperLimit - 1);
Thread.Sleep(DEFAULT_WAIT_AFTER_READ_OR_WRITE);
if (IsRequestToSend(com1))
{
if (!com2.CtsHolding)
{
Fail("Err_548458ahiede Expected RTS to be set");
}
}
else
{
if (com2.CtsHolding)
{
Fail("Err_028538aieoz Expected RTS to be cleared");
}
}
//Write the byte and verify the RTS pin is cleared appropriately
com2.Write(bytes, 0, 1);
Thread.Sleep(DEFAULT_WAIT_AFTER_READ_OR_WRITE);
if (IsRequestToSend(com1))
{
if (com2.CtsHolding)
{
Fail("Err_508845aueid Expected RTS to be cleared");
}
}
else
{
if (com2.CtsHolding)
{
Fail("Err_48848ajeoid Expected RTS to be set");
}
}
//Read 1 less byte then when the RTS pin would be set
com1.Read(bytes, 0, (upperLimit - lowerLimit) - 1);
Thread.Sleep(DEFAULT_WAIT_AFTER_READ_OR_WRITE);
if (IsRequestToSend(com1))
{
if (com2.CtsHolding)
{
Fail("Err_952085aizpea Expected RTS to be cleared");
}
}
else
{
if (com2.CtsHolding)
{
Fail("Err_725527ahjiuzp Expected RTS to be set");
}
}
//Read the byte and verify the RTS pin is set appropriately
com1.Read(bytes, 0, 1);
Thread.Sleep(DEFAULT_WAIT_AFTER_READ_OR_WRITE);
if (IsRequestToSend(com1))
{
if (!com2.CtsHolding)
{
Fail("Err_652820aopea Expected RTS to be set");
}
}
else
{
if (com2.CtsHolding)
{
Fail("Err_35585ajuei Expected RTS to be cleared");
}
}
}
finally
{
//Rollback any changed that were made to the SerialPort
com1.BaudRate = com1BaudRate;
com2.BaudRate = com2BaudRate;
com1.DiscardInBuffer();//This can cuase the XOn character to be sent to com2.
Thread.Sleep(DEFAULT_WAIT_AFTER_READ_OR_WRITE);
com2.DiscardInBuffer();
}
}
private void VerifyXOnXOffHandshake(SerialPort com1, SerialPort com2)
{
bool origRtsEnable = com2.RtsEnable;
Random rndGen = new Random();
byte[] xmitXOnBytes = new byte[s_DEFAULT_BYTE_SIZE_XON_XOFF];
byte[] xmitXOffBytes = new byte[s_DEFAULT_BYTE_SIZE_XON_XOFF];
try
{
com2.RtsEnable = true;
for (int i = 0; i < xmitXOnBytes.Length; i++)
{
byte rndByte;
do
{
rndByte = (byte)rndGen.Next(0, 256);
} while (rndByte == 17 || rndByte == 19);
xmitXOnBytes[i] = rndByte;
}
for (int i = 0; i < xmitXOffBytes.Length; i++)
{
byte rndByte;
do
{
rndByte = (byte)rndGen.Next(0, 256);
} while (rndByte == 17 || rndByte == 19);
xmitXOffBytes[i] = rndByte;
}
int XOnIndex = rndGen.Next(0, xmitXOnBytes.Length);
int XOffIndex = rndGen.Next(0, xmitXOffBytes.Length);
xmitXOnBytes[XOnIndex] = (byte)17;
xmitXOffBytes[XOffIndex] = (byte)19;
Debug.WriteLine("XOnIndex={0} XOffIndex={1}", XOnIndex, XOffIndex);
com2.Write(xmitXOnBytes, 0, xmitXOnBytes.Length);
com2.Write(xmitXOnBytes, 0, xmitXOnBytes.Length);
while (true)
{
try
{
com1.ReadByte();
}
catch (TimeoutException)
{
break;
}
}
try
{
com1.Write(new byte[s_DEFAULT_BYTE_SIZE], 0, s_DEFAULT_BYTE_SIZE);
}
catch (TimeoutException)
{
Fail("Err_2357pquaz!!! TimeoutException thrown after XOn char sent and CtsHolding={0} with Handshake={1}", com1.CtsHolding, com1.Handshake);
}
com2.Write(xmitXOffBytes, 0, xmitXOffBytes.Length);
while (true)
{
try
{
com1.ReadByte();
}
catch (TimeoutException)
{
break;
}
}
try
{
com1.Write(new byte[s_DEFAULT_BYTE_SIZE], 0, s_DEFAULT_BYTE_SIZE);
if (Handshake.XOnXOff == com1.Handshake || Handshake.RequestToSendXOnXOff == com1.Handshake)
{
Fail("Err_1349znpq!!! TimeoutException NOT thrown after XOff char sent and CtsHolding={0} with Handshake={1}", com1.CtsHolding, com1.Handshake);
}
}
catch (TimeoutException)
{
if (Handshake.XOnXOff != com1.Handshake && Handshake.RequestToSendXOnXOff != com1.Handshake)
{
Fail("Err_2507pqzhn!!! TimeoutException thrown after XOff char sent and CtsHolding={0} with Handshake={1}", com1.CtsHolding, com1.Handshake);
}
}
com2.Write(xmitXOnBytes, 0, xmitXOnBytes.Length);
while (true)
{
try
{
com1.ReadByte();
}
catch (TimeoutException)
{
break;
}
}
try
{
com1.Write(new byte[s_DEFAULT_BYTE_SIZE], 0, s_DEFAULT_BYTE_SIZE);
}
catch (TimeoutException)
{
Fail("Err_2570aqpa!!! TimeoutException thrown after XOn char sent and CtsHolding={0} with Handshake={1}", com1.CtsHolding, com1.Handshake);
}
}
finally
{
com2.RtsEnable = origRtsEnable;
com2.DiscardInBuffer();
}
}
private void VerifyRTSXOnXOffHandshake(SerialPort com1, SerialPort com2)
{
bool origRtsEnable = com2.RtsEnable;
Random rndGen = new Random();
byte[] xmitXOnBytes = new byte[s_DEFAULT_BYTE_SIZE_XON_XOFF];
byte[] xmitXOffBytes = new byte[s_DEFAULT_BYTE_SIZE_XON_XOFF];
try
{
for (int i = 0; i < xmitXOnBytes.Length; i++)
{
byte rndByte;
do
{
rndByte = (byte)rndGen.Next(0, 256);
} while (rndByte == 17 || rndByte == 19);
xmitXOnBytes[i] = rndByte;
}
for (int i = 0; i < xmitXOffBytes.Length; i++)
{
byte rndByte;
do
{
rndByte = (byte)rndGen.Next(0, 256);
} while (rndByte == 17 || rndByte == 19);
xmitXOffBytes[i] = rndByte;
}
xmitXOnBytes[rndGen.Next(0, xmitXOnBytes.Length)] = (byte)17;
xmitXOffBytes[rndGen.Next(0, xmitXOffBytes.Length)] = (byte)19;
com2.RtsEnable = false;
com2.Write(xmitXOffBytes, 0, xmitXOffBytes.Length);
com2.Write(xmitXOnBytes, 0, xmitXOnBytes.Length);
while (true)
{
try
{
com1.ReadByte();
}
catch (TimeoutException)
{
break;
}
}
try
{
com1.Write(new byte[s_DEFAULT_BYTE_SIZE], 0, s_DEFAULT_BYTE_SIZE);
if (Handshake.RequestToSend == com1.Handshake || Handshake.RequestToSendXOnXOff == com1.Handshake)
{
Fail("Err_1253aasyo!!! TimeoutException NOT thrown after XOff and XOn char sent when CtsHolding={0} with Handshake={1}", com1.CtsHolding, com1.Handshake);
}
}
catch (TimeoutException)
{
if (Handshake.RequestToSend != com1.Handshake && Handshake.RequestToSendXOnXOff != com1.Handshake)
{
Fail("Err_51390awi!!! TimeoutException thrown after XOff and XOn char sent when CtsHolding={0} with Handshake={1}", com1.CtsHolding, com1.Handshake);
}
}
com2.Write(xmitXOffBytes, 0, xmitXOffBytes.Length);
com2.RtsEnable = false;
com2.RtsEnable = true;
while (true)
{
try
{
com1.ReadByte();
}
catch (TimeoutException)
{
break;
}
}
try
{
com1.Write(new byte[s_DEFAULT_BYTE_SIZE], 0, s_DEFAULT_BYTE_SIZE);
if (Handshake.XOnXOff == com1.Handshake || Handshake.RequestToSendXOnXOff == com1.Handshake)
{
Fail("Err_2457awez!!! TimeoutException NOT thrown after RTSEnable set to false then true when CtsHolding={0} with Handshake={1}", com1.CtsHolding, com1.Handshake);
}
}
catch (TimeoutException)
{
if (Handshake.XOnXOff != com1.Handshake && Handshake.RequestToSendXOnXOff != com1.Handshake)
{
Fail("Err_3240aw4er!!! TimeoutException thrown RTSEnable set to false then true when CtsHolding={0} with Handshake={1}", com1.CtsHolding, com1.Handshake);
}
}
com2.Write(xmitXOnBytes, 0, xmitXOnBytes.Length);
while (true)
{
try
{
com1.ReadByte();
}
catch (TimeoutException)
{
break;
}
}
}
finally
{
com2.RtsEnable = origRtsEnable;
com2.DiscardInBuffer();
}
}
private void BUT_savesettings_Click(object sender, EventArgs e)
{
ArdupilotMega.Comms.ICommsSerial comPort = new SerialPort();
try
{
comPort.PortName = MainV2.comPort.BaseStream.PortName;
comPort.BaudRate = MainV2.comPort.BaseStream.BaudRate;
comPort.ReadTimeout = 4000;
comPort.Open();
}
catch { CustomMessageBox.Show("Invalid ComPort or in use"); return; }
lbl_status.Text = "Connecting";
if (doConnect(comPort))
{
comPort.DiscardInBuffer();
lbl_status.Text = "Doing Command";
if (RTI.Text != "")
{
// remote
string answer = doCommand(comPort, "RTI5");
string[] items = answer.Split('\n');
foreach (string item in items)
{
if (item.StartsWith("S"))
{
string[] values = item.Split(':', '=');
if (values.Length == 3)
{
Control[] controls = this.Controls.Find("R" + values[0].Trim(), false);
if (controls.Length > 0)
{
if (controls[0].GetType() == typeof(CheckBox))
{
string value = ((CheckBox)controls[0]).Checked ? "1" : "0";
if (value != values[2].Trim())
{
string cmdanswer = doCommand(comPort, "RT" + values[0].Trim() + "=" + value + "\r");
if (cmdanswer.Contains("OK"))
{
}
else
{
CustomMessageBox.Show("Set Command error");
}
}
}
else
{
if (controls[0].Text != values[2].Trim() && controls[0].Text != "")
{
string cmdanswer = doCommand(comPort, "RT" + values[0].Trim() + "=" + controls[0].Text + "\r");
if (cmdanswer.Contains("OK"))
{
}
else
{
CustomMessageBox.Show("Set Command error");
}
}
}
}
}
}
}
// write it
doCommand(comPort, "RT&W");
// return to normal mode
doCommand(comPort, "RTZ");
Sleep(100);
}
comPort.DiscardInBuffer();
{
//local
string answer = doCommand(comPort, "ATI5");
string[] items = answer.Split('\n');
foreach (string item in items)
{
if (item.StartsWith("S"))
{
string[] values = item.Split(':', '=');
if (values.Length == 3)
{
Control[] controls = this.Controls.Find(values[0].Trim(), false);
if (controls.Length > 0)
{
if (controls[0].GetType() == typeof(CheckBox))
{
string value = ((CheckBox)controls[0]).Checked ? "1" : "0";
if (value != values[2].Trim())
{
string cmdanswer = doCommand(comPort, "AT" + values[0].Trim() + "=" + value + "\r");
if (cmdanswer.Contains("OK"))
{
}
else
{
CustomMessageBox.Show("Set Command error");
}
}
}
else
{
if (controls[0].Text != values[2].Trim())
{
string cmdanswer = doCommand(comPort, "AT" + values[0].Trim() + "=" + controls[0].Text + "\r");
if (cmdanswer.Contains("OK"))
{
}
else
{
CustomMessageBox.Show("Set Command error");
}
}
}
}
}
}
}
// write it
doCommand(comPort, "AT&W");
// return to normal mode
doCommand(comPort, "ATZ");
}
lbl_status.Text = "Done";
}
else
{
// return to normal mode
doCommand(comPort, "ATZ");
lbl_status.Text = "Fail";
CustomMessageBox.Show("Failed to enter command mode");
}
comPort.Close();
}
public void Read()
{
int read_count=0;
if (debug) Console.WriteLine("Cserial:Read()\n");
System.Threading.Thread.Sleep(1);//wait and for other
//
try
{
_serialPort.DiscardInBuffer();
}catch(Exception e)
{
if (this.debug) Console.WriteLine(e.ToString());
}
this.read_status = SerialStatus.ReadStart;//start
while (Cserial._continue)
{
try
{
if (data_type == 0)
{
read_message = _serialPort.ReadLine();
read_deal();
}
else
{
try
{
read_count = this._serialPort.BytesToRead;
if (read_count < 1)
{
System.Threading.Thread.Sleep(this.read_sleep);//wait and for other
continue;
}
}
catch (InvalidOperationException e) { if (this.debug) Console.WriteLine(e.ToString()); break; }//InvalidOperationException
if (read_count > buffer_len - this.index)
{
if (this.index - this.deal_index > Cdata.min_len) read_deal();
if (buffer_len - this.index < buffer_len / 2) this.adjust_buffer();
read_count = buffer_len - this.index;
}
try//_serialPort.Read
{
read_byte_count = _serialPort.Read(this.read_buffer, this.index, read_count);
this.index += read_byte_count;
while (this.cdata.in_deal) System.Threading.Thread.Sleep(1);//wait and for other thread
read_deal();
}
catch (ArgumentNullException e) { if (this.debug) Console.WriteLine(e.ToString()); }
catch (InvalidOperationException e) { if (this.debug) Console.WriteLine(e.ToString()); break; }
catch (ArgumentOutOfRangeException e) { if (this.debug) Console.WriteLine(e.ToString()); }
catch (ArgumentException e) { if (this.debug) Console.WriteLine(e.ToString());}
catch (TimeoutException e) { if (this.debug) Console.WriteLine(e.ToString()); }
catch (Exception e) { if (this.debug) Console.WriteLine(e.ToString()); }
}//else
}catch (Exception e) { if (this.debug) Console.WriteLine(e.ToString()); }
}
this.read_status = SerialStatus.ReadEnd;
}
private static void Port_DataReceived(object sender, SerialDataReceivedEventArgs e)
{
try {
Thread.Sleep(5000);
//int bytesToRead = port.BytesToRead;
//byte[] buffer = new byte[bytesToRead];
// port.Read(buffer, 0, bytesToRead);
//string data = Encoding.ASCII.GetString(buffer);
string data = port.ReadLine();
port.DiscardOutBuffer();
port.DiscardInBuffer();
Console.WriteLine("data: " + data);
Dictionary <string, object> returnDict = new Dictionary <string, object>();
returnDict.Add("deviceID", DEVICEID);
returnDict.Add("timesent", DateTime.Now);
foreach (var item in data.Split(new char[] { ';' }, StringSplitOptions.RemoveEmptyEntries))
{
string[] splitBursch = item.Split(new char[] { ':' }, StringSplitOptions.RemoveEmptyEntries);
if (splitBursch[0] == "noisevalues")
{
//string[] valuesSplit = splitBursch[1].Split(new char[] { '_' }, StringSplitOptions.RemoveEmptyEntries);
//double[] valuesDouble = new double[valuesSplit.Length];
//for (int i = 0; i < valuesSplit.Length; i++)
//{
// if (Double.TryParse(valuesSplit[i], System.Globalization.NumberStyles.Any, formatInfo, out double res1))
// {
// valuesDouble[i] = res1;
// }
//}
//var templist = valuesDouble.ToList();
//templist.Sort();
//valuesDouble = templist.ToArray();
//(double quartil1, double median, double quartil3) = Quartiles(valuesDouble);
//returnDict.Add("noisequartal1", quartil1);
//returnDict.Add("noisemedian", median);
//returnDict.Add("noisequartal3", quartil3);
}
else if (Double.TryParse(splitBursch[1], System.Globalization.NumberStyles.Any, formatInfo, out double res))
{
returnDict.Add(splitBursch[0], res);
}
else
{
Console.WriteLine("Failed parsing to double: '" + splitBursch[1] + "' at property '" + splitBursch[0] + "'");
Console.WriteLine("At data: " + data);
}
}
Console.WriteLine(Newtonsoft.Json.JsonConvert.SerializeObject(returnDict));
if (returnDict.Count > 7)
{
SendToApi(Newtonsoft.Json.JsonConvert.SerializeObject(returnDict));
}
else
{
Console.WriteLine("Sending was cancelled due to weird values in dictionary");
}
}
catch (Exception er)
{
Console.WriteLine("*************************");
Console.WriteLine(er.Message);
Console.WriteLine("*************************");
}
}
void Update()
{
if (sp.IsOpen)
{
//read from the COM Port
string inData = sp.ReadLine();
//Debug.Log("Arduino==>" + inData);
//must have these lines for when the arduino sends faster than the readTimeout
sp.BaseStream.Flush();
sp.DiscardInBuffer();
if (inData != "") //throw out empty data
//DATA FORMAT:
//"RVw,RVx,RVy,RVz,Ax,Ay,Az\n" no spaces, newline character at the end
//parse the data into an array of strings
{
data = inData.Split(',');
/*
* Debug.Log("Data: ");
* foreach (var item in data)
* {
* Debug.Log(item);
* }
* Debug.Log("End Data");
*/
//Debug.Log("0: " + data[0] + " 1: " + data[1] + " 2: " + data[2] + "3: " + data[3]);
//initialize starting orientation after a waiting period
/*
* if (setInitialOrientationCount > 1000 && !initialized) //wait for 1000 frames to pass- about 1 sec
* {
* initialOrientation = new Quaternion(float.Parse(data[3]), -float.Parse(data[0]), -float.Parse(data[1]), -float.Parse(data[2]));
*
* initialized = true;
* }
* else
* {
* setInitialOrientationCount++;
* }
*/
//create quaternions from incoming orientation data. Constructor: Quaternion, x, y, z, w)
//parse out erroneous data (experimentally found)
if (data.Length < 7 || data[3] == "nan" || float.Parse(data[0]) > 2)
{
//Debug.Log("Incorrect data was sent.");
}
else //data is correct and complete, continue to pass that on to the target
{
//write to a file
// Set a variable to the Documents path.
string docPath = Environment.GetFolderPath(Environment.SpecialFolder.MyDocuments);
//Append text to an existing file named "outputData.txt".
using (StreamWriter outputFile = new StreamWriter(Path.Combine(docPath, "outputData.txt"), true))
{
outputFile.WriteLine(DateTime.Now.ToString("h:mm:ss") + "- Qw: " + data[0] + " Qx: " + data[1] + " Qy: " + data[2] + " Qz: " + data[3] + "Ax: " + data[4] + " Ay: " + data[5] + " Az: " + data[6]);
}
//transform orientation!
Quaternion newQuat = new Quaternion(float.Parse(data[0]), float.Parse(data[3]), -float.Parse(data[2]), -float.Parse(data[1])); //out-of-orderness and negatives are to align the rotation movement with the cursor object
target.transform.rotation = Quaternion.Normalize(newQuat); //normalize, then update orientation
// target.transform.rotation = Quaternion.RotateTowards(Quaternion.Normalize(newQuat), Quaternion.Normalize(homeOrientation), 90)
//transform positioning
target.transform.position = target.transform.position + new Vector3(prevVel.x * Time.deltaTime + ((1 / 2) * float.Parse(data[4]) * Time.deltaTime * Time.deltaTime), prevVel.y * Time.deltaTime + ((1 / 2) * float.Parse(data[5]) * Time.deltaTime * Time.deltaTime), prevVel.z * Time.deltaTime + ((1 / 2) * float.Parse(data[6]) * Time.deltaTime * Time.deltaTime));
//multiply accel by delta time to get velocity, then store that for the next frame to use
prevVel.x = float.Parse(data[4]) * Time.deltaTime;
prevVel.y = float.Parse(data[5]) * Time.deltaTime;
prevVel.z = float.Parse(data[6]) * Time.deltaTime;
}
}
}
else //serial port is not open
{
Debug.LogError("Serial port: " + sp.PortName + " is unavailable");
}
}
// Use this for initialization
public static void Init()
{
handPortL = new SerialPort("\\\\.\\COM255", 115200);
handPortR = new SerialPort("\\\\.\\COM256", 115200);
if (!handPortL.IsOpen)
{
try
{
handPortL.Open();
handPortL.BaudRate = 115200;
handPortL.WriteLine("H2"); //confirm which hand is which
handPortL.DiscardOutBuffer();
handPortL.DiscardInBuffer();
var i = 1;
Console.WriteLine("!!! COM PORTS Left Did Open " + i + " times !!!");
i++;
}
catch (Exception e)
{
Console.WriteLine("Could not open comport for hands" + e);
}
}
if (!handPortR.IsOpen)
{
try
{
handPortR.Open();
handPortR.BaudRate = 115200; //stable
handPortR.WriteLine("H1");
handPortR.DiscardOutBuffer();
handPortR.DiscardInBuffer();
var i = 1;
Console.WriteLine("!!! COM PORTS Right Did Open " + i + " times !!!");
i++;
}
catch (Exception e)
{
Console.WriteLine("Could not open comport for hands" + e);
}
}
try
{
Console.WriteLine("Try Start Ran ");
handPortL.WriteLine("500,500,500,500"); //change to write?
handPortR.WriteLine("500,500,500,500");
handPortL.DiscardOutBuffer();
handPortL.DiscardInBuffer();
handPortR.DiscardOutBuffer();
handPortR.DiscardInBuffer();
var i = 1;
Console.WriteLine("Wrote to L and R " + i + " Times");
i++;
}
catch (Exception e)
{
Console.WriteLine("Stuff sucks on start " + e);
}
}
/// <summary>
/// Verify if the serial port is opened and opens it if necessary
/// </summary>
/// <returns><c>true</c>, if port is opened, <c>false</c> otherwise.</returns>
/// <param name="portSpeed">Port speed.</param>
public static bool checkOpen(int portSpeed = 9600)
{
if (s_serial == null) {
string portName = GetPortName ();
if (portName == "") {
print ("Error: Couldn't find serial port.");
return false;
} else {
if (s_debug)
print("Opening serial port: " + portName);
}
s_serial = new SerialPort (portName, portSpeed);
s_serial.Open ();
//print ("default ReadTimeout: " + s_serial.ReadTimeout);
//s_serial.ReadTimeout = 10;
// clear input buffer from previous garbage
s_serial.DiscardInBuffer ();
}
return s_serial.IsOpen;
}
/// <summary>
/// 标准模块初始化
/// </summary>
/// <returns>返回初始化是否成功</returns>
public bool StandarBoardInit()
{
if (cMain.isDebug)
{
mStandarBoardInit = true;
return(true);
}
byte[] WriteBuff = new byte[10]; //发送数据
byte[] ReadBuff = new byte[20]; //接收数据
int ReturnByte = 0; //返回数据
bool IsReturn = false; //是否成功返回
bool IsTimeOut = false; //是否超时
DateTime NowTime = DateTime.Now; //当前时间
TimeSpan ts; //时间差
byte CrcHi = 0, CrcLo = 0; //CRC校验
try
{
if (!comPort.IsOpen)
{
comPort.Open();
}
WriteBuff[0] = (byte)(_StandarModebusAddress & 0xFF);
WriteBuff[1] = 0x03;
WriteBuff[2] = 0x00;
WriteBuff[3] = 0x01;
WriteBuff[4] = 0x00;
WriteBuff[5] = 0x01;
cMain.CRC_16(WriteBuff, 6, ref CrcLo, ref CrcHi);
WriteBuff[6] = CrcLo;
WriteBuff[7] = CrcHi;
comPort.DiscardInBuffer();
comPort.Write(WriteBuff, 0, 8);
NowTime = DateTime.Now;
do
{
//System.Windows.Forms.Application.DoEvents();
Threading.Thread.Sleep(20);
if (comPort.BytesToRead >= WriteBuff[5] * 2 + 5)//收到数据
{
ReturnByte = comPort.BytesToRead;
IsReturn = true;
}
ts = DateTime.Now - NowTime;
if (ts.TotalMilliseconds > timeOut)//时间超时
{
IsTimeOut = true;
}
} while (!IsReturn && !IsTimeOut);
if (!IsReturn && IsTimeOut)//超时
{
if (ErrStr.IndexOf("接收数据已超时") < 0)
{
ErrStr = ErrStr + DateTime.Now.ToString() + ",StandarBoardInit," + "初始失败,接收数据已超时" + (char)13 + (char)10;
}
return(false);
}
else
{
comPort.Read(ReadBuff, 0, ReturnByte);
if ((ReadBuff[0] != WriteBuff[0]) || (ReadBuff[1] != WriteBuff[1]))//数据检验失败
{
if (ErrStr.IndexOf("接收数据错误") < 0)
{
ErrStr = ErrStr + DateTime.Now.ToString() + ",StandarBoardInit," + ":初始失败,接收数据错误" + (char)13 + (char)10;
}
return(false);
}
else
{
mStandarBoardInit = true;
}
}
}
catch (Exception exc)
{
if (ErrStr.IndexOf(exc.ToString()) < 0)
{
ErrStr = ErrStr + DateTime.Now.ToString() + ",StandarBoardInit," + ":" + exc.ToString() + (char)13 + (char)10;
}
return(false);
}
return(true);
}
public void DiscardInBuffer()
{
serial?.DiscardInBuffer();
}
//Init DexDrive (string returned if an error happened)
public string StartDexDrive(string ComPortName)
{
//Define a port to open
OpenedPort = new SerialPort(ComPortName, 38400, Parity.None, 8, StopBits.One);
OpenedPort.ReadBufferSize = 256;
//Buffer for storing read data from the DexDrive
byte[] ReadData = null;
//Try to open a selected port (in case of an error return a descriptive string)
try { OpenedPort.Open(); }
catch (Exception e) { return(e.Message); }
//Dexdrive won't respond if RTS is not toggled on/off
OpenedPort.RtsEnable = false;
Thread.Sleep(300);
OpenedPort.RtsEnable = true;
Thread.Sleep(300);
//DTR line is used for additional power
OpenedPort.DtrEnable = true;
//Check if DexDrive is attached to the port
//Detection may fail 1st or 2nd time, so the command is sent 5 times
for (int i = 0; i < 5; i++)
{
OpenedPort.DiscardInBuffer();
OpenedPort.Write("XXXXX");
Thread.Sleep(20);
}
//Check for "IAI" string
ReadData = ReadDataFromPort();
if (ReadData[0] != 0x49 || ReadData[1] != 0x41 || ReadData[2] != 0x49)
{
return("DexDrive was not detected on '" + ComPortName + "' port.");
}
//Wake DexDrive up (kick it from POUT mode)
SendDataToPort((byte)DexCommands.INIT, new byte[] { 0x10, 0xAA, 0xBB, 0xCC, 0xDD, 0xEE, 0xFF, 0xAA, 0xBB, 0xCC, 0xDD, 0xEE, 0xFF, 0xAA, 0xBB, 0xCC, 0xDD }, 50);
//SendDataToPort((byte)DexCommands.INIT, new byte[] { 0x10, 0x29, 0x23, 0xbe, 0x84, 0xe1, 0x6c, 0xd6, 0xae, 0x52, 0x90, 0x49, 0xf1, 0xf1, 0xbb, 0xe9, 0xeb }, 50);
//Check for "PSX" string
ReadData = ReadDataFromPort();
if (ReadData[5] != 0x50 || ReadData[6] != 0x53 || ReadData[7] != 0x58)
{
return("Detected device is not a PS1 DexDrive.");
}
//Fetch the firmware version
FirmwareVersion = (ReadData[8] >> 6).ToString() + "." + ((ReadData[8] >> 2) & 0xF).ToString() + (ReadData[8] & 0x3).ToString();
//Send magic handshake signal 10 times
for (int i = 0; i < 10; i++)
{
SendDataToPort((byte)DexCommands.MAGIC_HANDSHAKE, null, 0);
}
Thread.Sleep(50);
//Turn on the status light
SendDataToPort((byte)DexCommands.LIGHT, new byte[] { 1 }, 50);
//Everything went well, DexDrive is ready to recieve commands
return(null);
}
/// <summary>
/// Detect board version
/// </summary>
/// <param name="port"></param>
/// <returns> boards enum value</returns>
public static boards DetectBoard(string port)
{
SerialPort serialPort = new SerialPort();
serialPort.PortName = port;
if (!MainV2.MONO)
{
try
{
var ports = Win32DeviceMgmt.GetAllCOMPorts();
foreach (var item in ports)
{
log.InfoFormat("{0}: {1} - {2}", item.name, item.description, item.board);
if (port.ToLower() == item.name.ToLower())
{
if (item.board == "PX4 FMU v4.x")
{
log.Info("is a px4v4 pixracer");
return(boards.px4v4);
}
if (item.board == "PX4 FMU v2.x")
{
CustomMessageBox.Show(Strings.PleaseUnplugTheBoardAnd);
DateTime DEADLINE = DateTime.Now.AddSeconds(30);
while (DateTime.Now < DEADLINE)
{
string[] allports = SerialPort.GetPortNames();
foreach (string port1 in allports)
{
log.Info(DateTime.Now.Millisecond + " Trying Port " + port1);
try
{
using (var up = new Uploader(port1, 115200))
{
up.identify();
Console.WriteLine(
"Found board type {0} boardrev {1} bl rev {2} fwmax {3} on {4}",
up.board_type,
up.board_rev, up.bl_rev, up.fw_maxsize, port1);
if (up.fw_maxsize == 2080768 && up.board_type == 9 && up.bl_rev >= 5)
{
log.Info("is a px4v3");
return(boards.px4v3);
}
else
{
log.Info("is a px4v2");
return(boards.px4v2);
}
}
}
catch (Exception ex)
{
log.Error(ex);
}
}
}
log.Info("Failed to detect px4 board type");
return(boards.none);
}
/*
* if (item.board == "Arduino Mega 2560")
* {
* log.Info("is a 2560v2");
* return boards.b2560v2;
* }*/
if (item.board == "revo-mini")
{
log.Info("is a revo-mini");
return(boards.revomini);
}
if (item.board == "mini-pix")
{
log.Info("is a mini-pix");
return(boards.minipix);
}
if (item.board == "mindpx-v2")
{
log.Info("is a mindpx-v2");
return(boards.mindpxv2);
}
}
}
} catch { }
ObjectQuery query = new ObjectQuery("SELECT * FROM Win32_SerialPort"); // Win32_USBControllerDevice
ManagementObjectSearcher searcher = new ManagementObjectSearcher(query);
foreach (ManagementObject obj2 in searcher.Get())
{
log.InfoFormat("-----------------------------------");
log.InfoFormat("Win32_USBDevice instance");
log.InfoFormat("-----------------------------------");
foreach (var item in obj2.Properties)
{
log.InfoFormat("{0}: {1}", item.Name, item.Value);
}
// check vid and pid
if (obj2.Properties["PNPDeviceID"].Value.ToString().Contains(@"USB\VID_2341&PID_0010"))
{
// check port name as well
if (obj2.Properties["Name"].Value.ToString().ToUpper().Contains(serialPort.PortName.ToUpper()))
{
log.Info("is a 2560-2");
return(boards.b2560v2);
}
}
if (obj2.Properties["PNPDeviceID"].Value.ToString().Contains(@"USB\VID_26AC&PID_0010"))
{
// check port name as well
//if (obj2.Properties["Name"].Value.ToString().ToUpper().Contains(serialPort.PortName.ToUpper()))
{
log.Info("is a px4");
return(boards.px4);
}
}
if (obj2.Properties["PNPDeviceID"].Value.ToString().Contains(@"USB\VID_26AC&PID_0011"))
{
CustomMessageBox.Show(Strings.PleaseUnplugTheBoardAnd);
DateTime DEADLINE = DateTime.Now.AddSeconds(30);
while (DateTime.Now < DEADLINE)
{
string[] allports = SerialPort.GetPortNames();
foreach (string port1 in allports)
{
log.Info(DateTime.Now.Millisecond + " Trying Port " + port1);
try
{
using (var up = new Uploader(port1, 115200))
{
up.identify();
Console.WriteLine(
"Found board type {0} boardrev {1} bl rev {2} fwmax {3} on {4}",
up.board_type,
up.board_rev, up.bl_rev, up.fw_maxsize, port1);
if (up.fw_maxsize == 2080768 && up.board_type == 9 && up.bl_rev >= 5)
{
log.Info("is a px4v3");
return(boards.px4v3);
}
else
{
log.Info("is a px4v2");
return(boards.px4v2);
}
}
}
catch (Exception ex)
{
log.Error(ex);
}
}
}
log.Info("Failed to detect px4 board type");
return(boards.none);
}
if (obj2.Properties["PNPDeviceID"].Value.ToString().Contains(@"USB\VID_26AC&PID_0021"))
{
log.Info("is a px4v3 X2.1");
return(boards.px4v3);
}
if (obj2.Properties["PNPDeviceID"].Value.ToString().Contains(@"USB\VID_26AC&PID_0012"))
{
log.Info("is a px4v4 pixracer");
return(boards.px4v4);
}
if (obj2.Properties["PNPDeviceID"].Value.ToString().Contains(@"USB\VID_26AC&PID_0013"))
{
log.Info("is a px4v4pro pixhawk 3 pro");
return(boards.px4v4pro);
}
if (obj2.Properties["PNPDeviceID"].Value.ToString().Contains(@"USB\VID_26AC&PID_0001"))
{
log.Info("is a px4v2 bootloader");
return(boards.px4v2);
}
if (obj2.Properties["PNPDeviceID"].Value.ToString().Contains(@"USB\VID_26AC&PID_0016"))
{
log.Info("is a px4v2 bootloader");
CustomMessageBox.Show(
"You appear to have a bootloader with a bad PID value, please update your bootloader.");
return(boards.px4v2);
}
//|| obj2.Properties["PNPDeviceID"].Value.ToString().Contains(@"USB\VID_26AC&PID_0012") || obj2.Properties["PNPDeviceID"].Value.ToString().Contains(@"USB\VID_26AC&PID_0013") || obj2.Properties["PNPDeviceID"].Value.ToString().Contains(@"USB\VID_26AC&PID_0014") || obj2.Properties["PNPDeviceID"].Value.ToString().Contains(@"USB\VID_26AC&PID_0015") || obj2.Properties["PNPDeviceID"].Value.ToString().Contains(@"USB\VID_26AC&PID_0016")
if (obj2.Properties["PNPDeviceID"].Value.ToString().Contains(@"USB\VID_27AC&PID_1140"))
{
log.Info("is a vrbrain 4.0 bootloader");
return(boards.vrbrainv40);
}
if (obj2.Properties["PNPDeviceID"].Value.ToString().Contains(@"USB\VID_27AC&PID_1145"))
{
log.Info("is a vrbrain 4.5 bootloader");
return(boards.vrbrainv45);
}
if (obj2.Properties["PNPDeviceID"].Value.ToString().Contains(@"USB\VID_27AC&PID_1150"))
{
log.Info("is a vrbrain 5.0 bootloader");
return(boards.vrbrainv50);
}
if (obj2.Properties["PNPDeviceID"].Value.ToString().Contains(@"USB\VID_27AC&PID_1151"))
{
log.Info("is a vrbrain 5.1 bootloader");
return(boards.vrbrainv51);
}
if (obj2.Properties["PNPDeviceID"].Value.ToString().Contains(@"USB\VID_27AC&PID_1152"))
{
log.Info("is a vrbrain 5.2 bootloader");
return(boards.vrbrainv52);
}
if (obj2.Properties["PNPDeviceID"].Value.ToString().Contains(@"USB\VID_27AC&PID_1154"))
{
log.Info("is a vrbrain 5.4 bootloader");
return(boards.vrbrainv54);
}
if (obj2.Properties["PNPDeviceID"].Value.ToString().Contains(@"USB\VID_27AC&PID_1910"))
{
log.Info("is a vrbrain core 1.0 bootloader");
return(boards.vrcorev10);
}
if (obj2.Properties["PNPDeviceID"].Value.ToString().Contains(@"USB\VID_27AC&PID_1351"))
{
log.Info("is a vrubrain 5.1 bootloader");
return(boards.vrubrainv51);
}
if (obj2.Properties["PNPDeviceID"].Value.ToString().Contains(@"USB\VID_27AC&PID_1352"))
{
log.Info("is a vrubrain 5.2 bootloader");
return(boards.vrubrainv52);
}
}
}
else
{
// if its mono
if ((int)DialogResult.Yes == CustomMessageBox.Show("Is this a APM 2+?", "APM 2+", MessageBoxButtons.YesNo))
{
return(boards.b2560v2);
}
else
{
if ((int)DialogResult.Yes ==
CustomMessageBox.Show("Is this a PX4/PIXHAWK/PIXRACER?", "PX4/PIXHAWK", MessageBoxButtons.YesNo))
{
if ((int)DialogResult.Yes ==
CustomMessageBox.Show("Is this a PIXRACER?", "PIXRACER", MessageBoxButtons.YesNo))
{
return(boards.px4v4);
}
if ((int)DialogResult.Yes ==
CustomMessageBox.Show("Is this a CUBE?", "CUBE", MessageBoxButtons.YesNo))
{
return(boards.px4v3);
}
if ((int)DialogResult.Yes ==
CustomMessageBox.Show("Is this a PIXHAWK?", "PIXHAWK", MessageBoxButtons.YesNo))
{
return(boards.px4v2);
}
return(boards.px4);
}
else
{
return(boards.b2560);
}
}
}
if ((int)DialogResult.Yes == CustomMessageBox.Show("Is this a Linux board?", "Linux", MessageBoxButtons.YesNo))
{
if ((int)DialogResult.Yes == CustomMessageBox.Show("Is this Bebop2?", "Bebop2", MessageBoxButtons.YesNo))
{
return(boards.bebop2);
}
if ((int)DialogResult.Yes == CustomMessageBox.Show("Is this Disco?", "Disco", MessageBoxButtons.YesNo))
{
return(boards.disco);
}
}
if (serialPort.IsOpen)
{
serialPort.Close();
}
serialPort.DtrEnable = true;
serialPort.BaudRate = 57600;
serialPort.Open();
Thread.Sleep(100);
int a = 0;
while (a < 20) // 20 * 50 = 1 sec
{
//Console.WriteLine("write " + DateTime.Now.Millisecond);
serialPort.DiscardInBuffer();
serialPort.Write(new byte[] { (byte)'0', (byte)' ' }, 0, 2);
a++;
Thread.Sleep(50);
//Console.WriteLine("btr {0}", serialPort.BytesToRead);
if (serialPort.BytesToRead >= 2)
{
byte b1 = (byte)serialPort.ReadByte();
byte b2 = (byte)serialPort.ReadByte();
if (b1 == 0x14 && b2 == 0x10)
{
serialPort.Close();
log.Info("is a 1280");
return(boards.b1280);
}
}
}
if (serialPort.IsOpen)
{
serialPort.Close();
}
log.Warn("Not a 1280");
Thread.Sleep(500);
serialPort.DtrEnable = true;
serialPort.BaudRate = 115200;
serialPort.Open();
Thread.Sleep(100);
a = 0;
while (a < 4)
{
byte[] temp = new byte[] { 0x6, 0, 0, 0, 0 };
temp = BoardDetect.genstkv2packet(serialPort, temp);
a++;
Thread.Sleep(50);
try
{
if (temp[0] == 6 && temp[1] == 0 && temp.Length == 2)
{
serialPort.Close();
//HKEY_LOCAL_MACHINE\SYSTEM\CurrentControlSet\Enum\USB\VID_2341&PID_0010\640333439373519060F0\Device Parameters
if (!MainV2.MONO &&
!Thread.CurrentThread.CurrentCulture.IsChildOf(CultureInfoEx.GetCultureInfo("zh-Hans")))
{
ObjectQuery query = new ObjectQuery("SELECT * FROM Win32_SerialPort");
// Win32_USBControllerDevice
ManagementObjectSearcher searcher = new ManagementObjectSearcher(query);
foreach (ManagementObject obj2 in searcher.Get())
{
//Console.WriteLine("Dependant : " + obj2["Dependent"]);
// all apm 1-1.4 use a ftdi on the imu board.
/* obj2.Properties.ForEach(x =>
* {
* try
* {
* log.Info(((PropertyData)x).Name.ToString() + " = " + ((PropertyData)x).Value.ToString());
* }
* catch { }
* });
*/
// check vid and pid
if (obj2.Properties["PNPDeviceID"].Value.ToString().Contains(@"USB\VID_2341&PID_0010"))
{
// check port name as well
if (
obj2.Properties["Name"].Value.ToString()
.ToUpper()
.Contains(serialPort.PortName.ToUpper()))
{
log.Info("is a 2560-2");
return(boards.b2560v2);
}
}
}
log.Info("is a 2560");
return(boards.b2560);
}
}
}
catch
{
}
}
serialPort.Close();
log.Warn("Not a 2560");
if ((int)DialogResult.Yes == CustomMessageBox.Show("Is this a APM 2+?", "APM 2+", MessageBoxButtons.YesNo))
{
return(boards.b2560v2);
}
else
{
if ((int)DialogResult.Yes ==
CustomMessageBox.Show("Is this a PX4/PIXHAWK?", "PX4/PIXHAWK", MessageBoxButtons.YesNo))
{
if ((int)DialogResult.Yes ==
CustomMessageBox.Show("Is this a PIXHAWK?", "PIXHAWK", MessageBoxButtons.YesNo))
{
return(boards.px4v2);
}
return(boards.px4);
}
else
{
return(boards.b2560);
}
}
}
private void Reset()
{
_serialPort?.DiscardOutBuffer();
_serialPort?.DiscardInBuffer();
IsRecv = false;
}
private void ThreadProc()
{
//localization debug and testing...
//Thread.CurrentThread.CurrentUICulture = new System.Globalization.CultureInfo("pt-BR");
//Thread.CurrentThread.CurrentUICulture = new System.Globalization.CultureInfo("en-US");
SerialPort port1 = null, port2 = null;
string serialErr = null;
bool firstPort1Error = true, firstPort2Error = true;
try
{
try
{
port1 = new SerialPort(com1, baud1, Parity.None, 8, StopBits.One);
port1.DiscardNull = false;
port1.DtrEnable = false;
port1.Handshake = Handshake.None;
port1.ReadBufferSize = 8192;
port1.ReadTimeout = 2000;
port1.ReceivedBytesThreshold = 8192;
port1.RtsEnable = false;
port1.WriteBufferSize = 8192;
port1.WriteTimeout = 2000;
}
catch (Exception ex)
{
Err(Program.str("errorCreatingArduino") + ex.Message);
return;
}
if (!running)
{
return;
}
try
{
port2 = new SerialPort(com2, baud2, Parity.None, 8, StopBits.One);
port2.DiscardNull = false;
port2.DtrEnable = false;
port2.Handshake = Handshake.None;
port2.ReadBufferSize = 8192;
port2.ReadTimeout = 2000;
port2.ReceivedBytesThreshold = 8192;
port2.RtsEnable = false;
port2.WriteBufferSize = 8192;
port2.WriteTimeout = 2000;
}
catch (Exception ex)
{
Err(Program.str("errorCreatingBluetooth") + ex.Message);
return;
}
if (!running)
{
return;
}
try
{
port1.Open();
}
catch (Exception ex)
{
Err(Program.str("errorOpeningArduino") + ex.Message);
return;
}
try
{
port2.Open();
}
catch (Exception ex)
{
Err(Program.str("errorOpeningBluetooth") + ex.Message);
return;
}
try
{
port1.DiscardInBuffer();
}
catch
{
}
try
{
port1.DiscardOutBuffer();
}
catch
{
}
try
{
port2.DiscardInBuffer();
}
catch
{
}
try
{
port2.DiscardOutBuffer();
}
catch
{
}
if (!running)
{
return;
}
Thread t1 = new Thread(() =>
{
byte[] buffer1 = new byte[8192];
while (running && serialErr == null)
{
int s = 0;
try
{
s = port1.Read(buffer1, 0, 8192);
}
catch (TimeoutException)
{
//just ignore these timeouts as they will happen quite often here!
}
catch (Exception ex)
{
//the system is being shut down
if (!running || !port1.IsOpen || !port2.IsOpen)
{
return;
}
serialErr = Program.str("errorArduino") + ex.Message;
threadWakeUpEvent.Set();
return;
}
if (s > 0)
{
count1 += s; //won't do any harm... ;)
try
{
port2.Write(buffer1, 0, s);
notResponding2 = false;
}
catch (TimeoutException)
{
notResponding2 = true;
}
catch (Exception ex)
{
//the system is being shut down
if (!running || !port1.IsOpen || !port2.IsOpen)
{
return;
}
serialErr = Program.str("errorSendingBluetooth") + ex.Message;
threadWakeUpEvent.Set();
return;
}
}
}
});
Thread t2 = new Thread(() =>
{
byte[] buffer2 = new byte[8192];
while (running && serialErr == null)
{
int s = 0;
try
{
s = port2.Read(buffer2, 0, 8192);
}
catch (TimeoutException)
{
//just ignore these timeouts as they will happen quite often here!
}
catch (Exception ex)
{
//the system is being shut down
if (!running || !port1.IsOpen || !port2.IsOpen)
{
return;
}
serialErr = Program.str("errorBluetooth") + ex.Message;
threadWakeUpEvent.Set();
return;
}
if (s > 0 && running)
{
count2 += s; //won't do any harm... ;)
try
{
port1.Write(buffer2, 0, s);
notResponding1 = false;
}
catch (TimeoutException)
{
notResponding1 = true;
}
catch (Exception ex)
{
//the system is being shut down
if (!running || !port1.IsOpen || !port2.IsOpen)
{
return;
}
serialErr = Program.str("errorSendingArduino") + ex.Message;
threadWakeUpEvent.Set();
return;
}
}
}
});
t1.Start();
t2.Start();
port1.ErrorReceived += (sender, e) =>
{
//the system is being shut down
if (!running || !port1.IsOpen || !port2.IsOpen)
{
return;
}
//a few drivers indicate overrun if the port had been receiving
//too many bytes before it was open (which could easily happen if
//either the Arduino or the Bluetooth had been sending data before
//this application was started)
if (firstPort1Error && e.EventType == SerialError.Overrun)
{
firstPort1Error = false;
return;
}
firstPort1Error = false;
//this is executed in another worker thread
serialErr = Program.str("errorArduino") + e.EventType;
threadWakeUpEvent.Set();
};
port2.ErrorReceived += (sender, e) =>
{
//the system is being shut down
if (!running || !port1.IsOpen || !port2.IsOpen)
{
return;
}
//a few drivers indicate overrun if the port had been receiving
//too many bytes before it was open (which could easily happen if
//either the Arduino or the Bluetooth had been sending data before
//this application was started)
if (firstPort2Error && e.EventType == SerialError.Overrun)
{
firstPort2Error = false;
return;
}
firstPort2Error = false;
//this is executed in another worker thread
serialErr = Program.str("errorBluetooth") + e.EventType;
threadWakeUpEvent.Set();
};
BeginInvoke(threadReadyDelegate);
while (running && serialErr == null)
{
if (sendBuffer1 != null)
{
try
{
port1.Write(sendBuffer1, 0, sendBuffer1.Length);
notResponding1 = false;
BeginInvoke(threadData1SentDelegate);
}
catch (TimeoutException)
{
notResponding1 = true;
}
catch (Exception ex)
{
//the system is being shut down
if (running && port1.IsOpen && port2.IsOpen)
{
serialErr = Program.str("errorSendingArduino") + ex.Message;
}
break;
}
finally
{
sendBuffer1 = null;
}
}
if (sendBuffer2 != null)
{
try
{
port2.Write(sendBuffer2, 0, sendBuffer2.Length);
notResponding2 = false;
BeginInvoke(threadData2SentDelegate);
}
catch (TimeoutException)
{
notResponding2 = true;
}
catch (Exception ex)
{
//the system is being shut down
if (running && port1.IsOpen && port2.IsOpen)
{
serialErr = Program.str("errorSendingBluetooth") + ex.Message;
}
break;
}
finally
{
sendBuffer2 = null;
}
}
if (serialErr == null)
{
threadWakeUpEvent.WaitOne();
}
}
running = false;
t1.Join();
t2.Join();
}
finally
{
try
{
if (port1 != null)
{
port1.Close();
}
}
catch
{
}
try
{
if (port2 != null)
{
port2.Close();
}
}
catch
{
}
try
{
if (port1 != null)
{
port1.Dispose();
}
}
catch
{
}
try
{
if (port2 != null)
{
port2.Dispose();
}
}
catch
{
}
if (serialErr != null)
{
Err(serialErr);
}
else
{
BeginInvoke(threadDeathDelegate);
}
}
}
internal byte[] GetResponse(byte[] ResponsePattern)
{
byte[] ResponseBuffer = new byte[0x2000];
int Length = 0;
bool IsIncomplete = false;
do
{
IsIncomplete = false;
try
{
int BytesRead = 0;
if (Port != null)
{
BytesRead = Port.Read(ResponseBuffer, Length, ResponseBuffer.Length - Length);
}
if (USBDevice != null)
{
BytesRead = USBDevice.InputPipe.Read(ResponseBuffer);
}
if (BytesRead == 0)
{
LogFile.Log("Emergency mode of phone is ignoring us", LogType.FileAndConsole);
throw new BadMessageException();
}
Length += BytesRead;
byte[] DecodedResponse;
if (DecodeResponses)
{
DecodedResponse = DecodeResponse(ResponseBuffer, (UInt32)Length);
}
else
{
DecodedResponse = new byte[Length];
Buffer.BlockCopy(ResponseBuffer, 0, DecodedResponse, 0, Length);
}
if (ResponsePattern != null)
{
for (int i = 0; i < ResponsePattern.Length; i++)
{
if (DecodedResponse[i] != ResponsePattern[i])
{
byte[] LogResponse = new byte[DecodedResponse.Length < 0x10 ? DecodedResponse.Length : 0x10];
LogFile.Log("Qualcomm serial response: " + Converter.ConvertHexToString(LogResponse, ""), LogType.FileOnly);
LogFile.Log("Expected: " + Converter.ConvertHexToString(ResponsePattern, ""), LogType.FileOnly);
throw new BadMessageException();
}
}
}
return(DecodedResponse);
}
catch (IncompleteMessageException)
{
IsIncomplete = true;
}
catch { } // Will be rethrown as BadConnectionException
}while (IsIncomplete);
Port?.DiscardInBuffer();
if (USBDevice != null)
{
USBDevice.InputPipe.Flush();
}
throw new BadConnectionException();
}
/// </summary>
public void DiscardBuffer()/// 丢弃来自串行驱动程序的接收和发送缓冲区的数据
{
comm.DiscardInBuffer();
comm.DiscardOutBuffer();
}
/// <summary>
/// Verify if the serial port is opened and opens it if necessary
/// </summary>
/// <returns><c>true</c>, if port is opened, <c>false</c> otherwise.</returns>
/// <param name="portSpeed">Port speed.</param>
public static bool checkOpen (int portSpeed = 9600)
{
if (s_serial == null) {
//string portName = GetPortName ();
if (portName == "") {
print ("Error: Couldn't find serial port.");
return false;
} else {
if (s_debug)
print("Opening serial port: " + portName);
}
s_serial = new SerialPort (portName, portSpeed);
try {
s_serial.Open ();
} catch(System.Exception e) {
print ("Failed to open");
return false;
}
//print ("default ReadTimeout: " + s_serial.ReadTimeout);
//s_serial.ReadTimeout = 10;
// clear input buffer from previous garbage
s_serial.DiscardInBuffer ();
}
print ("Successfully opened " + s_serial.PortName);
return s_serial.IsOpen;
}
//send a command to an address and wait for the client to response (bus mode)
//response is NULL if client does not response within maximum response time (CANCMD_MAX_RESPONSE_TIME_MS)
protected receiveData com_protocol_SendDataCommandInternal(UInt32 address, byte command, byte[] data, byte dataLength, bool responseMessage, byte responsePacketNumber)
{
if (Connected() == false)
{
return(null);
}
receiveData response = null;
for (int i = 0; ((i < COM_PROTOCOL_MAX_RESPONSE_TRIES) && (response == null)); i++)
{
BufferIndex = 0;
ClearChecksum();
Buffer[BufferIndex++] = COM_PROTOCOL_COMMAND_START;
byte packetnmbr;
if (responseMessage)
{
packetnmbr = responsePacketNumber;
}
else
{
packetnmbr = GetNextPacketNumber();
}
AddToSendBuffer(packetnmbr); UpdateChecksum(packetnmbr);
if (p2p_mode == false)
{
AddToSendBuffer((byte)(address >> 24)); UpdateChecksum((byte)(address >> 24));
AddToSendBuffer((byte)(address >> 16)); UpdateChecksum((byte)(address >> 16));
AddToSendBuffer((byte)(address >> 8)); UpdateChecksum((byte)(address >> 8));
AddToSendBuffer((byte)address); UpdateChecksum((byte)address);
}
AddToSendBuffer(dataLength); UpdateChecksum(dataLength);
AddToSendBuffer(command); UpdateChecksum(command);
for (int j = 0; j < dataLength; j++)
{
if (AddToSendBuffer(data[j]) == false)
{
//send buffer overflow
return(null);
}
UpdateChecksum(data[j]);
}
if ((BufferIndex + 4) > COM_PROTOCOL_BUFFER_SIZE)
{
//send buffer overflow
return(null);
}
AddToSendBuffer((byte)(GetChecksum() >> 8));
AddToSendBuffer((byte)GetChecksum());
try
{
Serport.Write(Buffer, 0, BufferIndex);
//successfully send the data
//delete any remaining data in the input buffer
Serport.DiscardInBuffer();
}
catch (System.Exception)
{
return(null);
}
if ((address == COM_PROTOCOL_ADDRESS_TO_ALL_NO_RESPONSE) || (responseMessage == true))
{
//we are not expecting a response
break;
}
else
{
CurrentReceiveState = com_protocol_receiveStates.RCV_start;
ReceivedEscape = false;
DateTime startTime = DateTime.Now;
while ((DateTime.Now - startTime).TotalMilliseconds <= COM_PROTOCOL_MAX_RESPONSE_TIME_MS)
{
response = com_protocol_ReceiveUpdateInternal(address);
if (response != null)
{
if ((((response.address != COM_PROTOCOL_ADDRESS_TO_ALL_NO_RESPONSE) &&
(response.address != COM_PROTOCOL_ADDRESS_TO_ALL)) || p2p_mode) &&
(response.packetNumber == packetnmbr))
{
//valid response received
break;
}
else
{
response = null;
if (p2p_mode)
{
break;
}
}
}
Thread.Sleep(5);
}
}
}
CurrentReceiveState = com_protocol_receiveStates.RCV_start;
ReceivedEscape = false;
return(response);
}
private void BUT_resettodefault_Click(object sender, EventArgs e)
{
ArdupilotMega.Comms.ICommsSerial comPort = new SerialPort();
try
{
comPort.PortName = MainV2.comPort.BaseStream.PortName;
comPort.BaudRate = MainV2.comPort.BaseStream.BaudRate;
comPort.ReadTimeout = 4000;
comPort.Open();
}
catch { CustomMessageBox.Show("Invalid ComPort or in use"); return; }
lbl_status.Text = "Connecting";
if (doConnect(comPort))
{
// cleanup
doCommand(comPort, "AT&T");
comPort.DiscardInBuffer();
lbl_status.Text = "Doing Command ATI & AT&F";
doCommand(comPort, "AT&F");
doCommand(comPort, "AT&W");
lbl_status.Text = "Reset";
doCommand(comPort, "ATZ");
comPort.Close();
BUT_getcurrent_Click(sender, e);
}
else
{
// off hook
doCommand(comPort, "ATO");
lbl_status.Text = "Fail";
CustomMessageBox.Show("Failed to enter command mode");
}
if (comPort.IsOpen)
comPort.Close();
}
public void DiscardBuffers()
{
_serialPort.DiscardOutBuffer();
_serialPort.DiscardInBuffer();
}
public bool SetPort(int _port)
{
if (this.IsOpen())
{
string _com = this.GetPortName();
Console.WriteLine("When Init \"" + _com + "\" Still Connecting ...");
try
{
_serialPort.DiscardInBuffer();
_serialPort.DiscardOutBuffer();
_serialPort.Dispose();
this.Close();
Console.WriteLine("\t> " + _com + " Connection Closing!!");
}
catch (Exception ex)
{
Console.WriteLine("\t> " + _com + " Can Not Being Close!!");
}
}
string _connectionSetupRef = "";
try
{
// initialize the serial port.
_serialPort = new SerialPort(("COM" + _port.ToString()),
_baudRate,
_parity,
_dataBits,
_stopBits);
_serialPort.Handshake = _handshake;
_serialPort.Encoding = _encoding;
_serialPort.ReadBufferSize = _readBufferSize;
_serialPort.ReadTimeout = _readTimeout;
_serialPort.WriteBufferSize = _writeBufferSize;
_serialPort.WriteTimeout = _writeTimeout;
// add event
_serialPort.DataReceived += this.DataReceived;
_serialPort.ErrorReceived += this.ErrorReceived;
// open the serial port.
_serialPort.Open();
_currentPortNumber = _port;
_connectionSetupRef += "Port \t:" + this.GetPortName().ToString() + "\n";
_connectionSetupRef += "BaudRate \t:" + _baudRate + "\n";
_connectionSetupRef += "Parity \t:" + _parity.ToString() + "\n";
_connectionSetupRef += "DataBits \t:" + _dataBits + "\n";
_connectionSetupRef += "StopBits \t:" + _stopBits + "\n";
_connectionSetupRef += "Handshake \t:" + _handshake.ToString() + "\n";
_connectionSetupRef += "Encoding \t:" + _encoding.ToString() + "\n";
_connectionSetupRef += "ReadBufferSize \t:" + _readBufferSize + "\n";
_connectionSetupRef += "ReadTimeout \t:" + _readTimeout + "\n";
_connectionSetupRef += "WriteBufferSize \t:" + _writeBufferSize + "\n";
_connectionSetupRef += "WriteTimeout \t:" + _writeTimeout + "\n";
Console.Write("COM" + _port + " Connected!!\n" + _connectionSetupRef);
return(true);
}
catch (Exception ex)
{
Console.WriteLine("COM" + _port + " Not Exist\n\t" + ex.Message);
_serialPort = null;
_currentPortNumber = -1;
}
return(false);
}
/// <summary>
/// 丢弃来自串行驱动程序的接收和发送缓冲区的数据
/// </summary>
public void DiscardBuffer()
{
comPort.DiscardInBuffer();
comPort.DiscardOutBuffer();
}
private void BUT_getcurrent_Click(object sender, EventArgs e)
{
ArdupilotMega.Comms.ICommsSerial comPort = new SerialPort();
try
{
comPort.PortName = MainV2.comPort.BaseStream.PortName;
comPort.BaudRate = MainV2.comPort.BaseStream.BaudRate;
comPort.ReadTimeout = 4000;
comPort.Open();
}
catch { CustomMessageBox.Show("Invalid ComPort or in use"); return; }
lbl_status.Text = "Connecting";
if (doConnect(comPort))
{
comPort.DiscardInBuffer();
lbl_status.Text = "Doing Command ATI & RTI";
ATI.Text = doCommand(comPort, "ATI1").Trim();
RTI.Text = doCommand(comPort, "RTI1").Trim();
RSSI.Text = doCommand(comPort, "ATI7").Trim();
lbl_status.Text = "Doing Command ATI5";
string answer = doCommand(comPort, "ATI5");
string[] items = answer.Split('\n');
foreach (string item in items)
{
if (item.StartsWith("S"))
{
string[] values = item.Split(':', '=');
if (values.Length == 3)
{
Control[] controls = this.Controls.Find(values[0].Trim(), false);
if (controls.Length > 0)
{
if (controls[0].GetType() == typeof(CheckBox))
{
((CheckBox)controls[0]).Checked = values[2].Trim() == "1";
}
else
{
controls[0].Text = values[2].Trim();
}
}
}
}
}
// remote
foreach (Control ctl in this.Controls)
{
if (ctl.Name.StartsWith("RS") && ctl.Name != "RSSI")
ctl.ResetText();
}
comPort.DiscardInBuffer();
lbl_status.Text = "Doing Command RTI5";
answer = doCommand(comPort, "RTI5");
items = answer.Split('\n');
foreach (string item in items)
{
if (item.StartsWith("S"))
{
string[] values = item.Split(':', '=');
if (values.Length == 3)
{
Control[] controls = this.Controls.Find("R" + values[0].Trim(), false);
if (controls.Length == 0)
continue;
if (controls[0].GetType() == typeof(CheckBox))
{
((CheckBox)controls[0]).Checked = values[2].Trim() == "1";
}
else if (controls[0].GetType() == typeof(TextBox))
{
((TextBox)controls[0]).Text = values[2].Trim();
}
else if (controls[0].GetType() == typeof(ComboBox))
{
((ComboBox)controls[0]).SelectedText = values[2].Trim();
}
}
else
{
Console.WriteLine("Odd config line :" + item);
}
}
}
// off hook
doCommand(comPort, "ATO");
lbl_status.Text = "Done";
}
else
{
// off hook
doCommand(comPort, "ATO");
lbl_status.Text = "Fail";
CustomMessageBox.Show("Failed to enter command mode");
}
comPort.Close();
BUT_savesettings.Enabled = true;
}
public HeatmasterGroup(ISettings settings)
{
// No implementation for Heatmaster on Unix systems
if (Software.OperatingSystem.IsUnix)
{
return;
}
string[] portNames = GetRegistryPortNames();
for (int i = 0; i < portNames.Length; i++)
{
bool isValid = false;
try
{
using (SerialPort serialPort = new SerialPort(portNames[i], 38400, Parity.None, 8, StopBits.One))
{
serialPort.NewLine = ((char)0x0D).ToString();
_report.Append("Port Name: ");
_report.AppendLine(portNames[i]);
try
{
serialPort.Open();
}
catch (UnauthorizedAccessException)
{
_report.AppendLine("Exception: Access Denied");
}
if (serialPort.IsOpen)
{
serialPort.DiscardInBuffer();
serialPort.DiscardOutBuffer();
serialPort.Write(new byte[] { 0xAA }, 0, 1);
int j = 0;
while (serialPort.BytesToRead == 0 && j < 10)
{
Thread.Sleep(20);
j++;
}
if (serialPort.BytesToRead > 0)
{
bool flag = false;
while (serialPort.BytesToRead > 0 && !flag)
{
flag |= serialPort.ReadByte() == 0xAA;
}
if (flag)
{
serialPort.WriteLine("[0:0]RH");
try
{
int k = 0;
int revision = 0;
while (k < 5)
{
string line = ReadLine(serialPort, 100);
if (line.StartsWith("-[0:0]RH:", StringComparison.Ordinal))
{
revision = int.Parse(line.Substring(9), CultureInfo.InvariantCulture);
break;
}
k++;
}
isValid = revision == 770;
if (!isValid)
{
_report.Append("Status: Wrong Hardware Revision " + revision.ToString(CultureInfo.InvariantCulture));
}
}
catch (TimeoutException)
{
_report.AppendLine("Status: Timeout Reading Revision");
}
}
else
{
_report.AppendLine("Status: Wrong Startflag");
}
}
else
{
_report.AppendLine("Status: No Response");
}
serialPort.DiscardInBuffer();
}
else
{
_report.AppendLine("Status: Port not Open");
}
}
}
catch (Exception e)
{
_report.AppendLine(e.ToString());
}
if (isValid)
{
_report.AppendLine("Status: OK");
_hardware.Add(new Heatmaster(portNames[i], settings));
}
_report.AppendLine();
}
}