diff --git a/piv.cs b/piv.cs index 7731d04..33ece37 100644 --- a/piv.cs +++ b/piv.cs @@ -821,7 +821,7 @@ namespace PIV } public static class OES2BVM { - private const UInt16 _len_pack = 32; + private const UInt16 _len_pack = 54; private static UInt16 _len_data = _len_pack - 8; private static UInt16 _len_crc = _len_pack - 2; @@ -843,7 +843,7 @@ namespace PIV public static Byte Start = CONST.START; public static UInt16 Address = (UInt16)CONST.ADDR.addrBVM | (UInt16)CONST.ADDR.addrPU | (UInt16)CONST.ADDR.addrPR; public static Byte Code = (Byte)CONST.CODE.msgRECEIVE_DATA; - public static UInt16 Lenght = 24; + public static UInt16 Lenght = _len_data; public static UInt16 Id_Pack { get @@ -941,6 +941,16 @@ namespace PIV public static float UM; public static float usAZ; public static float usUM; + + public static UInt16 eth_X; + public static UInt16 eth_Y; + public static Int32 eth_Vysota; + public static Int16 eth_Kren; + public static Int16 eth_Tangaj; + public static Int16 eth_Kurs; + public static float eth_AZupr; + public static float eth_UMupr; + public static UInt16 CRC16; public static Byte[] Data = new Byte[_len_pack]; @@ -957,6 +967,22 @@ namespace PIV Array.Copy(BitConverter.GetBytes(UM), 0, Data, 18, 4); Array.Copy(BitConverter.GetBytes(usAZ), 0, Data, 22, 4); Array.Copy(BitConverter.GetBytes(usUM), 0, Data, 26, 4); + //public static UInt16 eth_X; + //public static UInt16 eth_Y; + //public static Int32 eth_Vysota; + //public static Int16 eth_Kren; + //public static Int16 eth_Tangaj; + //public static Int16 eth_Kurs; + //public static float eth_AZupr; + //public static float eth_UMupr; + Array.Copy(BitConverter.GetBytes(eth_X), 0, Data, 30, 2); + Array.Copy(BitConverter.GetBytes(eth_Y), 0, Data, 32, 2); + Array.Copy(BitConverter.GetBytes(eth_Vysota), 0, Data, 34, 4); + Array.Copy(BitConverter.GetBytes(eth_Kren), 0, Data, 38, 2); + Array.Copy(BitConverter.GetBytes(eth_Tangaj), 0, Data, 40, 2); + Array.Copy(BitConverter.GetBytes(eth_Kurs), 0, Data, 42, 2); + Array.Copy(BitConverter.GetBytes(eth_AZupr), 0, Data, 44, 4); + Array.Copy(BitConverter.GetBytes(eth_UMupr), 0, Data, 48, 4); UInt16 _crc16 = CONST.CRC16(ref Data, _len_crc); Array.Copy(BitConverter.GetBytes(_crc16), 0, Data, _len_crc, 2); @@ -974,7 +1000,17 @@ namespace PIV UM = BitConverter.ToSingle(Data, 18); usAZ = BitConverter.ToSingle(Data, 22); usUM = BitConverter.ToSingle(Data, 26); - CRC16 = BitConverter.ToUInt16(Data, 30); + + eth_X = BitConverter.ToUInt16(Data, 30); + eth_Y = BitConverter.ToUInt16(Data, 32); + eth_Vysota = BitConverter.ToInt32(Data, 34); + eth_Kren = BitConverter.ToInt16(Data, 38); + eth_Tangaj = BitConverter.ToInt16(Data, 40); + eth_Kurs = BitConverter.ToInt16(Data, 42); + eth_AZupr = BitConverter.ToSingle(Data, 44); + eth_UMupr = BitConverter.ToSingle(Data, 48); + + CRC16 = BitConverter.ToUInt16(Data, _len_crc); } public static void ClearData() { @@ -1000,7 +1036,8 @@ namespace PIV private static UInt16 _id_pack = 0x0211; private static UInt16 _rejim = 0; - private static UInt16 _r_crc = 0; + private static UInt16 _r_upr = 0; +// private static UInt16 _r_crc = 0; private static UInt16 _r_rejimoes = 0; private static UInt16 _r_tk = 0; private static UInt16 _r_rks = 0; @@ -1009,7 +1046,7 @@ namespace PIV public static Byte Start = CONST.START; public static UInt16 Address = (UInt16)CONST.ADDR.addrOES; public static Byte Code = (Byte)CONST.CODE.msgRECEIVE_DATA; - public static UInt16 Lenght = 34; + public static UInt16 Lenght = _len_data; public static UInt16 Id_Pack { get @@ -1032,36 +1069,37 @@ namespace PIV } } public static Byte num_pack - { - get => _num_pack; - set { - _num_pack = value; + get => _num_pack; + set + { + _num_pack = value; + } } - } public static Byte dir_pack - { - get => _dir_pack; - set { - _dir_pack = value; + get => _dir_pack; + set + { + _dir_pack = value; + } } - } public static Byte addr_pack - { - get => _addr_pack; - set { - _addr_pack = value; + get => _addr_pack; + set + { + _addr_pack = value; + } } - } public static UInt32 Schetchik; public static UInt16 Rejim { get { _rejim = 0; - _rejim |= (UInt16)(_r_crc << 0); +// _rejim |= (UInt16)(_r_crc << 0); + _rejim |= (UInt16)(_r_upr << 0); _rejim |= (UInt16)(_r_rejimoes << 1); _rejim |= (UInt16)(_r_tk << 3); _rejim |= (UInt16)(_r_rks << 4); @@ -1071,15 +1109,17 @@ namespace PIV set { _rejim = value; - _r_crc = (UInt16)((value & 0x0001) >> 0); +// _r_crc = (UInt16)((value & 0x0001) >> 0); + _r_upr = (UInt16)((value & 0x0001) >> 0); _r_rejimoes = (UInt16)((value & 0x0006) >> 1); _r_tk = (UInt16)((value & 0x0008) >> 3); _r_rks = (UInt16)((value & 0x0010) >> 4); _r_eizo = (UInt16)((value & 0x0020) >> 5); } } - public static UInt16 r_crc { get => _r_crc; set => _r_crc = value; } - public static UInt16 r_rejimoes { get => _r_rejimoes; set => _r_rejimoes = value; } + public static UInt16 r_upr { get => _r_upr; set => _r_upr = value; } +// public static UInt16 r_crc { get => _r_crc; set => _r_crc = value; } + public static UInt16 r_rejimoes { get => _r_rejimoes; set => _r_rejimoes = value; } public static UInt16 r_tk { get => _r_tk; set => _r_tk = value; } public static UInt16 r_rks { get => _r_rks; set => _r_rks = value; } public static UInt16 r_eizo { get => _r_eizo; set => _r_eizo = value; } @@ -1243,7 +1283,7 @@ namespace PIV public static Byte Start = CONST.START; public static UInt16 Address = (UInt16)CONST.ADDR.addrBVM | (UInt16)CONST.ADDR.addrOES; public static Byte Code = (Byte)CONST.CODE.msgRECEIVE_DATA; - public static UInt16 Lenght = 77; + public static UInt16 Lenght = _len_data; public static UInt16 Id_Pack { get diff --git a/rtp.cs b/rtp.cs index 9ce48f0..2ee438c 100644 --- a/rtp.cs +++ b/rtp.cs @@ -88,7 +88,7 @@ namespace nRTP _SeqCounter_Hi = value; } } - public static UInt16 MH_DataLen; // Количество байт данных строки, включенной в пакет + public static UInt16 MH_DataLen = (UInt16)_len_data; // Количество байт данных строки, включенной в пакет public static UInt16 MH_RowNumber; // Номер строки public static UInt16 MH_Offset; // Смещение первого пиксела в строке (= 0) @@ -350,7 +350,7 @@ namespace nRTP _SeqCounter_Hi = value; } } - public static UInt16 MH_DataLen; // Количество байт данных строки, включенной в пакет + public static UInt16 MH_DataLen = (UInt16)_len_data; // Количество байт данных строки, включенной в пакет public static UInt16 MH_RowNumber; // Номер строки public static UInt16 MH_Offset; // Смещение первого пиксела в строке (= 0) diff --git a/udplib.cs b/udplib.cs index c58d01a..cc93c03 100644 --- a/udplib.cs +++ b/udplib.cs @@ -301,13 +301,16 @@ namespace UDPLIB RTPet.SD_Height = (ushort)Res.Crop.Height; RTPet.SD_AzUpr = 0; RTPet.SD_ElUpr = 0; - RTPet.SD_Course = 0; - RTPet.SD_Roll = 0; - RTPet.SD_Pitch = 0; + RTPet.SD_Shirota = RTP.SD_Shirota; + RTPet.SD_Dolgota = RTP.SD_Dolgota; + RTPet.SD_Vysota = RTP.SD_Vysota; + RTPet.SD_Course = RTP.SD_Course; + RTPet.SD_Roll = RTP.SD_Roll; + RTPet.SD_Pitch = RTP.SD_Pitch; RTPet.SD_FrameId = 0; RTPet.SD_X = 0; RTPet.SD_Y = 0; - RTPet.SD_lx = 0; + RTPet.SD_lx = ; RTPet.SD_ly = 0; RTPet.SD_rejim_oes = 0; RTPet.SD_color = 0;