public AP_GPS_GSOF(string portname)
{
var sport = new TcpSerial();
//new SerialPort(portname, 38400);
sport.Open();
port = sport.BaseStream;
requestBaud();
sport.BaudRate = 115200;
requestGSOF();
//requestPostion();
gsof_msg.data = new byte[1024 * 20];
read();
}
public void startslcan(byte canport)
{
but_slcanmode1.Enabled = false;
but_slcanmode2.Enabled = false;
try
{
if (!MainV2.comPort.BaseStream.IsOpen)
{
if (CustomMessageBox.Show(
"You are not currently connected via mavlink. Please make sure the device is already in slcan mode or this is the slcan serialport.",
"SLCAN", CustomMessageBox.MessageBoxButtons.OKCancel) != CustomMessageBox.DialogResult.OK)
{
return;
}
}
if (MainV2.comPort.BaseStream.IsOpen)
{
var cport = MainV2.comPort.MAV.param["CAN_SLCAN_CPORT"].Value;
MainV2.comPort.setParam((byte)MainV2.comPort.sysidcurrent, (byte)MainV2.comPort.compidcurrent,
"CAN_SLCAN_CPORT", canport, true);
if (cport == 0)
{
CustomMessageBox.Show("Reboot required" + " after setting CPORT. Please reboot!",
Strings.ERROR);
return;
}
MainV2.comPort.setParam((byte)MainV2.comPort.sysidcurrent, (byte)MainV2.comPort.compidcurrent,
"CAN_SLCAN_TIMOUT", 2, true);
MainV2.comPort.setParam((byte)MainV2.comPort.sysidcurrent, (byte)MainV2.comPort.compidcurrent,
"CAN_P" + canport + "_DRIVER", 1);
//MainV2.comPort.setParam((byte)MainV2.comPort.sysidcurrent, (byte)MainV2.comPort.compidcurrent, "CAN_SLCAN_SERNUM", 0, true); // usb
// blind send
var paramname = "CAN_SLCAN_SERNUM";
var req = new MAVLink.mavlink_param_set_t
{
target_system = (byte)MainV2.comPort.sysidcurrent,
target_component = (byte)MainV2.comPort.compidcurrent,
param_type = (byte)MainV2.comPort
.MAVlist[(byte)MainV2.comPort.sysidcurrent, (byte)MainV2.comPort.compidcurrent]
.param_types[paramname],
param_id = paramname.MakeBytesSize(16)
};
MainV2.comPort.sendPacket(req, (byte)MainV2.comPort.sysidcurrent,
(byte)MainV2.comPort.compidcurrent);
MainV2.comPort.sendPacket(req, (byte)MainV2.comPort.sysidcurrent,
(byte)MainV2.comPort.compidcurrent);
}
}
catch
{
}
{
// grab the connected port
var port = MainV2.comPort.BaseStream;
// place an invalid port in its place
if (port != null)
{
MainV2.comPort.BaseStream = new Comms.SerialPort()
{
PortName = port.PortName, BaudRate = port.BaudRate
}
}
;
//check if we started from within mavlink - if not get settings from menu and create port
if (port == null || !port.IsOpen)
{
switch (MainV2._connectionControl.CMB_serialport.Text)
{
case "TCP":
port = new TcpSerial();
break;
case "UDP":
port = new UdpSerial();
break;
case "WS":
port = new WebSocket();
break;
case "UDPCl":
port = new UdpSerialConnect();
break;
default:
port = new SerialPort()
{
PortName = MainV2._connectionControl.CMB_serialport.Text,
BaudRate = int.Parse(MainV2._connectionControl.CMB_baudrate.Text)
};
break;
}
}
if (can == null)
{
can = new uavcan();
}
can.SourceNode = 127;
can.NodeAdded += (id, msg) =>
{
this.BeginInvoke((Action) delegate
{
allnodes.Add(new UAVCANModel()
{
ID = id,
Name = "?",
Health = msg.health.ToString(),
Mode = msg.mode.ToString(),
Uptime = TimeSpan.FromSeconds(msg.uptime_sec),
VSC = msg.vendor_specific_status_code
});
uAVCANModelBindingSource.ResetBindings(false);
});
};
if (!port.IsOpen)
{
try
{
port.Open();
}
catch (Exception)
{
CustomMessageBox.Show(Strings.CheckPortSettingsOr);
return;
}
}
if (chk_log.Checked)
{
can.LogFile = Settings.Instance.LogDir + Path.DirectorySeparatorChar +
DateTime.Now.ToString("yyyy-MM-dd HH-mm-ss") + ".can";
}
var prd = new ProgressReporterDialogue();
prd.UpdateProgressAndStatus(-1, "Trying to connect");
prd.DoWork += sender => can.StartSLCAN(port.BaseStream);
prd.btnCancel.Click += (sender, args) =>
{
prd.doWorkArgs.CancelAcknowledged = true;
port.Close();
};
prd.RunBackgroundOperationAsync();
if (prd.doWorkArgs.CancelRequested || prd.doWorkArgs.ErrorMessage != null)
{
return;
}
can.SetupFileServer();
can.SetupDynamicNodeAllocator();
can.MessageReceived += (frame, msg, transferID) =>
{
if (msg.GetType() == typeof(UAVCAN.uavcan.uavcan_protocol_NodeStatus))
{
var ns = msg as UAVCAN.uavcan.uavcan_protocol_NodeStatus;
var nodes = allnodes.Where((a) => a.ID == frame.SourceNode);
if (nodes.Count() > 0 && nodes.First().Name == "?")
{
var statetracking = new UAVCAN.uavcan.statetracking();
// get node info
UAVCAN.uavcan.uavcan_protocol_GetNodeInfo_req gnireq = new UAVCAN.uavcan.uavcan_protocol_GetNodeInfo_req()
{
};
gnireq.encode(UAVCAN.uavcan.uavcan_transmit_chunk_handler, statetracking);
var slcan = can.PackageMessage(frame.SourceNode, 30, 0, gnireq);
can.WriteToStream(slcan);
}
foreach (var item in nodes)
{
switch (ns.health)
{
case (byte)UAVCAN.uavcan.UAVCAN_PROTOCOL_NODESTATUS_HEALTH_OK:
item.Health = "OK";
break;
case (byte)UAVCAN.uavcan.UAVCAN_PROTOCOL_NODESTATUS_HEALTH_WARNING:
item.Health = "WARNING";
break;
case (byte)UAVCAN.uavcan.UAVCAN_PROTOCOL_NODESTATUS_HEALTH_ERROR:
item.Health = "ERROR";
break;
case (byte)UAVCAN.uavcan.UAVCAN_PROTOCOL_NODESTATUS_HEALTH_CRITICAL:
item.Health = "CRITICAL";
break;
}
switch (ns.mode)
{
case (byte)UAVCAN.uavcan.UAVCAN_PROTOCOL_NODESTATUS_MODE_OPERATIONAL:
item.Mode = "OPERATIONAL";
break;
case (byte)UAVCAN.uavcan.UAVCAN_PROTOCOL_NODESTATUS_MODE_INITIALIZATION:
item.Mode = "INITIALIZATION";
break;
case (byte)UAVCAN.uavcan.UAVCAN_PROTOCOL_NODESTATUS_MODE_MAINTENANCE:
item.Mode = "MAINTENANCE";
break;
case (byte)UAVCAN.uavcan.UAVCAN_PROTOCOL_NODESTATUS_MODE_SOFTWARE_UPDATE:
item.Mode = "SOFTWARE_UPDATE";
break;
case (byte)UAVCAN.uavcan.UAVCAN_PROTOCOL_NODESTATUS_MODE_OFFLINE:
item.Mode = "OFFLINE";
break;
}
item.Uptime = TimeSpan.FromSeconds(ns.uptime_sec);
}
_updatePending = true;
}
else if (msg.GetType() == typeof(UAVCAN.uavcan.uavcan_protocol_GetNodeInfo_res))
{
var gnires = msg as UAVCAN.uavcan.uavcan_protocol_GetNodeInfo_res;
var nodes = allnodes.Where((a) => a.ID == frame.SourceNode);
foreach (var item in nodes)
{
item.Name = ASCIIEncoding.ASCII.GetString(gnires.name, 0, gnires.name_len);
item.HardwareVersion = gnires.hardware_version.major + "." + gnires.hardware_version.minor;
item.SoftwareVersion = gnires.software_version.major + "." + gnires.software_version.minor + "." + gnires.software_version.vcs_commit.ToString("X");
item.SoftwareCRC = gnires.software_version.image_crc;
item.HardwareUID = gnires.hardware_version.unique_id.Select(a => a.ToString("X2")).Aggregate((a, b) =>
{
return(a + " " + b);
});
item.RawMsg = gnires;
item.VSC = gnires.status.vendor_specific_status_code;
}
_updatePending = true;
}
else if (msg.GetType() == typeof(UAVCAN.uavcan.uavcan_protocol_debug_LogMessage))
{
var debug = msg as UAVCAN.uavcan.uavcan_protocol_debug_LogMessage;
this.BeginInvoke((Action) delegate()
{
DGDebug.Rows.Insert(0, new object[]
{
frame.SourceNode, debug.level.value,
ASCIIEncoding.ASCII.GetString(debug.source, 0, debug.source_len),
ASCIIEncoding.ASCII.GetString(debug.text, 0, debug.text_len)
});
if (DGDebug.Rows.Count > 100)
{
DGDebug.Rows.RemoveAt(DGDebug.Rows.Count - 1);
}
});
}
};
}
}
UAVCAN.uavcan can = new UAVCAN.uavcan();
private void BUT_getcurrent_Click(object sender, EventArgs e)
{
ICommsSerial comPort;
if (MainV2._connectionControl.CMB_serialport.Text == "TCP")
{
comPort = new TcpSerial();
}
else
{
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.Frequency freq = (uploader.Uploader.Frequency)Enum.Parse(typeof(uploader.Uploader.Frequency), doCommand(comPort, "ATI3"));
uploader.Uploader.Board board = (uploader.Uploader.Board)Enum.Parse(typeof(uploader.Uploader.Board), doCommand(comPort, "ATI2"));
ATI3.Text = freq.ToString();
ATI2.Text = board.ToString();
try
{
RTI2.Text = ((uploader.Uploader.Board)Enum.Parse(typeof(uploader.Uploader.Board), doCommand(comPort, "RTI2"))).ToString();
}
catch { }
// 8 and 9
if (freq == uploader.Uploader.Frequency.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.Frequency.FREQ_433)
{
S8.DataSource = Range(414000, 50, 460000);
RS8.DataSource = Range(414000, 50, 460000);
S9.DataSource = Range(414000, 50, 460000);
RS9.DataSource = Range(414000, 50, 460000);
}
else if (freq == uploader.Uploader.Frequency.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.Board.DEVICE_ID_RFD900 || board == uploader.Uploader.Board.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] is CheckBox)
{
((CheckBox)controls[0]).Checked = values[2].Trim() == "1";
}
else if (controls[0] is TextBox)
{
((TextBox)controls[0]).Text = values[2].Trim();
}
else if (controls[0].Name.Contains("S6")) //
{
var ans = Enum.Parse(typeof(mavlink_option), values[2].Trim());
((ComboBox)controls[0]).Text = ans.ToString();
}
else if (controls[0] is ComboBox)
{
((ComboBox)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] is CheckBox)
{
((CheckBox)controls[0]).Checked = values[2].Trim() == "1";
}
else if (controls[0] is TextBox)
{
((TextBox)controls[0]).Text = values[2].Trim();
}
else if (controls[0].Name.Contains("S6")) //
{
var ans = Enum.Parse(typeof(mavlink_option), values[2].Trim());
((ComboBox)controls[0]).Text = ans.ToString();
}
else if (controls[0] is ComboBox)
{
((ComboBox)controls[0]).Text = values[2].Trim();
}
}
else
{
/*
* if (item.Contains("S15"))
* {
* answer = doCommand(comPort, "RTS15?");
* int rts15 = 0;
* if (int.TryParse(answer, out rts15))
* {
* RS15.Enabled = true;
* RS15.Text = rts15.ToString();
* }
* }
*/
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;
}
private void BUT_savesettings_Click(object sender, EventArgs e)
{
//ICommsSerial comPort = new SerialPort();
ICommsSerial comPort;
if (MainV2._connectionControl.CMB_serialport.Text == "TCP")
{
comPort = new TcpSerial();
}
else
{
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";
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(), true);
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] is TextBox)
{
}
else if (controls[0].Name.Contains("S6")) //
{
if (((ComboBox)controls[0]).SelectedValue.ToString() != values[2].Trim())
{
string cmdanswer = doCommand(comPort, "RT" + values[0].Trim() + "=" + ((ComboBox)controls[0]).SelectedValue + "\r");
if (cmdanswer.Contains("OK"))
{
}
else
{
CustomMessageBox.Show("Set Command error");
}
}
}
else if (controls[0] is ComboBox)
{
string cmdanswer = doCommand(comPort, "RT" + values[0].Trim() + "=" + controls[0].Text + "\r");
if (cmdanswer.Contains("OK"))
{
}
else
{
CustomMessageBox.Show("Set Command error");
}
}
}
}
else
{
// bad ?ti5 line
}
}
}
// 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(), true);
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] is TextBox)
{
}
else if (controls[0].Name.Contains("S6")) //
{
if (((ComboBox)controls[0]).SelectedValue.ToString() != values[2].Trim())
{
string cmdanswer = doCommand(comPort, "AT" + values[0].Trim() + "=" + ((ComboBox)controls[0]).SelectedValue + "\r");
if (cmdanswer.Contains("OK"))
{
}
else
{
CustomMessageBox.Show("Set Command error");
}
}
}
else if (controls[0] is ComboBox)
{
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 static void Start()
{
var config = Settings.Instance[SettingsName];
if (config == null)
{
Settings.Instance[SettingsName] = connectionInfos.ToJSON();
config = Settings.Instance[SettingsName];
}
connectionInfos = config.FromJSON <List <ConnectionInfo> >();
foreach (var connectionInfo in connectionInfos)
{
if (connectionInfo.Enabled == false)
{
continue;
}
log.Info(connectionInfo.ToJSON());
if (connectionInfo.Format == ConnectionFormat.MAVLink)
{
if (connectionInfo.Protocol == ProtocolType.Udp && connectionInfo.Direction == Direction.Inbound)
{
try
{
var client = new UdpClient(connectionInfo.Port);
client.BeginReceive(clientdataMAVLink, client);
}
catch (Exception ex)
{
log.Error(ex);
}
continue;
}
if (connectionInfo.Protocol == ProtocolType.Udp && connectionInfo.Direction == Direction.Outbound)
{
try
{
// create and set default dest
var client = new UdpClient(connectionInfo.ConfigString, connectionInfo.Port);
client.SendAsync(new byte[] { 0 }, 1);
client.BeginReceive(clientdataMAVLink, client);
}
catch (Exception ex)
{
log.Error(ex);
}
continue;
}
if (connectionInfo.Protocol == ProtocolType.Tcp &&
connectionInfo.Direction == Direction.Outbound)
{
try
{
// try anything already connected
Task.Run(() =>
{
try
{
var serial = new TcpSerial();
serial.Host = connectionInfo.ConfigString;
serial.Port = connectionInfo.Port.ToString();
serial.ReadBufferSize = 1024 * 300;
serial.Open();
// sample 1.2seconds
Thread.Sleep(1200);
var btr = serial.BytesToRead;
var buffer = new byte[btr];
serial.Read(buffer, 0, buffer.Length);
//serial.Close();
var parse = new MAVLink.MavlinkParse();
var st = buffer.ToMemoryStream();
while (st.Position < st.Length)
{
var packet = parse.ReadPacket(st);
if (packet != null)
{
if (packet.msgid == (int)MAVLink.MAVLINK_MSG_ID.HEARTBEAT)
{
NewMavlinkConnection?.BeginInvoke(null, serial, null, null);
return;
}
}
}
}
catch
{
}
});
}
catch (Exception ex)
{
log.Error(ex);
}
continue;
}
if (connectionInfo.Protocol == ProtocolType.Tcp &&
connectionInfo.Direction == Direction.Inbound)
{
try
{
// try anything already connected
Task.Run(() =>
{
try
{
TcpListener listener = new TcpListener(IPAddress.Any, connectionInfo.Port);
listener.Start();
var client = listener.AcceptTcpClient();
var serial = new TcpSerial();
serial.client = client;
serial.ReadBufferSize = 1024 * 300;
serial.Open();
// sample 1.2seconds
Thread.Sleep(1200);
var btr = serial.BytesToRead;
var buffer = new byte[btr];
serial.Read(buffer, 0, buffer.Length);
//serial.Close();
var parse = new MAVLink.MavlinkParse();
var st = buffer.ToMemoryStream();
while (st.Position < st.Length)
{
var packet = parse.ReadPacket(st);
if (packet != null)
{
if (packet.msgid == (int)MAVLink.MAVLINK_MSG_ID.HEARTBEAT)
{
NewMavlinkConnection?.BeginInvoke(null, serial, null, null);
return;
}
}
}
}
catch
{
}
});
}
catch (Exception ex)
{
log.Error(ex);
}
continue;
}
if (connectionInfo.Protocol == ProtocolType.Serial &&
connectionInfo.Direction == Direction.Outbound)
{
try
{
// try anything already connected
Task.Run(() =>
{
Parallel.ForEach(SerialPort.GetPortNames(), port =>
{
try
{
var serial = new SerialPort(port, connectionInfo.Port);
serial.ReadBufferSize = 1024 * 300;
serial.Open();
// sample 1.2seconds
Thread.Sleep(1200);
var btr = serial.BytesToRead;
var buffer = new byte[btr];
serial.Read(buffer, 0, buffer.Length);
serial.Close();
var parse = new MAVLink.MavlinkParse();
var st = buffer.ToMemoryStream();
while (st.Position < st.Length)
{
var packet = parse.ReadPacket(st);
if (packet != null)
{
if (packet.msgid == (int)MAVLink.MAVLINK_MSG_ID.HEARTBEAT)
{
NewMavlinkConnection?.BeginInvoke(null, serial, null, null);
return;
}
}
}
}
catch
{
}
});
});
}
catch (Exception ex)
{
log.Error(ex);
}
continue;
}
}
else if (connectionInfo.Format == ConnectionFormat.Video)
{
if (connectionInfo.Protocol == ProtocolType.Udp && connectionInfo.Direction == Direction.Inbound)
{
try
{
var client = new UdpClient(connectionInfo.Port, AddressFamily.InterNetwork);
client.BeginReceive(clientdataVideo, client);
}
catch (Exception ex)
{
log.Error(ex);
}
continue;
}
if (connectionInfo.Protocol == ProtocolType.Tcp && connectionInfo.Direction == Direction.Inbound)
{
try
{
var client = new TcpListener(IPAddress.Any, connectionInfo.Port);
client.Start();
client.BeginAcceptTcpClient(clientdatatcpvideo, client);
}
catch (Exception ex)
{
log.Error(ex);
}
continue;
}
if (connectionInfo.Direction == Direction.Outbound)
{
NewVideoStream?.BeginInvoke(null,
connectionInfos.First(a => a == connectionInfo).ConfigString, null, null);
continue;
}
}
}
}
private void SetupSLCanPort(ICommsSerial port)
{
//check if we started from within mavlink - if not get settings from menu and create port
if (port == null || !port.IsOpen)
{
switch (MainV2._connectionControl.CMB_serialport.Text)
{
case "TCP":
port = new TcpSerial();
break;
case "UDP":
port = new UdpSerial();
break;
case "WS":
port = new WebSocket();
break;
case "UDPCl":
port = new UdpSerialConnect();
break;
default:
port = new SerialPort()
{
PortName = MainV2._connectionControl.CMB_serialport.Text,
BaudRate = int.Parse(MainV2._connectionControl.CMB_baudrate.Text)
};
break;
}
}
if (can == null)
{
can = new DroneCAN.DroneCAN();
}
can.SourceNode = 127;
can.NodeAdded += (id, msg) =>
{
this.BeginInvoke((Action) delegate
{
allnodes.Add(new DroneCANModel()
{
ID = id,
Name = "?",
Health = msg.health.ToString(),
Mode = msg.mode.ToString(),
Uptime = TimeSpan.FromSeconds(msg.uptime_sec),
VSC = msg.vendor_specific_status_code
});
uAVCANModelBindingSource.ResetBindings(false);
});
};
if (!port.IsOpen)
{
try
{
port.Open();
}
catch (Exception)
{
CustomMessageBox.Show(Strings.CheckPortSettingsOr);
return;
}
}
if (chk_log.Checked)
{
can.LogFile = Settings.Instance.LogDir + Path.DirectorySeparatorChar +
DateTime.Now.ToString("yyyy-MM-dd HH-mm-ss") + ".can";
}
var prd = new ProgressReporterDialogue();
prd.UpdateProgressAndStatus(-1, "Trying to connect");
prd.DoWork += sender => can.StartSLCAN(port.BaseStream);
prd.btnCancel.Click += (sender, args) =>
{
prd.doWorkArgs.CancelAcknowledged = true;
port.Close();
};
prd.RunBackgroundOperationAsync();
if (prd.doWorkArgs.CancelRequested || prd.doWorkArgs.ErrorMessage != null)
{
return;
}
can.SetupFileServer();
can.SetupDynamicNodeAllocator();
can.MessageReceived += (frame, msg, transferID) =>
{
if (msg.GetType() == typeof(DroneCAN.DroneCAN.uavcan_protocol_NodeStatus))
{
var ns = msg as DroneCAN.DroneCAN.uavcan_protocol_NodeStatus;
var nodes = allnodes.Where((a) => a.ID == frame.SourceNode);
if (nodes.Count() > 0 && nodes.First().Name == "?")
{
var statetracking = new DroneCAN.DroneCAN.statetracking();
// get node info
DroneCAN.DroneCAN.uavcan_protocol_GetNodeInfo_req gnireq =
new DroneCAN.DroneCAN.uavcan_protocol_GetNodeInfo_req()
{
};
gnireq.encode(DroneCAN.DroneCAN.dronecan_transmit_chunk_handler, statetracking);
var slcan = can.PackageMessageSLCAN(frame.SourceNode, 30, 0, gnireq);
can.WriteToStreamSLCAN(slcan);
}
foreach (var item in nodes)
{
switch (ns.health)
{
case (byte)DroneCAN.DroneCAN.uavcan_protocol_NodeStatus.UAVCAN_PROTOCOL_NODESTATUS_HEALTH_OK:
item.Health = "OK";
break;
case (byte)DroneCAN.DroneCAN.uavcan_protocol_NodeStatus.UAVCAN_PROTOCOL_NODESTATUS_HEALTH_WARNING:
item.Health = "WARNING";
break;
case (byte)DroneCAN.DroneCAN.uavcan_protocol_NodeStatus.UAVCAN_PROTOCOL_NODESTATUS_HEALTH_ERROR:
item.Health = "ERROR";
break;
case (byte)DroneCAN.DroneCAN.uavcan_protocol_NodeStatus.UAVCAN_PROTOCOL_NODESTATUS_HEALTH_CRITICAL:
item.Health = "CRITICAL";
break;
}
switch (ns.mode)
{
case (byte)DroneCAN.DroneCAN.uavcan_protocol_NodeStatus.UAVCAN_PROTOCOL_NODESTATUS_MODE_OPERATIONAL:
item.Mode = "OPERATIONAL";
break;
case (byte)DroneCAN.DroneCAN.uavcan_protocol_NodeStatus.UAVCAN_PROTOCOL_NODESTATUS_MODE_INITIALIZATION:
item.Mode = "INITIALIZATION";
break;
case (byte)DroneCAN.DroneCAN.uavcan_protocol_NodeStatus.UAVCAN_PROTOCOL_NODESTATUS_MODE_MAINTENANCE:
item.Mode = "MAINTENANCE";
break;
case (byte)DroneCAN.DroneCAN.uavcan_protocol_NodeStatus.UAVCAN_PROTOCOL_NODESTATUS_MODE_SOFTWARE_UPDATE:
item.Mode = "SOFTWARE_UPDATE";
break;
case (byte)DroneCAN.DroneCAN.uavcan_protocol_NodeStatus.UAVCAN_PROTOCOL_NODESTATUS_MODE_OFFLINE:
item.Mode = "OFFLINE";
break;
}
item.Uptime = TimeSpan.FromSeconds(ns.uptime_sec);
}
_updatePending = true;
}
else if (msg.GetType() == typeof(DroneCAN.DroneCAN.uavcan_protocol_GetNodeInfo_res))
{
var gnires = msg as DroneCAN.DroneCAN.uavcan_protocol_GetNodeInfo_res;
var nodes = allnodes.Where((a) => a.ID == frame.SourceNode);
foreach (var item in nodes)
{
item.Name = ASCIIEncoding.ASCII.GetString(gnires.name, 0, gnires.name_len);
item.HardwareVersion = gnires.hardware_version.major + "." + gnires.hardware_version.minor;
item.SoftwareVersion = gnires.software_version.major + "." + gnires.software_version.minor + "." +
gnires.software_version.vcs_commit.ToString("X");
item.SoftwareCRC = gnires.software_version.image_crc;
item.HardwareUID = gnires.hardware_version.unique_id.Select(a => a.ToString("X2")).Aggregate((a, b) =>
{
return(a + " " + b);
});
item.RawMsg = gnires;
item.VSC = gnires.status.vendor_specific_status_code;
}
_updatePending = true;
}
else if (msg.GetType() == typeof(DroneCAN.DroneCAN.uavcan_protocol_debug_LogMessage))
{
var debug = msg as DroneCAN.DroneCAN.uavcan_protocol_debug_LogMessage;
this.BeginInvoke((Action) delegate()
{
DGDebug.Rows.Insert(0, new object[]
{
frame.SourceNode, debug.level.value,
ASCIIEncoding.ASCII.GetString(debug.source, 0, debug.source_len),
ASCIIEncoding.ASCII.GetString(debug.text, 0, debug.text_len)
});
if (DGDebug.Rows.Count > 100)
{
DGDebug.Rows.RemoveAt(DGDebug.Rows.Count - 1);
}
});
}
};
}
