/// <summary> /// 将STP313传感器的数据映射到指定的控件中 /// </summary> /// <param name="dat"></param> /// <param name="tx"></param> /// <param name="lb"></param> public void DataMapping2Control_STP313(LIN_STP313_ReadData dat, ref TextBox[] tx) { tx[0].Text = ((dat.TOF1) / 58.0).ToString(); tx[1].Text = ((dat.TOF2) / 58.0).ToString(); tx[2].Text = (dat.Width * 16).ToString(); tx[3].Text = (dat.Level * 3.3 / 255).ToString(); if (dat.status == 1) { tx[4].Text = SensingStatus[0]; } else if (dat.status == 2) { tx[4].Text = SensingStatus[1]; } else if (dat.status == 4) { tx[4].Text = SensingStatus[2]; } else if (dat.status == 8) { tx[4].Text = SensingStatus[3]; } else if (dat.status == 16) { tx[4].Text = SensingStatus[4]; } else { tx[4].Text = "正常"; } }
/// <summary> /// 长距离传感器的数据处理 /// </summary> /// <param name="m_313Data">传感器原始数据的输入</param> /// <param name="InstallationPosition">传感器的安装位置信息</param> /// <param name="ultrasonic_location">输出的传感器的检测目标位置</param> private void STP313_Process(LIN_STP313_ReadData m_313Data, LocationPoint InstallationPosition, ref UltrasonicAxis ultrasonic_location) { if (m_313Data.status == 0) { if (m_313Data.TOF1 != 0) { double distance = m_313Data.TOF1 * TemperatureCompensation(25);//cm ultrasonic_location.x = InstallationPosition.x + distance * Math.Cos(InstallationPosition.yaw); ultrasonic_location.y = InstallationPosition.y + distance * Math.Sin(InstallationPosition.yaw); ultrasonic_location.state = UltrasonicStatus.Normal; } else { ultrasonic_location.x = 0; ultrasonic_location.y = 0; ultrasonic_location.state = UltrasonicStatus.OverDetection; } } else if (m_313Data.status == 16) { ultrasonic_location.x = 0; ultrasonic_location.y = 0; ultrasonic_location.state = UltrasonicStatus.BlindZone; } ultrasonic_location.x *= 0.01; ultrasonic_location.y *= 0.01; }
/// <summary> /// STP313 Read data function /// </summary> /// <param name="id">the id od the input datat</param> /// <returns></returns> public LIN_STP313_ReadData ReadData_STP313(int DevHandle, byte id) { int ret; LIN_STP313_ReadData rd_msg = new LIN_STP313_ReadData(); USB2LIN.LIN_MSG[] msg = new USB2LIN.LIN_MSG[1]; msg[0].Data = new Byte[9]; msg[0].DataLen = 7; msg[0].ID = id; IntPtr[] ptArray = new IntPtr[1]; ptArray[0] = Marshal.AllocHGlobal(Marshal.SizeOf(typeof(USB2LIN.LIN_MSG)) * msg.Length); IntPtr pt = Marshal.AllocHGlobal(Marshal.SizeOf(typeof(USB2LIN.LIN_MSG))); Marshal.Copy(ptArray, 0, pt, 1); //将数组中的数据拷贝到指针所指区域 for (int k = 0; k < msg.Length; k++) { Marshal.StructureToPtr(msg[k], (IntPtr)((UInt32)pt + k * Marshal.SizeOf(typeof(USB2LIN.LIN_MSG))), true); } ret = USB2LIN.LIN_Read(DevHandle, LINIndex, pt, 1); if (ret < USB2LIN.LIN_SUCCESS) { Console.WriteLine("LIN read data failed!\n"); return(rd_msg); } else { msg[0] = (USB2LIN.LIN_MSG)Marshal.PtrToStructure((IntPtr)((UInt32)pt + 0 * Marshal.SizeOf(typeof(USB2LIN.LIN_MSG))), typeof(USB2LIN.LIN_MSG)); rd_msg.TOF1 = BitConverter.ToUInt16(msg[0].Data, 0); rd_msg.Level = msg[0].Data[2]; rd_msg.Width = msg[0].Data[3]; rd_msg.TOF2 = BitConverter.ToUInt16(msg[0].Data, 4); rd_msg.status = msg[0].Data[6]; return(rd_msg); } }