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