public void SetupDynamicNodeAllocator()
{
MessageReceived += (frame, msg, transferID) =>
{
if (frame.IsServiceMsg && frame.SvcDestinationNode != SourceNode)
{
return;
}
if (frame.TransferType != CANFrame.FrameType.anonymous)
{
return;
}
if (msg.GetType() == typeof(uavcan.uavcan_protocol_dynamic_node_id_Allocation))
{
var allocation = msg as uavcan.uavcan_protocol_dynamic_node_id_Allocation;
if (allocation.first_part_of_unique_id)
{
// first part of id
allocation.first_part_of_unique_id = false;
dynamicBytes.Clear();
dynamicBytes.AddRange(allocation.unique_id.Take(allocation.unique_id_len));
var slcan = PackageMessage(SourceNode, frame.Priority, transferID, allocation);
lock (sr_lock)
WriteToStream(slcan);
}
else
{
dynamicBytes.AddRange(allocation.unique_id.Take(allocation.unique_id_len));
allocation.unique_id = dynamicBytes.ToArray();
allocation.unique_id_len = (byte)allocation.unique_id.Length;
if (allocation.unique_id_len >= 16)
{
for (int a = 125; a >= 1; a--)
{
if (!NodeList.ContainsKey(a))
{
allocation.node_id = (byte)a;
Console.WriteLine("Allocate " + a);
break;
}
}
dynamicBytes.Clear();
}
var slcan = PackageMessage(SourceNode, frame.Priority, transferID, allocation);
lock (sr_lock)
WriteToStream(slcan);
}
}
};
}
public void Add(string word)
{
if (word.Length > 1)
{
if (NodeList.ContainsKey(word[1]))
{
var node = NodeList[word[1]];
node.Add(word.Substring(1));
}
else
{
var node = new Node();
node.Add(word.Substring(1));
NodeList.Add(word[1], node);
}
}
else
{
Count++;
}
}
/// <summary>
/// Adds a new node to the graph.
/// </summary>
/// <param name="key">The key value of the node to add.</param>
/// <param name="data">The data of the node to add.</param>
/// <returns>A reference to the TNode that was created and added to the graph.</returns>
/// <remarks>If there already exists a node in the graph with the same <b>key</b> value then an
/// <b>ArgumentException</b> exception will be thrown.</remarks>
public virtual Node AddNode(string key, object data)
{
// Make sure the key is unique
if (!nodes.ContainsKey(key))
{
Node n = new Node(key, data);
nodes.Add(n);
return(n);
}
else
{
throw new ArgumentException("Já existe um 'nó' no grafo com a chave " + key);
}
}
/// <summary>
/// Start slcan stream sending a nodestatus packet every second
/// </summary>
/// <param name="stream"></param>
public void StartSLCAN(Stream stream)
{
//cleanup
stream.Write(new byte[] { (byte)'\r', (byte)'\r', (byte)'\r' }, 0, 3);
Thread.Sleep(50);
stream.Read(new byte[1024 * 1024], 0, 1024 * 1024);
// \a = false;
// \r = true;
// close
stream.Write(new byte[] { (byte)'C', (byte)'\r' }, 0, 2);
var resp1 = ReadLine(stream);
// speed
stream.Write(new byte[] { (byte)'S', (byte)'8', (byte)'\r' }, 0, 3);
var resp2 = ReadLine(stream);
//hwid
stream.Write(new byte[] { (byte)'N', (byte)'\r' }, 0, 2);
var resp3 = ReadLine(stream);
// open
stream.Write(new byte[] { (byte)'O', (byte)'\r' }, 0, 2);
var resp4 = ReadLine(stream);
// clear status
stream.Write(new byte[] { (byte)'F', (byte)'\r' }, 0, 2);
var resp5 = ReadLine(stream);
sr = stream;
bool run = true;
// read everything
Task.Run(() =>
{
int readfail = 0;
while (run)
{
try
{
var line = ReadLine(stream);
ReadMessage(line);
readfail = 0;
}
catch (ObjectDisposedException)
{
run = false;
}
catch (IOException)
{
run = false;
}
catch
{
readfail++;
if (readfail > 500)
{
run = false;
}
}
}
});
// 1 second nodestatus send
Task.Run(() =>
{
while (run)
{
try
{
if (NodeStatus)
{
var slcan = PackageMessage(SourceNode, 20, transferID++,
new uavcan.uavcan_protocol_NodeStatus()
{
health = (byte)uavcan.UAVCAN_PROTOCOL_NODESTATUS_HEALTH_OK, mode = (byte)uavcan.UAVCAN_PROTOCOL_NODESTATUS_MODE_OPERATIONAL, sub_mode = 0, uptime_sec = (uint)(DateTime.Now - uptime).TotalSeconds, vendor_specific_status_code = 0
});
lock (sr_lock)
WriteToStream(slcan);
}
}
catch (ObjectDisposedException)
{
run = false;
}
catch
{
}
Thread.Sleep(1000);
}
});
// build nodelist
MessageReceived += (frame, msg, transferID) =>
{
if (frame.IsServiceMsg && frame.SvcDestinationNode != SourceNode)
{
return;
}
if (msg.GetType() == typeof(uavcan.uavcan_protocol_NodeStatus))
{
if (!NodeList.ContainsKey(frame.SourceNode))
{
NodeList.Add(frame.SourceNode, msg as uavcan.uavcan_protocol_NodeStatus);
NodeAdded?.Invoke(frame.SourceNode, msg as uavcan.uavcan_protocol_NodeStatus);
}
}
else if (msg.GetType() == typeof(uavcan.uavcan_protocol_GetNodeInfo_req))
{
var gnires = new uavcan.uavcan_protocol_GetNodeInfo_res();
gnires.software_version.major = (byte)Assembly.GetExecutingAssembly().GetName().Version.Major;
gnires.software_version.minor = (byte)Assembly.GetExecutingAssembly().GetName().Version.Minor;
gnires.hardware_version.major = 0;
gnires.hardware_version.unique_id = ASCIIEncoding.ASCII.GetBytes("MissionPlanner\x0\x0\x0\x0\x0\x0");
gnires.name = ASCIIEncoding.ASCII.GetBytes("org.missionplanner");
gnires.name_len = (byte)gnires.name.Length;
gnires.status = new uavcan.uavcan_protocol_NodeStatus()
{
health = (byte)uavcan.UAVCAN_PROTOCOL_NODESTATUS_HEALTH_OK, mode = (byte)uavcan.UAVCAN_PROTOCOL_NODESTATUS_MODE_OPERATIONAL, sub_mode = 0, uptime_sec = (uint)(DateTime.Now - uptime).TotalSeconds, vendor_specific_status_code = 0
};
var slcan = PackageMessage(frame.SourceNode, frame.Priority, transferID, gnires);
lock (sr_lock)
WriteToStream(slcan);
}
};
}
/// <summary>
/// </summary>
/// <param name="key"></param>
/// <returns></returns>
public bool ContainsNode(string key)
{
return(nodes.ContainsKey(key));
}
/// <summary>
/// Adds a new node to the graph.
/// </summary>
/// <param name="key">The key value of the node to add.</param>
/// <param name="data">The data of the node to add.</param>
/// <returns>A reference to the Node that was created and added to the graph.</returns>
/// <remarks>If there already exists a node in the graph with the same <b>key</b> value then an
/// <b>ArgumentException</b> exception will be thrown.</remarks>
public virtual Node AddNode(string key, object data)
{
// Make sure the key is unique
if (!nodes.ContainsKey(key))
{
Node n = new Node(key, data);
nodes.Add(n);
return(n);
}
else
{
throw new ArgumentException("There already exists a node in the graph with key " + key);
}
}
public virtual Node AddNode(string s, object o, Vector3 worldPos)
{
if (!nodeList.ContainsKey(s))
{
Node n = new Node(s, o, worldPos);
nodeList.Add(n);
//Debug.Log("IT'S POS " + n.worldPos);
return(n);
}
else
{
throw new System.Exception("There is already a node of that type: " + s);
}
}
