diff --git a/rtp.cs b/rtp.cs index df8e970..053bf82 100644 --- a/rtp.cs +++ b/rtp.cs @@ -9,6 +9,8 @@ namespace nRTP public static class RTP { //RTPMsgHeader + private const UInt32 _len_pack = 50; + private static Byte _VerPXCC = 2; private static Byte _Ver = 2; // версия протокола (текущая версия 2) private static Byte _P = 0; // = 0 (не используется заполнение в конце пакета) @@ -88,6 +90,11 @@ namespace nRTP 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 град) @@ -112,7 +119,7 @@ namespace nRTP } } - public static Byte[] DataH0 = new Byte[56]; + public static Byte[] DataH0 = new Byte[68]; public static Byte[] DataH1 = new Byte[20]; public static void MakeDataH0() @@ -131,15 +138,20 @@ namespace nRTP Array.Copy(BitConverter.GetBytes(RTP.SD_Height), 0, DataH0, 22, 2); Array.Copy(BitConverter.GetBytes(RTP.SD_AzUpr), 0, DataH0, 24, 4); Array.Copy(BitConverter.GetBytes(RTP.SD_ElUpr), 0, DataH0, 28, 4); - Array.Copy(BitConverter.GetBytes(RTP.SD_Course), 0, DataH0, 32, 2); - Array.Copy(BitConverter.GetBytes(RTP.SD_Roll), 0, DataH0, 34, 2); - Array.Copy(BitConverter.GetBytes(RTP.SD_Pitch), 0, DataH0, 36, 2); - Array.Copy(BitConverter.GetBytes(RTP.SD_FrameId), 0, DataH0, 38, 2); - - Array.Copy(BitConverter.GetBytes(RTP.DE_TLV_X), 0, DataH0, 40, 4); - Array.Copy(BitConverter.GetBytes(RTP.DE_TLV_Y), 0, DataH0, 44, 4); - Array.Copy(BitConverter.GetBytes(RTP.DE_TLV_dX), 0, DataH0, 48, 4); - Array.Copy(BitConverter.GetBytes(RTP.DE_TLV_dY), 0, DataH0, 52, 4); + + Array.Copy(BitConverter.GetBytes(RTP.SD_Shirota), 0, DataH0, 32, 4); + Array.Copy(BitConverter.GetBytes(RTP.SD_Dolgota), 0, DataH0, 36, 4); + Array.Copy(BitConverter.GetBytes(RTP.SD_Vysota), 0, DataH0, 40, 4); + + Array.Copy(BitConverter.GetBytes(RTP.SD_Course), 0, DataH0, 44, 2); + Array.Copy(BitConverter.GetBytes(RTP.SD_Roll), 0, DataH0, 46, 2); + Array.Copy(BitConverter.GetBytes(RTP.SD_Pitch), 0, DataH0, 48, 2); + + Array.Copy(BitConverter.GetBytes(RTP.SD_FrameId), 0, DataH0, 50, 2); + Array.Copy(BitConverter.GetBytes(RTP.DE_TLV_X), 0, DataH0, 52, 4); + Array.Copy(BitConverter.GetBytes(RTP.DE_TLV_Y), 0, DataH0, 56, 4); + Array.Copy(BitConverter.GetBytes(RTP.DE_TLV_dX), 0, DataH0, 60, 4); + Array.Copy(BitConverter.GetBytes(RTP.DE_TLV_dY), 0, DataH0, 64, 4); } public static void MakeDataH1() {