public static UdpUser ConnectTo(string hostname, int port)
{
var connection = new UdpUser();
connection.client.Connect(hostname, port);
return(connection);
}
static void Main(string[] args)
{
//command server
var commandServer = new UdpListener(9000);
Task.Factory.StartNew(async() => {
while (true)
{
var received = await commandServer.Receive();
Console.WriteLine("Server:" + received.Message);
}
});
//video server
var videoServer = new UdpListener(6138);
Task.Factory.StartNew(async() => {
while (true)
{
var received = await videoServer.Receive();
Console.WriteLine("video:" + received.Message);
}
});
//messages server
var server = new UdpListener(6525);
//create a new client
var client = UdpUser.ConnectTo("192.168.10.1", 8889);
Dictionary <int, string> cmdIdLookup = new Dictionary <int, string>
{
{ 26, "Wifi" }, //2 bytes. Strength, Disturb.
{ 53, "Light" }, //1 byte?
{ 86, "FlyData" },
{ 4176, "Data" }, //wtf?
};
//wait for reply messages from server and send them to console
Task.Factory.StartNew(async() => {
while (true)
{
try
{
var received = await client.Receive();
//var received = await server.Receive();
int cmdId = ((int)received.bytes[5] | ((int)received.bytes[6] << 8));
var cmdName = "unknown";
if (cmdIdLookup.ContainsKey(cmdId))
{
cmdName = cmdIdLookup[cmdId];
}
var dataStr = BitConverter.ToString(received.bytes.Skip(9).Take(30).ToArray()).Replace("-", " ");
//Debug printing of select command messages.
if (cmdId != 26 && cmdId != 86 && cmdId != 53 && cmdId != 4176 && cmdId != 4177 && cmdId != 4178)
{
// if (cmdId == 86)
Console.WriteLine("cmdId:" + cmdId + "(0x" + cmdId.ToString("X2") + ")" + cmdName + " " + dataStr);
}
}
catch (Exception ex)
{
Debug.Write(ex);
}
}
});
byte[] connectPacket = Encoding.UTF8.GetBytes("conn_req:\x00\x00");
connectPacket[connectPacket.Length - 2] = 0x96;
connectPacket[connectPacket.Length - 1] = 0x17;
client.Send(connectPacket);
//var iframePacket = new byte[] { 0xcc, 0x58, 0x00, 0x7c, 0x60, 0x25, 0x00, 0x00, 0x00, 0x6c, 0x95 };
//client.Send(iframePacket);
//Start polling joystick
Task.Factory.StartNew(async() => {
initJoystick();
});
//Send joystick updates.
Task.Factory.StartNew(async() => {
while (true)
{
try
{
var rx = ((float)joyState.RotationX / 0x8000) - 1;
var ry = -(((float)joyState.RotationY / 0x8000) - 1);
var lx = ((float)joyState.X / 0x8000) - 1;
var ly = -(((float)joyState.Y / 0x8000) - 1);
var deadBand = 0.15f;
rx = Math.Abs(rx) < deadBand ? 0.0f : rx;
ry = Math.Abs(ry) < deadBand ? 0.0f : ry;
lx = Math.Abs(lx) < deadBand ? 0.0f : lx;
ly = Math.Abs(ly) < deadBand ? 0.0f : ly;
var limit = 0.5f;//Slow down while testing.
rx = rx * limit;
ry = ry * limit;
//lx = lx * limit;
//dont limit up/down so we can force land.
//ly = ly * 0.5f;
var packet = createJoyPacket(rx, ry, lx, ly, 0.0f);
//Console.WriteLine(rx + " " + ry + " " + lx + " " + ly);
client.Send(packet);
Thread.Sleep(50);//Often enough?
if (joyState.Buttons[3])
{
var takeOffPacket = new byte[] { 0xcc, 0x58, 0x00, 0x7c, 0x68, 0x54, 0x00, 0xe4, 0x01, 0xc2, 0x16 };
client.Send(takeOffPacket);
Thread.Sleep(250);
}
if (joyState.Buttons[0])
{
var landPacket = new byte[] { 0xcc, 0x60, 0x00, 0x27, 0x68, 0x55, 0x00, 0xe5, 0x01, 0x00, 0xba, 0xc7 };
client.Send(landPacket);
Thread.Sleep(250);
}
}
catch (Exception ex)
{
Debug.Write(ex);
}
}
});
//type ahead :-)
string read;
do
{
read = Console.ReadLine();
//client.Send(read);
} while (read != "quit");
Environment.Exit(0);
}