Exemplo n.º 1
0
        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);
                    }
                }
            };
        }
Exemplo n.º 2
0
 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++;
     }
 }
Exemplo n.º 3
0
 /// <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);
     }
 }
Exemplo n.º 4
0
        /// <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);
                }
            };
        }
Exemplo n.º 5
0
 /// <summary>
 /// </summary>
 /// <param name="key"></param>
 /// <returns></returns>
 public bool ContainsNode(string key)
 {
     return(nodes.ContainsKey(key));
 }
Exemplo n.º 6
0
 /// <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);
     }
 }
Exemplo n.º 7
0
 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);
     }
 }