/// <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);
}
}
