private void clientdata(IAsyncResult ar)
{
var client = ((UdpClient)ar.AsyncState);
if (client == null || client.Client == null)
{
return;
}
try
{
var port = ((IPEndPoint)client.Client.LocalEndPoint).Port;
var udpclient = new UdpSerial(client);
var mav = new MAVLinkInterface();
mav.BaseStream = udpclient;
MainV2.comPort = mav;
MainV2.Comports.Add(mav);
mav.Open(false, true);
mav.getParamList();
//mav.getParamListAsync(mav.MAV.sysid, mav.MAV.compid).ConfigureAwait(false);
}
catch (Exception ex)
{
Log.Warning("", ex.ToString());
}
}
private void clientdata(IAsyncResult ar)
{
var client = ((UdpClient)ar.AsyncState);
if (client == null || client.Client == null)
{
return;
}
try
{
var port = ((IPEndPoint)client.Client.LocalEndPoint).Port;
var udpclient = new UdpSerial(client);
var mav = new MAVLinkInterface();
mav.BaseStream = udpclient;
MainV2.comPort = mav;
MainV2.Comports.Add(mav);
//MainV2.instance.doConnect(mav, "preset", port.ToString());
Log.Warning("", "mav init " + mav.ToString());
var hb = mav.getHeartBeat();
Log.Warning("", "getHeartBeat " + hb.ToString());
mav.setAPType(mav.MAV.sysid, mav.MAV.compid);
Log.Warning("", "setAPType " + mav.MAV.ToJSON());
Forms.Device.BeginInvokeOnMainThread(() =>
{
});
Task.Run(() =>
{
while (true)
{
try
{
while (mav.BaseStream.BytesToRead < 10 || mav.giveComport == true)
{
Thread.Sleep(20);
}
var packet = mav.readPacket();
mav.MAV.cs.UpdateCurrentSettings(null);
}
catch (Exception ex)
{
Log.Warning("", ex.ToString());
Thread.Sleep(10);
}
}
});
}
catch (Exception ex)
{
Log.Warning("", ex.ToString());
}
}
private void clientdata(IAsyncResult ar)
{
var client = ((UdpClient)ar.AsyncState);
if (client == null || client.Client == null)
{
return;
}
try
{
var port = ((IPEndPoint)client.Client.LocalEndPoint).Port;
var udpclient = new UdpSerial(client);
var mav = new MAVLinkInterface();
mav.BaseStream = udpclient;
MainV2.comPort = mav;
MainV2.Comports.Add(mav);
mav.Open(false, true);
mav.getParamList();
Forms.Device.BeginInvokeOnMainThread(() =>
{
});
}
catch (Exception ex)
{
Log.Warning("", ex.ToString());
}
}
private void clientdata(IAsyncResult ar)
{
var client = ((UdpClient)ar.AsyncState);
if (client == null || client.Client == null)
{
return;
}
try
{
var port = ((IPEndPoint)client.Client.LocalEndPoint).Port;
var udpclient = new UdpSerial(client);
var mav = new MAVLinkInterface();
mav.BaseStream = udpclient;
MainV2.comPort = mav;
MainV2.Comports.Add(mav);
mav.Open(false, true);
mav.getParamList();
Forms.Device.BeginInvokeOnMainThread(() =>
{
});
Task.Run(() =>
{
while (true)
{
try
{
while (mav.BaseStream.BytesToRead < 10 || mav.giveComport == true)
{
Thread.Sleep(20);
}
var packet = mav.readPacket();
mav.MAV.cs.UpdateCurrentSettings(null);
}
catch (Exception ex)
{
Log.Warning("", ex.ToString());
Thread.Sleep(10);
}
}
});
}
catch (Exception ex)
{
Log.Warning("", ex.ToString());
}
}
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 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);
}
});
}
};
}
public SerialInjectGPS()
{
InitializeComponent();
Instance = this;
status_line3 = null;
CMB_serialport.Items.AddRange(SerialPort.GetPortNames());
CMB_serialport.Items.Add("UDP Host");
CMB_serialport.Items.Add("UDP Client");
CMB_serialport.Items.Add("TCP Client");
CMB_serialport.Items.Add("NTRIP");
if (threadrun)
{
BUT_connect.Text = Strings.Stop;
}
splitContainer1.Panel1Collapsed = true;
// restore last port and baud - its the simple things that make life better
if (Settings.Instance.ContainsKey("SerialInjectGPS_port"))
{
CMB_serialport.Text = Settings.Instance["SerialInjectGPS_port"];
}
if (Settings.Instance.ContainsKey("SerialInjectGPS_baud"))
{
CMB_baudrate.Text = Settings.Instance["SerialInjectGPS_baud"];
}
if (Settings.Instance.ContainsKey("SerialInjectGPS_SIAcc"))
{
txt_surveyinAcc.Text = Settings.Instance["SerialInjectGPS_SIAcc"];
}
if (Settings.Instance.ContainsKey("SerialInjectGPS_SITime"))
{
txt_surveyinDur.Text = Settings.Instance["SerialInjectGPS_SITime"];
}
// restore current static state
chk_rtcmmsg.Checked = rtcm_msg;
// restore setting
if (Settings.Instance.ContainsKey("SerialInjectGPS_m8pautoconfig"))
{
chk_m8pautoconfig.Checked = bool.Parse(Settings.Instance["SerialInjectGPS_m8pautoconfig"]);
}
if (Settings.Instance.ContainsKey("SerialInjectGPS_m8p_130p"))
{
chk_m8p_130p.Checked = bool.Parse(Settings.Instance["SerialInjectGPS_m8p_130p"]);
}
loadBasePosList();
loadBasePOS();
rtcm3.ObsMessage += Rtcm3_ObsMessage;
MissionPlanner.Utilities.Tracking.AddPage(this.GetType().ToString(), this.Text);
// Init RTCM broadcast port
UdpClient udp_client = new UdpClient();
udp_client.EnableBroadcast = true;
UdpSerial udp = new UdpSerial(udp_client);
broadcastPort.BaseStream = udp;
}
private void myDataGridView1_CellContentClick(object sender, DataGridViewCellEventArgs e)
{
if (e.ColumnIndex == Go.Index)
{
try
{
var protocol = myDataGridView1[Type.Index, e.RowIndex].Value.ToString();
var direction = myDataGridView1[Direction.Index, e.RowIndex].Value.ToString();
var port = myDataGridView1[Port.Index, e.RowIndex].Value.ToString();
var extra = myDataGridView1[Extra.Index, e.RowIndex].Value.ToString();
if (protocol == "TCP")
{
if (direction == "Inbound")
{
MainV2.comPort.MirrorStream = new TcpSerial();
CMB_baudrate.SelectedIndex = 0;
listener = new TcpListener(System.Net.IPAddress.Any, int.Parse(port.ToString()));
listener.Start(0);
listener.BeginAcceptTcpClient(new AsyncCallback(DoAcceptTcpClientCallback), listener);
BUT_connect.Text = Strings.Stop;
}
else if (direction == "Outbound")
{
MainV2.comPort.MirrorStream = new TcpSerial()
{
retrys = 999999, autoReconnect = true, Host = extra, Port = port
};
CMB_baudrate.SelectedIndex = 0;
MainV2.comPort.MirrorStream.Open();
}
}
else if (protocol == "UDP")
{
if (direction == "Inbound")
{
var udp = new UdpSerial()
{
ConfigRef = "SerialOutputPassUDP", Port = port.ToString()
};
udp.client = new UdpClient(int.Parse(port));
MainV2.comPort.MirrorStream = udp;
CMB_baudrate.SelectedIndex = 0;
MainV2.comPort.MirrorStream.Open();
}
else if (direction == "Outbound")
{
var udp = new UdpSerialConnect()
{
ConfigRef = "SerialOutputPassUDPCL"
};
udp.hostEndPoint = new IPEndPoint(IPAddress.Parse(extra), int.Parse(port));
udp.client = new UdpClient();
udp.IsOpen = true;
MainV2.comPort.MirrorStream = udp;
CMB_baudrate.SelectedIndex = 0;
}
}
else if (protocol == "Serial")
{
MainV2.comPort.MirrorStream = new SerialPort();
MainV2.comPort.MirrorStream.PortName = port;
MainV2.comPort.MirrorStream.BaudRate = int.Parse(extra);
MainV2.comPort.MirrorStream.Open();
}
}
catch (Exception ex) {
CustomMessageBox.Show("Error: " + ex.Message);
}
}
}