private void BUT_getcurrent_Click(object sender, EventArgs e)
{
ArdupilotMega.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;
}
private void BUT_savesettings_Click(object sender, EventArgs e)
{
ArdupilotMega.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();
}
/// <summary>
/// Detects APM board version
/// </summary>
/// <param name="port"></param>
/// <returns> (1280/2560/2560-2)</returns>
public static string DetectBoard(string port)
{
SerialPort serialPort = new SerialPort();
serialPort.PortName = port;
if (serialPort.IsOpen)
serialPort.Close();
serialPort.DtrEnable = true;
serialPort.BaudRate = 57600;
serialPort.Open();
System.Threading.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++;
System.Threading.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();
return "1280";
}
}
}
serialPort.Close();
Console.WriteLine("Not a 1280");
System.Threading.Thread.Sleep(500);
serialPort.DtrEnable = true;
serialPort.BaudRate = 115200;
serialPort.Open();
System.Threading.Thread.Sleep(100);
a = 0;
while (a < 4)
{
byte[] temp = new byte[] { 0x6, 0, 0, 0, 0 };
temp = ArduinoDetect.genstkv2packet(serialPort, temp);
a++;
System.Threading.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.CurrentUICulture.Name != "zh-Hans")
{
ObjectQuery query = new ObjectQuery("SELECT * FROM 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.
if (obj2["Dependent"].ToString().Contains(@"USB\\VID_2341&PID_0010"))
{
return "2560-2";
}
}
return "2560";
}
else
{
if (DialogResult.Yes == MessageBox.Show("Is this a APM 2?", "APM 2", MessageBoxButtons.YesNo))
{
return "2560-2";
}
else
{
return "2560";
}
}
}
}
catch { }
}
serialPort.Close();
Console.WriteLine("Not a 2560");
return "";
}
static byte[] readpacket(SerialPort serialPort)
{
byte[] temp = new byte[4000];
byte[] mes = new byte[2] { 0x0, 0xC0 }; // fail
int a = 7;
int count = 0;
serialPort.ReadTimeout = 1000;
while (count < a)
{
//Console.WriteLine("count {0} a {1} mes leng {2}",count,a,mes.Length);
try
{
temp[count] = (byte)serialPort.ReadByte();
}
catch { break; }
//Console.Write("{1}", temp[0], (char)temp[0]);
if (temp[0] != 0x1b)
{
count = 0;
continue;
}
if (count == 3)
{
a = (temp[2] << 8) + temp[3];
mes = new byte[a];
a += 5;
}
if (count >= 5)
{
mes[count - 5] = temp[count];
}
count++;
}
//Console.WriteLine("read ck");
try
{
temp[count] = (byte)serialPort.ReadByte();
}
catch { }
count++;
Array.Resize<byte>(ref temp, count);
//Console.WriteLine(this.BytesToRead);
return mes;
}
static byte[] genstkv2packet(SerialPort serialPort, byte[] message)
{
byte[] data = new byte[300];
byte ck = 0;
data[0] = 0x1b;
ck ^= data[0];
data[1] = 0x1;
ck ^= data[1];
data[2] = (byte)((message.Length >> 8) & 0xff);
ck ^= data[2];
data[3] = (byte)(message.Length & 0xff);
ck ^= data[3];
data[4] = 0xe;
ck ^= data[4];
int a = 5;
foreach (byte let in message)
{
data[a] = let;
ck ^= let;
a++;
}
data[a] = ck;
a++;
serialPort.Write(data, 0, a);
//Console.WriteLine("about to read packet");
byte[] ret = ArduinoDetect.readpacket(serialPort);
//if (ret[1] == 0x0)
{
//Console.WriteLine("received OK");
}
return ret;
}
/// <summary>
/// detects STK version 1 or 2
/// </summary>
/// <param name="port">comportname</param>
/// <returns>string either (1280/2560) or "" for none</returns>
public static string DetectVersion(string port)
{
SerialPort serialPort = new SerialPort();
serialPort.PortName = port;
if (serialPort.IsOpen)
serialPort.Close();
serialPort.DtrEnable = true;
serialPort.BaudRate = 57600;
serialPort.Open();
System.Threading.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++;
System.Threading.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();
return "1280";
}
}
}
serialPort.Close();
Console.WriteLine("Not a 1280");
System.Threading.Thread.Sleep(500);
serialPort.DtrEnable = true;
serialPort.BaudRate = 115200;
serialPort.Open();
System.Threading.Thread.Sleep(100);
a = 0;
while (a < 4)
{
byte[] temp = new byte[] { 0x6, 0, 0, 0, 0 };
temp = ArduinoDetect.genstkv2packet(serialPort, temp);
a++;
System.Threading.Thread.Sleep(50);
try
{
if (temp[0] == 6 && temp[1] == 0 && temp.Length == 2)
{
serialPort.Close();
return "2560";
}
}
catch { }
}
serialPort.Close();
Console.WriteLine("Not a 2560");
return "";
}
