Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- // !!! Attention !!!
- void initPort (SerialPort port) {
- // set this encoding to extend the ascii range
- port.Encoding = System.Text.Encoding.GetEncoding(28591);
- }
- // return default quat if packet is invalid
- Quaternion getQuatFromString (string s, Quaternion defaultQuat) {
- char[] packet = s.ToCharArray ();
- string str = "";
- foreach (char c in packet) {
- str += (int)c + " ";
- }
- //Debug.Log (serialRead + str);
- // Windows user: use (packet.Length != 18 || packet[0] != 36) as condition
- if (packet.Length != 19 || packet[0] != 36 || packet[18] != 13) {
- return defaultQuat;
- } else {
- float[] q = new float[4];
- q[0] = ((packet[2] << 8) | packet[3]) / 16384f;
- q[1] = ((packet[4] << 8) | packet[5]) / 16384f;
- q[2] = ((packet[6] << 8) | packet[7]) / 16384f;
- q[3] = ((packet[8] << 8) | packet[9]) / 16384f;
- for (int i = 0; i < 4; i++) if (q[i] >= 2) q[i] = -4 + q[i];
- //Debug.Log (quat.x + " " + quat.y + " " + quat.z + " " + quat.w);
- return new Quaternion (q [1], q [2], q [3], q [0]);
- }
- }
- // sample usage
- void readFromArduinoPort (SerialPort serial) {
- string serialRead = serial.ReadLine ();
- obj.rotation = getQuatFromString (serialRead, obj.rotation);
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement