public void SendDatas(string[] datas)
{
lock (locked)
{
if (serialPort.IsOpen == false)
{
return;
}
if (datas == null || datas.Length < 18)
{
return;
}
if (double.TryParse(datas[0 + 12], out var x) == true &&
double.TryParse(datas[1 + 12], out var y) == true &&
double.TryParse(datas[2 + 12], out var z) == true &&
double.TryParse(datas[3 + 12], out var roll) == true &&
double.TryParse(datas[4 + 12], out var yaw) == true &&
double.TryParse(datas[5 + 12], out var pitch) == true)
{
var data = new LandVisionDataPackage()
{
X = (short)(x / config.XYZScale),
Y = (short)(y / config.XYZScale),
Z = (short)(z / config.XYZScale),
Roll = (short)(roll / config.XYZScale),
Pitch = (short)(pitch / config.XYZScale)
};
data.Crc = LandVisionDataPackage.CalCrc(data);
var bytes = data.GetBytes(config.SendHeader, config.SendTail);
serialPort.Write(bytes, 0, bytes.Length);
}
}
}
public void SendDatas(double[] vels, double[] accs, double[] positions)
{
lock (locked)
{
if (serialPort.IsOpen == false)
{
return;
}
if (positions == null || positions.Length <= 6)
{
return;
}
var data = new LandVisionDataPackage()
{
X = (short)(positions[0] / config.XYZScale),
Y = (short)(positions[1] / config.XYZScale),
Z = (short)(positions[2] / config.XYZScale),
Roll = (short)(positions[3] / config.XYZScale),
Pitch = (short)(positions[5] / config.XYZScale)
};
data.Crc = LandVisionDataPackage.CalCrc(data);
var bytes = data.GetBytes(config.SendHeader, config.SendTail);
serialPort.Write(bytes, 0, bytes.Length);
}
}