Advertisement
Guest User

Untitled

a guest
Oct 27th, 2016
63
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 1.19 KB | None | 0 0
  1. // !!! Attention !!!
  2. void initPort (SerialPort port) {
  3. // set this encoding to extend the ascii range
  4. port.Encoding = System.Text.Encoding.GetEncoding(28591);
  5. }
  6.  
  7. // return default quat if packet is invalid
  8. Quaternion getQuatFromString (string s, Quaternion defaultQuat) {
  9. char[] packet = s.ToCharArray ();
  10. string str = "";
  11. foreach (char c in packet) {
  12. str += (int)c + " ";
  13. }
  14. //Debug.Log (serialRead + str);
  15.  
  16. // Windows user: use (packet.Length != 18 || packet[0] != 36) as condition
  17. if (packet.Length != 19 || packet[0] != 36 || packet[18] != 13) {
  18. return defaultQuat;
  19. } else {
  20. float[] q = new float[4];
  21. q[0] = ((packet[2] << 8) | packet[3]) / 16384f;
  22. q[1] = ((packet[4] << 8) | packet[5]) / 16384f;
  23. q[2] = ((packet[6] << 8) | packet[7]) / 16384f;
  24. q[3] = ((packet[8] << 8) | packet[9]) / 16384f;
  25. for (int i = 0; i < 4; i++) if (q[i] >= 2) q[i] = -4 + q[i];
  26.  
  27. //Debug.Log (quat.x + " " + quat.y + " " + quat.z + " " + quat.w);
  28. return new Quaternion (q [1], q [2], q [3], q [0]);
  29. }
  30. }
  31.  
  32. // sample usage
  33. void readFromArduinoPort (SerialPort serial) {
  34. string serialRead = serial.ReadLine ();
  35. obj.rotation = getQuatFromString (serialRead, obj.rotation);
  36. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement