diff --git a/udplib.cs b/udplib.cs new file mode 100644 index 0000000..e357c03 --- /dev/null +++ b/udplib.cs @@ -0,0 +1,569 @@ +using System; +using System.Collections.Generic; +using System.Net.Sockets; +using System.Net; +using System.Text; +using System.Threading; +using nRTP; +using System.Windows.Forms; +using System.Diagnostics; +using System.Drawing; +using System.Drawing.Imaging; +using System.IO; +using System.Reflection; +using UTIL; + +namespace UDPLIB +{ + public class UDPreceive + { + Int32 localPort = 5004; + Int32 remotePort = 5006; + Thread UDPreceiveThread = null; + // Thread processFrameThread = null; + Thread sendFrameThread = null; + // Thread processDataFrameThread = null; + Stopwatch fpstimer; + private volatile bool listening = false; + UInt16 iwidth = 0; + UInt16 iheight = 0; + List udata50 = new List(); + List udata51 = new List(); + UInt32 cnt_frames = 0; + + #region Прием данных + public UDPreceive(Int32 localPortUDP) + { + localPort = localPortUDP; + for (int i = 0; i < 2500; i++) + { + udata50.Add(new Byte[0]); + udata51.Add(new Byte[0]); + } + fpstimer = new Stopwatch(); + + UDPreceiveThread = new Thread(new ThreadStart(UDPReceive7)); + listening = true; + UDPreceiveThread.Start(); + cnt_frames = 0; + } + public void Stop() + { + listening = false; + cnt_frames = 0; + } + public Boolean IsAlive + { + get + { + if (UDPreceiveThread == null || !UDPreceiveThread.IsAlive) + return false; + else + return true; + } + } + void UDPReceive7() + { + UdpClient receiver = null; + IPEndPoint endPoint = null; + EndPoint remoteIp = null; + DialogResult res = DialogResult.OK; + try + { + receiver = new UdpClient(localPort); + endPoint = new IPEndPoint(IPAddress.Any, localPort); + remoteIp = new IPEndPoint(IPAddress.Any, 0); + } + catch (Exception ee) + { + res = MessageBox.Show("Порт " + localPort.ToString() + " используется како-то программой. \r\n" + + "Прием видео невозможен. Перезапустите программу.", "Ошибка!!!", MessageBoxButtons.OK, MessageBoxIcon.Error); + System.Diagnostics.Process.GetCurrentProcess().Kill(); + return; + } + + Int32 numrow = 0; + Int32 numlist = 0; + Int32 prevnumrow = 0; + Byte[] imgarr = new Byte[1]; + Byte[] blackline = null; + fpstimer.Start(); + + while (listening) + { + try + { + UInt32 packetsize = 0; + UInt32 ssrc = 0; + if (receiver.Available == 0) + continue; + byte[] data = receiver.Receive(ref endPoint); + numrow = BitConverter.ToUInt16(data, 16); + packetsize = (UInt32)data.Length; + ssrc = BitConverter.ToUInt32(data, 8); + prevnumrow = numrow; + if (ssrc == 12345678) + { + switch (numlist) + { + case 0: + udata50[numrow] = data; + break; + case 1: + udata51[numrow] = data; + break; + } + } + if (numrow == 0 && ssrc == 12345678) // Packet 0 + { + iwidth = BitConverter.ToUInt16(data, 20); + iheight = BitConverter.ToUInt16(data, 22); + + blackline = new Byte[iwidth * 3 + 20]; + + Res.Iwidth = iwidth; + Res.Iheight = iheight; + Res.AzUpr = BitConverter.ToSingle(data, 24); + Res.ElUpr = BitConverter.ToSingle(data, 28); + Res.Course = BitConverter.ToInt16(data, 32); + Res.Roll = BitConverter.ToInt16(data, 34); + Res.Pitch = BitConverter.ToInt16(data, 36); + } + + if (data[1] == 0xC7 && iheight != 0 && iwidth != 0) + { + switch (numlist) + { + case 0: + { + Thread sendFrame0 = new Thread(new ParameterizedThreadStart(processFrame7)); + sendFrame0.Start(new thrdata(0, iwidth, iheight)); + sendFrame0.IsBackground = true; + } + numlist = 1; + break; + case 1: + { + Thread sendFrame1 = new Thread(new ParameterizedThreadStart(processFrame7)); + sendFrame1.Start(new thrdata(1, iwidth, iheight)); + sendFrame1.IsBackground = true; + } + numlist = 0; + break; + } + } + } + catch (Exception) + { + using (StreamWriter sw = new StreamWriter(System.IO.File.OpenWrite("grav01.log"))) + { + sw.WriteLine(DateTime.Now.ToString("HH:mm:ss:fff ") + "Error dll recv"); + } + } + } + receiver.Close(); + } + void processFrame7(object obj) + { + try + { + thrdata arr = obj as thrdata; + List udata = new List(); + ushort prevnumrow = 0, numrow = 0; + UInt32 cnt_frame = 0; + int iwi = 0; + int ihe = 0; + switch (arr.arr) + { + case 0: + udata.AddRange(udata50); + break; + case 1: + udata.AddRange(udata51); + break; + } + iwi = BitConverter.ToUInt16(udata[0], 20); + ihe = BitConverter.ToUInt16(udata[0], 22); + + Bitmap img = new Bitmap(iwi, ihe, System.Drawing.Imaging.PixelFormat.Format24bppRgb); + Rectangle rect = new Rectangle(0, 0, iwi, ihe); + + unsafe + { + BitmapData bmpData = img.LockBits(rect, System.Drawing.Imaging.ImageLockMode.ReadWrite, img.PixelFormat); + byte* curpos = ((Byte*)bmpData.Scan0); + + prevnumrow = numrow; + cnt_frame++; + int dd = 0; + int ff = 0; + try + { + for (int h = 1; h < ihe + 1; h++) + { + dd = h; + for (int w = 20; w < iwi * 3 + 20; w += 3) + { + ff = w; + if (udata[h].Length < (iwi * 3 + 20)) + { + curpos += 3; + break; + } + *(curpos++) = udata[h][w + 2]; + *(curpos++) = udata[h][w + 1]; + *(curpos++) = udata[h][w + 0]; + } + } + } + catch (Exception ee) + { + Console.WriteLine($"!!! {ee.Message} {dd} {ff} {udata[dd].Length}"); + return; + } + udata.Clear(); + img.UnlockBits(bmpData); + } + Res.Clearimg = (Bitmap)img.Clone(); + Res.Bmp = img; + + fpstimer.Stop(); + // Res.Fps = gk.filtered(1000F / fpstimer.ElapsedMilliseconds); + Res.Fps = 1000F / fpstimer.ElapsedMilliseconds; + Res.Frames = cnt_frames++; + // Res.Fps = 1000F / fpstimer.ElapsedMilliseconds; + fpstimer.Reset(); + fpstimer.Start(); + CallBack.Event_newimg_Handler(); + } + catch (Exception) + { + using (StreamWriter sw = new StreamWriter(System.IO.File.OpenWrite("grav01.log"))) + { + sw.WriteLine(DateTime.Now.ToString("HH:mm:ss:fff ") + "Error dll process"); + } + } + } + #endregion + } + + public class UDPTransmit + { + UdpClient transmitter = null; + IPEndPoint endPoint = null; + Point pnt; + + public UDPTransmit(String IPaddr, Int32 remotePortUDP, Point p) + { + transmitter = new UdpClient(); + endPoint = new IPEndPoint(IPAddress.Parse(IPaddr), remotePortUDP); + pnt = p; + pnt.X = Util.minmax((ushort)64, (ushort)576, (ushort)pnt.X); + pnt.Y = Util.minmax((ushort)64, (ushort)416, (ushort)pnt.Y); + + Bitmap crop = Res.Clearimg.Clone(new Rectangle(pnt.X - 64, pnt.Y - 64, 128, 128), PixelFormat.Format24bppRgb); + Res.Crop = crop; + Thread UDPtransmitThread = new Thread(new ThreadStart(UDPtransmitFrame)); + UDPtransmitThread.Start(); + } + public UDPTransmit(String IPaddr, Int32 remotePortUDP, Bitmap crop) + { + transmitter = new UdpClient(); + endPoint = new IPEndPoint(IPAddress.Parse(IPaddr), remotePortUDP); + Res.Crop = crop; + Thread UDPtransmitThread = new Thread(new ThreadStart(UDPtransmitFrame)); + UDPtransmitThread.Start(); + } + + void UDPtransmitFrame() + { + transmitter.Connect(endPoint); + + //using (Graphics g = Graphics.FromImage(Res.Bmp)) + //{ + // g.Clear(); + //}; + + // RTPMsgHeader + RTP.MH_VerPXCC = 2; + RTP.MH_MPT = 0xC6; + RTP.SeqCounter = 0; //! + RTP.MH_Timestamp = (uint)DateTimeOffset.Now.ToUnixTimeSeconds(); //! + RTP.MH_PT = 99; + RTP.MH_M = 0; + RTP.MH_SSRC = 12345678; + RTP.MH_DataLen = 36; + RTP.MH_RowNumber = 0; + RTP.MH_Offset = 0; + + // RTPVideoSupplementalData + RTP.SD_Width = (ushort)Res.Crop.Width; + RTP.SD_Height = (ushort)Res.Crop.Height; + RTP.SD_AzUpr = 0; + RTP.SD_ElUpr = 0; + RTP.SD_Course = 0; + RTP.SD_Roll = 0; + RTP.SD_Pitch = 0; + RTP.SD_FrameId = 0; + + // RTPVideoSupplementalDataExtra + RTP.DE_TLV_X = 0; + RTP.DE_TLV_Y = 0; + RTP.DE_TLV_dX = 0; + RTP.DE_TLV_dY = 0; + + RTP.MakeDataH0(); + transmitter.Send(RTP.DataH0, RTP.DataH0.Length); + + Byte[] dataudp = new Byte[20 + RTP.SD_Width * 3]; + + Byte[] dataimg = new Byte[RTP.SD_Width * 3]; + for (int i = 0; i < RTP.SD_Height; i++) + { + // RTPMsgHeader + RTP.MH_VerPXCC = 2; + if (i == RTP.SD_Height - 1) + { + RTP.MH_PT = 99; + RTP.MH_M = 1; + } + else + { + RTP.MH_PT = 99; + RTP.MH_M = 0; + } + ++RTP.SeqCounter; + RTP.MH_RowNumber = (ushort)(i + 1); + RTP.MH_DataLen = (ushort)(RTP.SD_Width * 3); + RTP.MakeDataH1(); + uint ccc = 0; + try + { + for (int j = 0; j < RTP.SD_Width; j++) + { + Color c = Res.Crop.GetPixel(j, i); + dataimg[ccc++] = c.B; + dataimg[ccc++] = c.G; + dataimg[ccc++] = c.R; + } + } + catch (Exception ee) + { + Console.WriteLine(ee.Message); + Console.WriteLine("Error 4 getpixel"); + return; + } + Array.Copy(RTP.DataH1, dataudp, RTP.DataH1.Length); + Array.Copy(dataimg, 0, dataudp, RTP.DataH1.Length, dataimg.Length); + transmitter.Send(dataudp, dataudp.Length); + } + transmitter.Close(); + CallBack.Event_crop_transmitted_Handler(); + } + } + + + public class UDPTransmitSim + { + UdpClient transmitter = null; + IPEndPoint endPoint = null; + Point pnt; + Int32 sizeX = 0; + Int32 sizeY = 0; + Int32 delayMS = 0; + public Bitmap img = null; // Resources.che1; + volatile Boolean flag = false; + + public UDPTransmitSim(String IPaddr, Int32 remotePortUDP, Int32 sizeX, Int32 sizeY, Int32 delayMS) + { + transmitter = new UdpClient(); + endPoint = new IPEndPoint(IPAddress.Parse(IPaddr), remotePortUDP); + this.sizeX = sizeX; + this.sizeY = sizeY; + this.delayMS = delayMS; + + img = new Bitmap(Res.Sim, new Size(sizeX, sizeY)); + + Thread UDPtransmitThread = new Thread(new ThreadStart(UDPtransmitFrame)); + UDPtransmitThread.Start(); + } + public UDPTransmitSim(String IPaddr, Int32 remotePortUDP, Int32 delayMS, Bitmap i) + { + transmitter = new UdpClient(); + endPoint = new IPEndPoint(IPAddress.Parse(IPaddr), remotePortUDP); + sizeX = i.Width; + sizeY = i.Height; + this.delayMS = delayMS; + + img = i; + + Thread UDPtransmitThread = new Thread(new ThreadStart(UDPtransmitFrame)); + UDPtransmitThread.Start(); + } + public void Start() + { + flag = true; + } + public void Stop() + { + flag &= false; + } + void UDPtransmitFrame() + { + while (flag) + { + transmitter.Connect(endPoint); + // RTPMsgHeader + RTP.MH_VerPXCC = 2; + RTP.MH_MPT = 0xC6; + RTP.SeqCounter = 0; //! + RTP.MH_Timestamp = (uint)DateTimeOffset.Now.ToUnixTimeSeconds(); //! + RTP.MH_PT = 99; + RTP.MH_M = 0; + RTP.MH_SSRC = 12345678; + RTP.MH_DataLen = 36; + RTP.MH_RowNumber = 0; + RTP.MH_Offset = 0; + + // RTPVideoSupplementalData + RTP.SD_Width = (UInt16)sizeX; + RTP.SD_Height = (UInt16)sizeY; + RTP.SD_AzUpr = 0; + RTP.SD_ElUpr = 0; + RTP.SD_Course = 0; + RTP.SD_Roll = 0; + RTP.SD_Pitch = 0; + RTP.SD_FrameId = 0; + + // RTPVideoSupplementalDataExtra + RTP.DE_TLV_X = 0; + RTP.DE_TLV_Y = 0; + RTP.DE_TLV_dX = 0; + RTP.DE_TLV_dY = 0; + + RTP.MakeDataH0(); + transmitter.Send(RTP.DataH0, RTP.DataH0.Length); + + Byte[] dataudp = new Byte[20 + RTP.SD_Width * 3]; + + Byte[] dataimg = new Byte[RTP.SD_Width * 3]; + for (int i = 0; i < sizeY; i++) + { + // RTPMsgHeader + RTP.MH_VerPXCC = 2; + if (i == (sizeY - 1)) + { + RTP.MH_PT = 99; + RTP.MH_M = 1; + } + else + { + RTP.MH_PT = 99; + RTP.MH_M = 0; + } + ++RTP.SeqCounter; + RTP.MH_RowNumber = (ushort)(i + 1); + RTP.MH_DataLen = (ushort)(RTP.SD_Width * 3); + RTP.MakeDataH1(); + uint ccc = 0; + try + { + for (int j = 0; j < sizeX; j++) + { + Color c = img.GetPixel(j, i); + dataimg[ccc++] = c.B; + dataimg[ccc++] = c.G; + dataimg[ccc++] = c.R; + } + } + catch (Exception ee) + { + Console.WriteLine(ee.Message); + Console.WriteLine("Error 4 getpixel"); + return; + } + Array.Copy(RTP.DataH1, dataudp, RTP.DataH1.Length); + Array.Copy(dataimg, 0, dataudp, RTP.DataH1.Length, dataimg.Length); + transmitter.Send(dataudp, dataudp.Length); + } + transmitter.Close(); + Thread.Sleep(delayMS); + } + } + } + + class thrdata + { + public uint arr; + public uint iw; + public uint ih; + public thrdata(uint a, uint w, uint h) + { + arr = a; + iw = w; + ih = h; + } + } + public static class Res + { + public static Bitmap Bmp { set; get; } + public static Bitmap Sim { set; get; } + public static Bitmap Crop { set; get; } + public static Bitmap Clearimg { set; get; } + public static UInt16 Iwidth { set; get; } + public static UInt16 Iheight { set; get; } + public static UInt32 Frames { set; get; } + public static float Fps { set; get; } + public static float AzUpr { set; get; } + public static float ElUpr { set; get; } + public static Int16 Course { set; get; } + public static Int16 Roll { set; get; } + public static Int16 Pitch { set; get; } + public static String Version + { + get + { + return "1.1.0.253"; +// return Assembly.GetExecutingAssembly().GetName().Version.ToString(); + } + } + public static String Configuration + { + get + { +#if DEBUG + return "Debug"; +#else + return "Release"; +#endif + } + } + public static Boolean status_rs { set; get; } + public static Boolean status_eth { set; get; } + } + public static class CallBack + { + public delegate void Event_newimg(); + public static Event_newimg Event_newimg_Handler; + + public delegate void Event_crop_transmitted(); + public static Event_crop_transmitted Event_crop_transmitted_Handler; + + + //public delegate void callbackEvent_bmp(Bitmap bmp); + //public delegate void callbackEvent_newimage(); + //public delegate void callbackEvent_newpacket(); + //public delegate void callbackEvent_str(String str); + //public delegate void callback_BVM2OES(); + //public delegate void callback_etalon(Point pnt); + //public static callbackEvent_bmp callbackEventHandler_bmp; + //public static callbackEvent_newimage callbackEventHandler_newimage; + //public static callbackEvent_newpacket callbackEventHandler_newpacket; + //public static callbackEvent_str callbackEventHandler_str; + //public static callback_BVM2OES callback_BVM2OESHandler; + //public static callback_etalon callback_etalonHandler; + //public static Bitmap img { get; set; } + + + } +} diff --git a/util.cs b/util.cs index 7b18449..103d9af 100644 --- a/util.cs +++ b/util.cs @@ -644,4 +644,53 @@ namespace UTIL } } } + public class GFilterRA + { + public GFilterRA() + { } + public GFilterRA(double coef, UInt16 interval) + { + _coef = coef; + _prd = interval; + } + public GFilterRA(double coef) + { + _coef = coef; + } + UInt32 millis() + { + return (UInt32)DateTimeOffset.Now.ToUnixTimeMilliseconds(); + } + public void setCoef(double coef) + { + _coef = coef; + } + public void setPeriod(UInt16 interval) + { + _prd = interval; + } + public double filteredTime(double value) + { + if (millis() - _tmr >= _prd) + { + _tmr += _prd; + filtered(value); + } + return _fil; + } + double filtered(double value) + { + return _fil += (value - _fil) * _coef; + } + void setStep(UInt16 interval) + { + _prd = interval; + } + + double _coef = 0.0F, _fil = 0.0F; + UInt32 _tmr = 0; + UInt32 _prd = 0; + }; + + }