/// <summary>
/// Sends gimbal angle
/// </summary>
/// <param name="module"></param>
/// <param name="action"></param>
/// <param name="gimbalAngle"></param>
/// <returns></returns>
public bool sendCommand(byte module, byte action, GimbalAngle gimbalAngle)
{
byte[] data_bytes = new byte[31]; // { (byte)'A', module, action, data.id, data.order, data.position };
data_bytes[0] = (byte)'A';
data_bytes[1] = module;
data_bytes[2] = action;
byte[] converted = BitConverter.GetBytes(gimbalAngle.gimbalPitch);
for (int i = 0; i < 4; i++)
{
data_bytes[i + 4] = converted[i];
}
converted = BitConverter.GetBytes(gimbalAngle.gimbalRoll);
for (int i = 0; i < 4; i++)
{
data_bytes[i + 8] = converted[i];
}
converted = BitConverter.GetBytes(gimbalAngle.gimbalYaw);
for (int i = 0; i < 4; i++)
{
data_bytes[i + 12] = converted[i];
}
converted = BitConverter.GetBytes(gimbalAngle.gimbalSpeed);
for (int i = 0; i < 4; i++)
{
data_bytes[i + 16] = converted[i];
}
NetMQMessage req_msg = new NetMQMessage();
req_msg.Append(data_bytes);
NetMQ.Msg resp_msg = new StdMessage(0x00, 0x00).to_Msg(); // it will be filled when receiving the response
reqSrv = new ReliableExternalClient(
GlobalSettings.Instance.getRequestPort(), TimeSpan.FromMilliseconds(1000), 3);
if (!reqSrv.sendAndReceive(ref req_msg, ref resp_msg))
{
UnityEngine.Debug.Log("Client: server not respoding");
return(false);
}
// Just after the request the atreyu server must respond with a notification
// (ACK, UNKNOWN_MODULE, or UNDEFINED_MODULE) through the same 'requests' socket.
if (StdMessage.isStdNotification(ref resp_msg) && StdMessage.getMessageAction(ref resp_msg) != (byte)NotificationType.ACK)
{
if (module == (byte)Modules.STD_COMMANDS_MODULE)
{
UnityEngine.Debug.Log(
String.Format("Client: STD command was not accepted {0}", new StdMessage(module, action).to_string()));
}
else if (module == (byte)Modules.POSITIONING_MODULE)
{
UnityEngine.Debug.Log(
String.Format("Client: POSITIONING_MODULE command was not accepted {0}", new StdMessage(ref resp_msg).to_string()));
}
else if (module == (byte)Modules.POINTCLOUD_MODULE)
{
UnityEngine.Debug.Log(
String.Format("Client: POINTCLOUD_MODULE command was not accepted {0}", new StdMessage(ref resp_msg).to_string()));
}
return(false);
}
return(true);
}
/// <summary>
/// Command to send anchors to calibration
/// </summary>
/// <param name="module"></param>
/// <param name="action"></param>
/// <param name="data"></param>
/// <returns></returns>
public bool sendCommand(byte module, byte action, ServerMessages.IPSFrameAnchorData[] data)
{
NetMQMessage req_msg = new NetMQMessage();
byte[] header_bytes = new byte[8]; // { (byte)'A', module, action, data.id, data.order, data.position };
header_bytes[0] = (byte)'A';
header_bytes[1] = module;
header_bytes[2] = action;
header_bytes[3] = 0;
byte[] converted = BitConverter.GetBytes(data.Length);
System.Buffer.BlockCopy(converted, 0, header_bytes, 4, 4);
req_msg.Append(header_bytes);
for (int j = 0; j < data.Length; j++)
{
byte[] data_bytes = new byte[17];
UnityEngine.Debug.Log("Anchor ID: " + data[j].id);
converted = BitConverter.GetBytes(data[j].id);
System.Buffer.BlockCopy(converted, 0, data_bytes, 0, 4);
converted = BitConverter.GetBytes((int)data[j].position.x);
System.Buffer.BlockCopy(converted, 0, data_bytes, 4, 4);
converted = BitConverter.GetBytes((int)data[j].position.y);
System.Buffer.BlockCopy(converted, 0, data_bytes, 8, 4);
converted = BitConverter.GetBytes((int)data[j].position.z);
System.Buffer.BlockCopy(converted, 0, data_bytes, 12, 4);
data_bytes[16] = data[j].order;
req_msg.Append(data_bytes);
}
NetMQ.Msg resp_msg = new StdMessage(0x00, 0x00).to_Msg(); // it will be filled when receiving the response
reqSrv = new ReliableExternalClient(
GlobalSettings.Instance.getRequestPort(), TimeSpan.FromMilliseconds(1000), 3);
if (!reqSrv.sendAndReceive(ref req_msg, ref resp_msg))
{
UnityEngine.Debug.Log("Client: server not respoding");
return(false);
}
// Just after the request the atreyu server must respond with a notification
// (ACK, UNKNOWN_MODULE, or UNDEFINED_MODULE) through the same 'requests' socket.
if (StdMessage.isStdNotification(ref resp_msg) && StdMessage.getMessageAction(ref resp_msg) != (byte)NotificationType.ACK)
{
if (module == (byte)Modules.STD_COMMANDS_MODULE)
{
UnityEngine.Debug.Log(
String.Format("Client: STD command was not accepted {0}", new StdMessage(module, action).to_string()));
}
else if (module == (byte)Modules.POSITIONING_MODULE)
{
UnityEngine.Debug.Log(
String.Format("Client: POSITIONING_MODULE command was not accepted {0}", new StdMessage(ref resp_msg).to_string()));
}
else if (module == (byte)Modules.POINTCLOUD_MODULE)
{
UnityEngine.Debug.Log(
String.Format("Client: POINTCLOUD_MODULE command was not accepted {0}", new StdMessage(ref resp_msg).to_string()));
}
return(false);
}
return(true);
}
