//Rotates the gimball at a speed and with a roll, yaw and pitch
public void SendGimbalInfo()
{
UnityEngine.Debug.Log("Pitch: " + pitchValue);
UnityEngine.Debug.Log("Roll: " + rollValue);
UnityEngine.Debug.Log("Yaw: " + yawValue);
UnityEngine.Debug.Log("Speed: " + speedValue);
GimbalAngle ga = new GimbalAngle(pitchValue, rollValue, yawValue, speedValue);
//UnityEngine.Debug.Log("Send");
clientUnity.client.sendCommand((byte)Modules.GIMBAL_MULTIPLEXER_MODULE, (byte)GimbalMultiplexerCommandType.GIMBAL_GOTO_ANGLE, ga);
}
//Changes the speed at which the gimball rotates
public void SpeedValueChanged(float speed)
{
speedValue = (speed * Mathf.PI / 180.0f);
if (continuousToggle.isOn && ((Time.time - lastTime) > clampTime))
{
lastTime = Time.time;
GimbalAngle ga = new GimbalAngle(pitchValue, rollValue, yawValue, speedValue);
clientUnity.client.sendCommand((byte)Modules.GIMBAL_MULTIPLEXER_MODULE, (byte)GimbalMultiplexerCommandType.GIMBAL_GOTO_ANGLE, ga);
//UnityEngine.Debug.Log("Changed! " + i++);
}
}
/// <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);
}
