Skip to content

Instantly share code, notes, and snippets.

@peteroid
Last active January 4, 2017 00:34
Show Gist options
  • Save peteroid/0ef71fa27b47698a19d799d2c392e682 to your computer and use it in GitHub Desktop.
Save peteroid/0ef71fa27b47698a19d799d2c392e682 to your computer and use it in GitHub Desktop.
UDP Network Manager for connect to Arduino and receive the sensor data
using UnityEngine;
using System.Collections.Generic;
using System;
using System.Text;
using System.Net;
using System.Net.Sockets;
using System.Threading;
using System.Linq;
public class UDPNetworkManager : MonoBehaviour {
public const int UDP_PORT_SCAN_START = 12345;
public const int UDP_PORT_SCAN_SIZE = 500;
public const int UDP_SENDER_PORT = 12346;
public bool isThreading = false;
public string senderIP = "192.168.0.101";
public string lastReceivedUDPPacket = "";
public char[] lastReceivedUDPPacketCharArray;
public Quaternion lastQuaternion;
private int receiverPort = -1;
private IPEndPoint remoteEndPoint;
private Thread receiveThread;
private UdpClient receiverClient, senderClient;
public static Encoding extendedASCIIEncoding = Encoding.GetEncoding(28591);
private bool hasInit = false;
private IPEndPoint anyIP = new IPEndPoint(IPAddress.Any, 0);
private bool _isThreading = false;
private static List<int> allocatedPorts = new List<int>();
/**************** PUBLIC METHOD *****************/
public void StartReceivingData() {
SendString(receiverPort.ToString());
}
public void StopReceivingData() {
SendString("00000");
}
public string GetLatestUDPPacket() {
return lastReceivedUDPPacket;
}
public bool GetLatestButtonClicked() {
return lastReceivedUDPPacketCharArray[1] == 0;
}
public Quaternion GetLatestQuaternion() {
lastQuaternion = GetQuatFromString(lastQuaternion);
return lastQuaternion;
}
/**************** EVENT *****************/
public void Start() {
Init();
}
public void Update() {
if (!_isThreading) {
TryUpdateLatestUDPPacket();
}
}
void OnDisable() {
if (_isThreading) {
if (receiveThread != null)
receiveThread.Abort();
StopReceivingData();
Destroy(this);
}
}
// abort the thread before quiting the app
public void OnApplicationQuit() {
Debug.Log("Quiting...");
if (_isThreading && receiveThread != null) {
receiveThread.Abort();
}
if (receiverClient != null) {
receiverClient.Close();
}
}
/**************** PRIVATE METHOD *****************/
// init
private void Init() {
if (hasInit) return;
Debug.Log("UDP init");
_isThreading = isThreading;
receiverPort = TryGetFreeUDPPort();
remoteEndPoint = new IPEndPoint(IPAddress.Parse(senderIP), UDP_SENDER_PORT);
senderClient = new UdpClient();
receiverClient = new UdpClient(receiverPort);
receiverClient.EnableBroadcast = true;
receiverClient.Client.Blocking = false;
if (_isThreading) {
receiveThread = new Thread(new ThreadStart(ReceiveData));
receiveThread.IsBackground = true;
receiveThread.Start();
}
hasInit = true;
}
// try to get a free UDP port
private int TryGetFreeUDPPort() {
IEnumerable<int> range = Enumerable.Range(UDP_PORT_SCAN_START, UDP_PORT_SCAN_SIZE);
IEnumerable<int> portsInUse =
from p in range
join used in System.Net.NetworkInformation.IPGlobalProperties.GetIPGlobalProperties().GetActiveUdpListeners()
on p equals used.Port
select p;
var firstFreeUDPPortInRange = range.Except(portsInUse).Except(allocatedPorts).FirstOrDefault();
if (firstFreeUDPPortInRange > 0) {
// do stuff
allocatedPorts.Add(firstFreeUDPPortInRange);
return firstFreeUDPPortInRange;
} else {
// complain about lack of free ports?
return -1;
}
}
// receive thread
private void ReceiveData() {
while (true) {
TryUpdateLatestUDPPacket();
}
}
private void TryUpdateLatestUDPPacket() {
if (receiverClient.Available > 0) {
byte[] data;
// read until the last packet
do {
data = receiverClient.Receive(ref anyIP);
} while (receiverClient.Available > 0);
string text = extendedASCIIEncoding.GetString(data);
// latest UDPpacket
lastReceivedUDPPacket = text;
lastReceivedUDPPacketCharArray = text.ToCharArray();
}
}
private void SendString(string message) {
try {
byte[] data = Encoding.UTF8.GetBytes(message);
senderClient.Send(data, data.Length, remoteEndPoint);
} catch (Exception err) {
print(err.ToString());
}
}
private Quaternion GetQuatFromString(Quaternion defaultQuat) {
char[] packet = lastReceivedUDPPacketCharArray;
//string str = "";
//foreach (char c in packet) {
// str += (int)c + " ";
//}
//Debug.Log(str);
if (packet.Length != 14 || packet[0] != 36) {
return defaultQuat;
} else {
float[] q = new float[4];
q[0] = ((packet[2] << 8) | packet[3]) / 16384f;
q[1] = ((packet[4] << 8) | packet[5]) / 16384f;
q[2] = ((packet[6] << 8) | packet[7]) / 16384f;
q[3] = ((packet[8] << 8) | packet[9]) / 16384f;
for (int i = 0; i < 4; i++) if (q[i] >= 2) q[i] = -4 + q[i];
//Debug.Log (quat.x + " " + quat.y + " " + quat.z + " " + quat.w);
return new Quaternion(q[1], q[2], q[3], q[0]);
}
}
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment