string doCommand(ArdupilotMega.ICommsSerial comPort, string cmd, int level = 0)
{
if (!comPort.IsOpen)
{
return("");
}
comPort.ReadTimeout = 1000;
// setup to known state
comPort.Write("\r\n");
// alow some time to gather thoughts
Sleep(100);
// ignore all existing data
comPort.DiscardInBuffer();
lbl_status.Text = "Doing Command " + cmd;
Console.WriteLine("Doing Command " + cmd);
// write command
comPort.Write(cmd + "\r\n");
// read echoed line or existing data
string temp;
try
{
temp = Serial_ReadLine(comPort);
}
catch { temp = comPort.ReadExisting(); }
Console.WriteLine("cmd " + cmd + " echo " + temp);
// delay for command
Sleep(500);
// get responce
string ans = "";
while (comPort.BytesToRead > 0)
{
try
{
ans = ans + Serial_ReadLine(comPort) + "\n";
}
catch { ans = ans + comPort.ReadExisting() + "\n"; }
Sleep(50);
if (ans.Length > 500)
{
break;
}
}
Console.WriteLine("responce " + level + " " + ans.Replace('\0', ' '));
// try again
if (ans == "" && level == 0)
{
return(doCommand(comPort, cmd, 1));
}
return(ans);
}
string Serial_ReadLine(ArdupilotMega.ICommsSerial comPort)
{
StringBuilder sb = new StringBuilder();
DateTime Deadline = DateTime.Now.AddMilliseconds(comPort.ReadTimeout);
while (DateTime.Now < Deadline)
{
if (comPort.BytesToRead > 0)
{
byte data = (byte)comPort.ReadByte();
sb.Append((char)data);
if (data == '\n')
{
break;
}
}
}
return(sb.ToString());
}
bool doConnect(ArdupilotMega.ICommsSerial comPort)
{
// clear buffer
comPort.DiscardInBuffer();
// setup a known enviroment
comPort.Write("\r\n");
// wait
Sleep(1100);
// send config string
comPort.Write("+++");
// wait
Sleep(1100);
// check for config responce "OK"
Console.WriteLine("Connect btr " + comPort.BytesToRead + " baud " + comPort.BaudRate);
string conn = comPort.ReadExisting();
Console.WriteLine("Connect first responce " + conn.Replace('\0', ' ') + " " + conn.Length);
if (conn.Contains("OK"))
{
//return true;
}
else
{
// cleanup incase we are already in cmd mode
comPort.Write("\r\n");
}
string version = doCommand(comPort, "ATI");
Console.Write("Connect Version: " + version.Trim() + "\n");
if (version.Contains("on HM-TRP"))
{
return(true);
}
return(false);
}
