无人机状态信息UDP转发源码
本工程师通过本机固定端口接收无人机发送来的状态数据，并按照其自己需求将无人机数据进行在封装，并将其发送出去！
相关函数：本工程包括1）无人机状态参数接收，再装发；2）参数的高低位转换；3）结构体的序列化和反序列化
1、数据结构体
无人机数据格式UAV_DATA：
[StructLayout(LayoutKind.Sequential, CharSet = CharSet.Ansi, Pack = 1)]
public struct UAV_DATA
{
public uint time_boot_ms; ///< 时间戳,表示系统启动开始的秒数
public Int32 lat; ///< 飞机纬度 /(double)1E7;
public Int32 lon; ///< 飞机经度 /(double)1E7;
public Int32 alt; ///< 飞机海拔高度 /1000.0
public Int32 homelat; ///< home点纬度 /(double)1E7;
public Int32 homelon; ///< home点经度 /(double)1E7;
public Int32 homealt; ///< home点海拔高度 /1000.0
public Int16 vx; ///< 飞机水平速度 /100.0
public Int16 vz; ///< 飞机垂直速度 /100.0
public Int16 distance; ///< 目标物距离/100.0
public ushort vol; ///< 电池电压 /1000.0f
///
}转发封包数据结构UAV_State_parameters
[StructLayout(LayoutKind.Sequential,CharSet = CharSet.Ansi,Pack = 1)]
public struct UAV_State_parameters
{
//网络报文头
public ushort u_Len;
//[MarshalAs(UnmanagedType.ByValArray, SizeConst = 4)]
public Int32 u_Sour;
//[MarshalAs(UnmanagedType.ByValArray, SizeConst = 4)]
public int u_Dest;
public byte u_Chnl;
public byte u_StaN_Tx;
public byte u_StaN_Rx;
public ushort u_Pck;
//通用报文头
public ushort u_MsgID;
public Int32 u_SourcePlatCode;
public Int32 u_SourceDeviceCode;
public Int32 u_ReceivePlatCode;
public Int32 u_ReceiveDeviceCode;
public ushort u_SerialNum; //流水号
public ushort u_CRC;
public byte u_RowNum;
public Int32 u_CreateTime;
public byte u_DataLength;
//数据
public Int32 u_CollectTime;
public ushort u_RowLength;
//数据包部分
public float u_HorizSpeed;
public float u_VertSpeed;
public byte u_Voltage;
public float u_DeviceAltitude;
public Int32 u_PitchAngle;
public Int32 u_DeviceLongitude;
public Int32 u_DeviceLatitude;
public float u_OperatAltitude;
public Int32 u_OperatLongitude;
public Int32 u_OperatLatitude;
public float u_TargetRange;
public Int32 u_TargetLongitude;
public Int32 u_TargetLatitude;
}2、数据接收转发函数
数据接收
private void DATA_input_Click(object sender, EventArgs e)
{
string strHostIP = "";
IPHostEntry oIPHost = Dns.GetHostEntry(Environment.MachineName);
if (oIPHost.AddressList.Length > 0)
strHostIP = oIPHost.AddressList[0].ToString();
this.textBox1.Text = strHostIP;//本机IP
System.Windows.Forms.Control.CheckForIllegalCrossThreadCalls = false;
if (udpClient != null)
{
UdpThread.Abort();
Thread.Sleep(TimeSpan.FromMilliseconds(500d));
udpClient.Close();
}
try
{
udpClient = new UdpClient(int.Parse(textBox2.Text));
UdpThread = new Thread(new ThreadStart(UdpReciveThread));
UdpThread.Start();
//UdpThread.Abort();
}
catch (Exception y)
{
MessageBox.Show(this, y.Message, "Error", MessageBoxButtons.OK, MessageBoxIcon.Error);
this.Dispose(true);
}
}接收数据线程函数
//接收数据线程
void UdpReciveThread()
{
IPHostEntry oIPHost = Dns.GetHostEntry(Environment.MachineName);
//IPEndPoint remoteHost = new IPEndPoint(oIPHost.AddressList[0], 6569);
IPEndPoint remoteHost = new IPEndPoint(IPAddress.Any, 0);
UAV_DATA Utest = new UAV_DATA();
while (udpClient != null && Thread.CurrentThread.ThreadState.Equals(ThreadState.Running))
{
try
{
byte[] buf = udpClient.Receive(ref remoteHost);
byte[] testbuf = new byte[36];
for( int i = 0 ; i <36 ;i++)
{
testbuf[i] = buf[i+6];
}
Utest = rawDeserialize(testbuf);
UAV_received.time_boot_ms = Utest.time_boot_ms; //< 时间戳,表示系统启动开始的秒数
UAV_received.lat = Utest.lat ; //< 飞机纬度 /(double)1E7;
UAV_received.lon = Utest.lon; //< 飞机经度 /(double)1E7;
UAV_received.alt = Utest.alt; //< 飞机海拔高度 /1000.0
UAV_received.homelat = Utest.homealt; //< home点纬度 /(double)1E7;
UAV_received.homelon = Utest.homelon; //< home点经度 /(double)1E7;
UAV_received.homealt = Utest.homelat; //< home点海拔高度 /1000.0
UAV_received.vx = Utest.vx ; //< 飞机水平速度 /100.0
UAV_received.vz = Utest.vz; //< 飞机垂直速度 /100.0
UAV_received.distance = Utest.distance; //< 目标物距离/100.0
UAV_received.vol = Utest.vol; //< 电池电压 /1000.0f
byte[] tebuf = rawSerialize(UAV_received);
string bufs = System.Text.Encoding.UTF8.GetString(tebuf );
}
catch (Exception y)
{
}
//设置读取时间
Thread.Sleep(1000);
}
textBox5.Text += "结束..." + (char)13;
}转发数据
private void DATA_send_Click(object sender, EventArgs e)
{
// udpClient = new UdpClient(int.Parse(textBox4.Text));
m_ip_local = textBox1.Text;
m_ip_send = textBox3.Text;
System.Windows.Forms.Control.CheckForIllegalCrossThreadCalls = false;
p_send_control = 1; //确认转发
p_SerialNum = 0; //流水号
UdpThread_2 = new Thread(new ThreadStart(UDPSendthread));
UdpThread_2.Start();
}转发线程函数
//发送数据线程
void UDPSendthread()
{
UdpClient uc = new UdpClient(textBox3.Text, int.Parse(textBox4.Text));
UAV_DATA USend = new UAV_DATA();
USend = UAV_received;
string str = " ";
while (uc != null && Thread.CurrentThread.ThreadState.Equals(ThreadState.Running) && p_send_control == 1)
{
//IP赋值
UAV_send_Data.u_Sour =SwapInt32((Int32)IpToInt(m_ip_send));
UAV_send_Data.u_Dest = SwapInt32((Int32)IpToInt(m_ip_send));
//
// int p_time;
UAV_send_Data.u_CreateTime =SwapInt32(1473321129);
//数据
UAV_send_Data.u_CollectTime = SwapInt32(1473321129);
// UAV_send_Data.u_Dest = 0x01;
if (p_SerialNum < 65535)
{
p_SerialNum += 1; //流水号
}
else
{
p_SerialNum = 0; //归零
}
if (p_num_lot < 100)
{
p_num_lot += 1;
}
else {
p_num_lot = 5;
}
UAV_send_Data.u_SerialNum = SwapShort16(p_SerialNum); //流水号赋值
UAV_send_Data.u_HorizSpeed =(float) 1.0; //< 飞机水平速度 /100.0
UAV_send_Data.u_VertSpeed = (float)1.0; //< 飞机垂直速度 /100.0
UAV_send_Data.u_Voltage = 0x01; //< 电池电压 /1000.0f
UAV_send_Data.u_DeviceAltitude = (float)1.0; //< 飞机海拔高度 /1000.0
UAV_send_Data.u_PitchAngle = SwapInt32(100000);
UAV_send_Data.u_DeviceLongitude = SwapInt32(UAV_received.lon + p_num_lot * 100); //< 飞机经度 /(double)1E7;
UAV_send_Data.u_DeviceLatitude = SwapInt32(UAV_received.lat + p_num_lot * 100); //< 飞机纬度 /(double)1E7;;
UAV_send_Data.u_OperatAltitude = (float)1.0; //< home点海拔高度 /1000.0
UAV_send_Data.u_OperatLongitude = SwapInt32(UAV_received.homelon); //< home点经度 /(double)1E7;
UAV_send_Data.u_OperatLatitude = SwapInt32(UAV_received.homelat); //< home点纬度 /(double)1E7;
UAV_send_Data.u_TargetRange = (float)1.0; //< 目标物距离/100.0
UAV_send_Data.u_TargetLongitude = SwapInt32(UAV_received.homelon); //目标精度
UAV_send_Data.u_TargetLatitude = SwapInt32(UAV_received.homelat); //目标纬度
try
{
byte[] sendbuf = rawSerialize(UAV_send_Data);
// int num = 112;
// byte[] sendbuffs
uc.Send(sendbuf, sendbuf.Length);
// byte[] testbuf = rawSerialize(USend);
// string bufs = System.Text.Encoding.UTF8.GetString(testbuf);
string str1 = " ";
for (int i = 0; i < 90;i++ )
{
str1 += sendbuf[i].ToString();
}
str = str1;
//textBox5.Text += Environment.NewLine;
textBox5.Text = "本次发送的数据：" + Environment.NewLine + str;
// textBox5.Text += bufs + Environment.NewLine;
}
catch (Exception y)
{
}
//设置发送间隔时间
Thread.Sleep(1000);
}
textBox5.Text += str + "结束..." + Environment.NewLine; ;
}源码地址：
http://download.csdn.net/download/u011326478/10247537