Skip to content

Instantly share code, notes, and snippets.

@peteroid

peteroid/unity-arduino-mpu6050.cs

Last active December 16, 2017 12:30
Show Gist options

  • Select an option

    Learn more about clone URLs
  • Save peteroid/60be49c185e6e29fd04fe5254ebb47d0 to your computer and use it in GitHub Desktop.

Select an option

Learn more about clone URLs
Save peteroid/60be49c185e6e29fd04fe5254ebb47d0 to your computer and use it in GitHub Desktop.
Download ZIP

Revisions

  1. peteroid revised this gist Nov 4, 2016. 1 changed file with 25 additions and 0 deletions.
    25 changes: 25 additions & 0 deletions unity-arduino-mpu6050.cs
    View file
    Original file line number Diff line number Diff line change
    @@ -76,4 +76,29 @@ void readFromArduinoPort (SerialPort serial) {
    if (outIndex != -1 && !quat.Equals(Quaternion.identity)) {
    objs [outIndex].rotation = quat;
    }
    }

    /* Filtering */
    // variables
    int filterStartCount = 100;
    float filterThreshold = 1f;

    // functions
    float getQuatDiff (Quaternion q1, Quaternion q2) {
    return (q1.x - q2.x) * (q1.x - q2.x) +
    (q1.y - q2.y) * (q1.y - q2.y) +
    (q1.z - q2.z) * (q1.z - q2.z) +
    (q1.w - q2.w) * (q1.w - q2.w);
    }

    // sample usage
    float diffQ;
    for (int i = 0; i < quats.Length; i++) {
    diffQ = getQuatDiff (objs [i].rotation, quats [i]);
    if ((filterStartCount-- == 0) && diffQ > filterThreshold) {
    Debug.Log (diffQ + " is filtered due to unusual amount of rotation");
    // exit the function, break out the loop or skip the iteration to avoid the assignment
    continue;
    }
    objs [i].rotation = quats [i];
    }
  2. peteroid revised this gist Nov 2, 2016. 1 changed file with 33 additions and 0 deletions.
    33 changes: 33 additions & 0 deletions unity-arduino-mpu6050.cs
    View file
    Original file line number Diff line number Diff line change
    @@ -4,6 +4,7 @@ void initPort (SerialPort port) {
    port.Encoding = System.Text.Encoding.GetEncoding(28591);
    }

    // Take a string and parse the quaternion and corresponding index
    // return:
    // quat = Quaternion.identity if packet is invalid
    // index = -1 if packet is invalid
    @@ -33,6 +34,38 @@ void getQuatFromString (string s, out Quaternion quaternion, out int index) {
    }
    }

    // Take a string and parse the quaternion according to the count
    // return
    // empty array - if the string is invalid
    // quaternion array - if the string is valid
    Quaternion[] getQuatsFromString (string s, int count) {
    char[] packet = s.ToCharArray ();
    string str = "";
    foreach (char c in packet) {
    str += (int)c + " ";
    }
    Debug.Log (str);

    // Windows user: use (packet.Length != 28 || packet[0] != 36) as condition
    if (packet.Length != 29 || packet[0] != 36 || packet[28] != 13) {
    return new Quaternion[0];
    } else {
    Quaternion[] quats = new Quaternion[count];
    for (int j = 0; j < count; j++) {
    float[] q = new float[4];
    q[0] = ((packet[2 + j * 8] << 8) | packet[3 + j * 8]) / 16384f;
    q[1] = ((packet[4 + j * 8] << 8) | packet[5 + j * 8]) / 16384f;
    q[2] = ((packet[6 + j * 8] << 8) | packet[7 + j * 8]) / 16384f;
    q[3] = ((packet[8 + j * 8] << 8) | packet[9 + j * 8]) / 16384f;
    for (int i = 0; i < 4; i++) if (q[i] >= 2) q[i] = -4 + q[i];

    quats [j] = new Quaternion (q [1], q [2], q [3], q [0]);
    }

    return quats;
    }
    }

    // sample usage
    void readFromArduinoPort (SerialPort serial) {
    string serialRead = serial.ReadLine ();
  3. peteroid revised this gist Nov 1, 2016. 1 changed file with 16 additions and 6 deletions.
    22 changes: 16 additions & 6 deletions unity-arduino-mpu6050.cs
    View file
    Original file line number Diff line number Diff line change
    @@ -4,18 +4,21 @@ void initPort (SerialPort port) {
    port.Encoding = System.Text.Encoding.GetEncoding(28591);
    }

    // return default quat if packet is invalid
    Quaternion getQuatFromString (string s, Quaternion defaultQuat) {
    // return:
    // quat = Quaternion.identity if packet is invalid
    // index = -1 if packet is invalid
    void getQuatFromString (string s, out Quaternion quaternion, out int index) {
    char[] packet = s.ToCharArray ();
    string str = "";
    foreach (char c in packet) {
    str += (int)c + " ";
    }
    //Debug.Log (serialRead + str);
    //Debug.Log (str);

    // Windows user: use (packet.Length != 18 || packet[0] != 36) as condition
    if (packet.Length != 19 || packet[0] != 36 || packet[18] != 13) {
    return defaultQuat;
    quaternion = Quaternion.identity;
    index = -1;
    } else {
    float[] q = new float[4];
    q[0] = ((packet[2] << 8) | packet[3]) / 16384f;
    @@ -25,12 +28,19 @@ Quaternion getQuatFromString (string s, Quaternion defaultQuat) {
    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]);
    quaternion = new Quaternion (q [1], q [2], q [3], q [0]);
    index = packet [1] - '0';
    }
    }

    // sample usage
    void readFromArduinoPort (SerialPort serial) {
    string serialRead = serial.ReadLine ();
    obj.rotation = getQuatFromString (serialRead, obj.rotation);
    int outIndex;
    Quaternion quat;
    getQuatFromString (serialRead, out quat, out outIndex);

    if (outIndex != -1 && !quat.Equals(Quaternion.identity)) {
    objs [outIndex].rotation = quat;
    }
    }
  4. peteroid revised this gist Oct 27, 2016. No changes.
  5. peteroid revised this gist Oct 27, 2016. 1 changed file with 11 additions and 4 deletions.
    15 changes: 11 additions & 4 deletions unity-arduino-mpu6050.cs
    View file
    Original file line number Diff line number Diff line change
    @@ -1,7 +1,7 @@
    // sample usage
    void readFromArduinoPort (SerialPort serial) {
    string serialRead = serial.ReadLine ();
    obj.rotation = getQuatFromString (serialRead, obj.rotation);
    // !!! 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
    @@ -13,6 +13,7 @@ Quaternion getQuatFromString (string s, Quaternion defaultQuat) {
    }
    //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 {
    @@ -26,4 +27,10 @@ Quaternion getQuatFromString (string s, Quaternion defaultQuat) {
    //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);
    }
  6. peteroid revised this gist Oct 27, 2016. 1 changed file with 26 additions and 26 deletions.
    52 changes: 26 additions & 26 deletions unity-arduino-mpu6050.cs
    View file
    Original file line number Diff line number Diff line change
    @@ -1,29 +1,29 @@
    // sample usage
    void readFromArduinoPort (SerialPort serial) {
    string serialRead = serial.ReadLine ();
    obj.rotation = getQuatFromString (serialRead, obj.rotation);
    }
    // sample usage
    void readFromArduinoPort (SerialPort serial) {
    string serialRead = serial.ReadLine ();
    obj.rotation = getQuatFromString (serialRead, obj.rotation);
    }

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

    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];
    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]);
    }
    }
    //Debug.Log (quat.x + " " + quat.y + " " + quat.z + " " + quat.w);
    return new Quaternion (q [1], q [2], q [3], q [0]);
    }
    }
  7. peteroid revised this gist Oct 27, 2016. 1 changed file with 2 additions and 2 deletions.
    4 changes: 2 additions & 2 deletions unity-arduino-mpu6050.cs
    View file
    Original file line number Diff line number Diff line change
    @@ -5,10 +5,10 @@ void readFromArduinoPort (SerialPort serial) {
    }

    // return default quat if packet is invalid
    Quaternion getQuatFromString (string s, Quaternion defaultQuat) {
    Quaternion getQuatFromString (string s, Quaternion defaultQuat) {
    char[] packet = s.ToCharArray ();
    string str = "";
    foreach (char c in packet) {
    foreach (char c in packet) {
    str += (int)c + " ";
    }
    //Debug.Log (serialRead + str);
  8. peteroid revised this gist Oct 27, 2016. 1 changed file with 1 addition and 0 deletions.
    1 change: 1 addition & 0 deletions unity-arduino-mpu6050.cs
    View file
    Original file line number Diff line number Diff line change
    @@ -16,6 +16,7 @@ Quaternion getQuatFromString (string s, Quaternion defaultQuat) {
    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;
  9. peteroid revised this gist Oct 27, 2016. 1 changed file with 7 additions and 1 deletion.
    8 changes: 7 additions & 1 deletion unity-arduino-mpu6050.cs
    View file
    Original file line number Diff line number Diff line change
    @@ -1,4 +1,10 @@
    // return default quat if packet is invalid
    // sample usage
    void readFromArduinoPort (SerialPort serial) {
    string serialRead = serial.ReadLine ();
    obj.rotation = getQuatFromString (serialRead, obj.rotation);
    }

    // return default quat if packet is invalid
    Quaternion getQuatFromString (string s, Quaternion defaultQuat) {
    char[] packet = s.ToCharArray ();
    string str = "";
  10. peteroid created this gist Oct 27, 2016.
    22 changes: 22 additions & 0 deletions unity-arduino-mpu6050.cs
    View file
    Original file line number Diff line number Diff line change
    @@ -0,0 +1,22 @@
    // 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);

    if (packet.Length != 19 || packet[0] != 36 || packet[18] != 13) {
    return defaultQuat;
    } else {
    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]);
    }
    }