From 8a73f602b51d5d017bd20d534a370dbd81a041ea Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=D0=94=D0=B5=D0=BD=D0=B8=D1=81=20=D0=9A=D1=83=D0=B7=D0=BD?= =?UTF-8?q?=D0=B5=D1=86=D0=BE=D0=B2?= Date: Fri, 27 Oct 2023 09:22:15 +0300 Subject: [PATCH] 20231027-1 --- rtp.cs | 260 ++++++++++++++++++++++++++++++++++++++++++++++++++++++ udplib.cs | 6 +- 2 files changed, 263 insertions(+), 3 deletions(-) diff --git a/rtp.cs b/rtp.cs index f1b3570..14286e9 100644 --- a/rtp.cs +++ b/rtp.cs @@ -105,6 +105,265 @@ namespace nRTP public static Int16 SD_Pitch; // Тангаж БЛА (0,01 град) public static Int16 SD_FrameId; + public static UInt16 rez0; // координата центра цели в растре изображения по горизонтали + public static UInt16 rez1; //координата центра цели в растре изображения по вертикали + + public static Byte rez2; // размер цели в растре эталонного изображения по горизонтали + public static Byte rez3; // размер цели в растре эталонного изображения по вертикали + + public static Byte SD_rejim_oes // Режим ОЭС: «0» – Обзор, «1» – АС + { + get => _rejim_oes; + set => _rejim_oes = value; + } + public static Byte SD_zahvat // Захват: «0» – отсутствие захвата, «1» – налачие захвата + { + get => _zahvat; + set => _zahvat = value; + } + public static Byte SD_color // Цвет изображения: «1» – цветное, «0» – монохромное + { + get => _color; + set => _color = value; + } + public static Byte SD_Status + { + get + { + _status |= (Byte)(_rejim_oes << 0); + _status |= (Byte)(_zahvat << 1); + _status |= (Byte)(_color << 2); + return _status; + } + set + { + _status = value; + _rejim_oes = (Byte)((value >> 0) & 0x01); + _zahvat = (Byte)((value >> 1) & 0x01); + _color = (Byte)((value >> 2) & 0x01); + } + } + public static Byte SD_rezerv; + + //RTPVideoSupplementalDataExtra + public static UInt32 DE_TLV_X; // Линия визирования цели в растроввых координатах + public static UInt32 DE_TLV_Y; // + public static UInt32 DE_TLV_dX; // поправки для линии визирования цели в растроввых координатах + public static UInt32 DE_TLV_dY; // + + public static UInt32 SeqCounter + { + get + { + return (UInt32)((_SeqCounter_Hi << 16) | _SeqCounter_Low); + } + set + { + _SeqCounter_Low = (UInt16)value; + _SeqCounter_Hi = (UInt16)((value >> 16) & 0xFFFF); + } + } + + public static Byte[] DataH0 = new Byte[76]; + public static Byte[] DataH1 = new Byte[20]; + + public static void MakeDataH0() + { + Array.Copy(BitConverter.GetBytes(MH_VerPXCC), 0, DataH0, 0, 1); + Array.Copy(BitConverter.GetBytes(MH_MPT), 0, DataH0, 1, 1); + Array.Copy(BitConverter.GetBytes(MH_SeqCounter_Low), 0, DataH0, 2, 2); + Array.Copy(BitConverter.GetBytes(MH_Timestamp), 0, DataH0, 4, 4); + Array.Copy(BitConverter.GetBytes(MH_SSRC), 0, DataH0, 8, 4); + Array.Copy(BitConverter.GetBytes(MH_SeqCounter_Hi), 0, DataH0, 12, 2); + Array.Copy(BitConverter.GetBytes(MH_DataLen), 0, DataH0, 14, 2); + Array.Copy(BitConverter.GetBytes(MH_RowNumber), 0, DataH0, 16, 2); + Array.Copy(BitConverter.GetBytes(MH_Offset), 0, DataH0, 18, 2); + + Array.Copy(BitConverter.GetBytes(SD_Width), 0, DataH0, 20, 2); + Array.Copy(BitConverter.GetBytes(SD_Height), 0, DataH0, 22, 2); + Array.Copy(BitConverter.GetBytes(SD_AzUpr), 0, DataH0, 24, 4); + Array.Copy(BitConverter.GetBytes(SD_ElUpr), 0, DataH0, 28, 4); + + Array.Copy(BitConverter.GetBytes(SD_Shirota), 0, DataH0, 32, 4); + Array.Copy(BitConverter.GetBytes(SD_Dolgota), 0, DataH0, 36, 4); + Array.Copy(BitConverter.GetBytes(SD_Vysota), 0, DataH0, 40, 4); + + Array.Copy(BitConverter.GetBytes(SD_Course), 0, DataH0, 44, 2); + Array.Copy(BitConverter.GetBytes(SD_Roll), 0, DataH0, 46, 2); + Array.Copy(BitConverter.GetBytes(SD_Pitch), 0, DataH0, 48, 2); + Array.Copy(BitConverter.GetBytes(SD_FrameId), 0, DataH0, 50, 2); + + Array.Copy(BitConverter.GetBytes(rez0), 0, DataH0, 52, 2); + Array.Copy(BitConverter.GetBytes(rez1), 0, DataH0, 54, 2); + + Array.Copy(BitConverter.GetBytes(rez2), 0, DataH0, 56, 1); + Array.Copy(BitConverter.GetBytes(rez3), 0, DataH0, 57, 1); + Array.Copy(BitConverter.GetBytes(SD_Status), 0, DataH0, 58, 1); + Array.Copy(BitConverter.GetBytes(SD_rezerv), 0, DataH0, 59, 1); + + Array.Copy(BitConverter.GetBytes(DE_TLV_X), 0, DataH0, 60, 4); + Array.Copy(BitConverter.GetBytes(DE_TLV_Y), 0, DataH0, 64, 4); + Array.Copy(BitConverter.GetBytes(DE_TLV_dX), 0, DataH0, 68, 4); + Array.Copy(BitConverter.GetBytes(DE_TLV_dY), 0, DataH0, 72, 4); + } + public static void MakeDataH1() + { + Array.Copy(BitConverter.GetBytes(MH_VerPXCC), 0, DataH1, 0, 1); + Array.Copy(BitConverter.GetBytes(MH_MPT), 0, DataH1, 1, 1); + Array.Copy(BitConverter.GetBytes(MH_SeqCounter_Low), 0, DataH1, 2, 2); + Array.Copy(BitConverter.GetBytes(MH_Timestamp), 0, DataH1, 4, 4); + Array.Copy(BitConverter.GetBytes(MH_SSRC), 0, DataH1, 8, 4); + Array.Copy(BitConverter.GetBytes(MH_SeqCounter_Hi), 0, DataH1, 12, 2); + Array.Copy(BitConverter.GetBytes(MH_DataLen), 0, DataH1, 14, 2); + Array.Copy(BitConverter.GetBytes(MH_RowNumber), 0, DataH1, 16, 2); + Array.Copy(BitConverter.GetBytes(MH_Offset), 0, DataH1, 18, 2); + } + public static void GetDataH0() + { + MH_VerPXCC = DataH0[0]; + MH_MPT = DataH0[1]; + MH_SeqCounter_Low = BitConverter.ToUInt16(DataH0, 2); + MH_Timestamp = BitConverter.ToUInt32(DataH0, 4); + MH_SSRC = BitConverter.ToUInt32(DataH0, 8); + MH_SeqCounter_Hi = BitConverter.ToUInt16(DataH0, 12); + MH_DataLen = BitConverter.ToUInt16(DataH0, 14); + MH_RowNumber = BitConverter.ToUInt16(DataH0, 16); + MH_Offset = BitConverter.ToUInt16(DataH0, 18); + SD_Width = BitConverter.ToUInt16(DataH0, 20); + SD_Height = BitConverter.ToUInt16(DataH0, 22); + SD_AzUpr = BitConverter.ToUInt16(DataH0, 24); + SD_ElUpr = BitConverter.ToUInt16(DataH0, 28); + SD_Shirota = BitConverter.ToUInt16(DataH0, 32); + SD_Dolgota = BitConverter.ToUInt16(DataH0, 36); + SD_Vysota = BitConverter.ToUInt16(DataH0, 40); + SD_Course = BitConverter.ToInt16(DataH0, 44); + SD_Roll = BitConverter.ToInt16(DataH0, 46); + SD_Pitch = BitConverter.ToInt16(DataH0, 48); + SD_FrameId = BitConverter.ToInt16(DataH0, 50); + rez0 = BitConverter.ToUInt16(DataH0, 52); + rez1 = BitConverter.ToUInt16(DataH0, 54); + rez2 = DataH0[56]; + rez3 = DataH0[57]; + SD_Status = DataH0[58]; + SD_rezerv = DataH0[59]; + DE_TLV_X = BitConverter.ToUInt32(DataH0, 60); + DE_TLV_Y = BitConverter.ToUInt32(DataH0, 64); + DE_TLV_dX = BitConverter.ToUInt32(DataH0, 68); + DE_TLV_dY = BitConverter.ToUInt32(DataH0, 72); + } + public static void GetDataH1() + { + MH_VerPXCC = DataH0[0]; + MH_MPT = DataH0[1]; + MH_SeqCounter_Low = BitConverter.ToUInt16(DataH0, 2); + MH_Timestamp = BitConverter.ToUInt32(DataH0, 4); + MH_SSRC = BitConverter.ToUInt32(DataH0, 8); + MH_SeqCounter_Hi = BitConverter.ToUInt16(DataH0, 12); + MH_DataLen = BitConverter.ToUInt16(DataH0, 14); + MH_RowNumber = BitConverter.ToUInt16(DataH0, 16); + MH_Offset = BitConverter.ToUInt16(DataH0, 18); + } + } + + public static class RTPet + { + //RTPMsgHeader + private const UInt32 _len_pack = 50; + + private static Byte _VerPXCC = 2; + private static Byte _Ver = 2; // версия протокола (текущая версия 2) + private static Byte _P = 0; // = 0 (не используется заполнение в конце пакета) + private static Byte _X = 0; // = 0 (не используются дополнительные заголовки) + private static Byte _CC = 0; // = 0 (CSRC - идентификаторы не используются); + private static Byte _MPT; + private static Byte _M; // маркерный бит. + private static Byte _PT = 99; // поле идентифицирует формат трафика RTP и определяет его интерпретацию. Задается 99 + private static UInt32 _Seqcounter = 0; + private static UInt16 _SeqCounter_Hi; + private static UInt16 _SeqCounter_Low; + private static Byte _rejim_oes; + private static Byte _zahvat; + private static Byte _color; + private static Byte _status; + + public static Byte MH_VerPXCC + { + get => _VerPXCC; + set + { + _VerPXCC = value; + _Ver = (Byte)(value & 0x03); + _P = (Byte)((value >> 2) & 0x01); + _X = (Byte)((value >> 3) & 0x01); + _CC = (Byte)((value >> 4) & 0x0F); + } + } + public static Byte MH_MPT + { + get + { + return (Byte)((_PT << 1) | _M); + } + set + { + _MPT = value; + _M = (Byte)(value & 0x01); + _PT = (Byte)((value >> 1) & 0x7F); + } + } + public static Byte MH_M // маркерный бит. + { + get => _M; + set + { + _M = value; + } + } + public static Byte MH_PT // поле идентифицирует формат трафика RTP и определяет его интерпретацию. Задается 99 + { + get => _PT; + set + { + _PT = value; + } + } + public static UInt16 MH_SeqCounter_Low // Номер последовательности (младшие 16 бит) + { + get => _SeqCounter_Low; + set + { + _SeqCounter_Low = value; + } + } + public static UInt32 MH_Timestamp; // Метка времени (90 кГц отсчеты), одинакова для всех пакетов кадра + public static UInt32 MH_SSRC; // 12345678 (идентификатор источника информации) + public static UInt16 MH_SeqCounter_Hi // Номер последовательности (старшие 16 бит) + { + get => _SeqCounter_Hi; + set + { + _SeqCounter_Hi = value; + } + } + public static UInt16 MH_DataLen; // Количество байт данных строки, включенной в пакет + public static UInt16 MH_RowNumber; // Номер строки + public static UInt16 MH_Offset; // Смещение первого пиксела в строке (= 0) + + //RTPVideoSupplementalData + public static UInt16 SD_Width; // Ширина (пиксели) + public static UInt16 SD_Height; // Высота (пиксели) + public static float SD_AzUpr; // (град) + public static float SD_ElUpr; // (град) + + public static Int32 SD_Shirota; // Широта БЛА (1е-7 град) + public static Int32 SD_Dolgota; // Долгота БЛА (1е-7 град) + public static Int32 SD_Vysota; // Высота БЛА (0,01 м) + + public static Int16 SD_Course; // Курс БЛА (0,01 град) + public static Int16 SD_Roll; // Крен БЛА (0,01 град) + public static Int16 SD_Pitch; // Тангаж БЛА (0,01 град) + public static Int16 SD_FrameId; + public static UInt16 SD_X; // координата центра цели в растре изображения по горизонтали public static UInt16 SD_Y; //координата центра цели в растре изображения по вертикали @@ -264,4 +523,5 @@ namespace nRTP MH_Offset = BitConverter.ToUInt16(DataH0, 18); } } + } diff --git a/udplib.cs b/udplib.cs index 04fdf33..ad29fe6 100644 --- a/udplib.cs +++ b/udplib.cs @@ -153,11 +153,11 @@ namespace UDPLIB } } } - catch (Exception) + catch (Exception ee) { - using (StreamWriter sw = new StreamWriter(System.IO.File.OpenWrite("grav01.log"))) + using (StreamWriter sw = new StreamWriter(System.IO.File.OpenWrite("udplib.log"))) { - sw.WriteLine(DateTime.Now.ToString("HH:mm:ss:fff ") + "Error dll recv"); + sw.WriteLine($"{DateTime.Now.ToString("HH:mm:ss:fff ")} Error dll recv\r\n{ee.Message}"); } } }