Get the quaternion from arduino output string with filter

unity-arduino-mpu6050.cs

// !!! Attention !!!
void initPort (SerialPort port) {
  // set this encoding to extend the ascii range
  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
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 (str);

  // Windows user: use (packet.Length != 18 || packet[0] != 36) as condition
  if (packet.Length != 19 || packet[0] != 36 || packet[18] != 13) {
    quaternion = Quaternion.identity;
    index = -1;
  } 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);
    quaternion = new Quaternion (q [1], q [2], q [3], q [0]);
    index = packet [1] - '0';
  }
}

// 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 ();
  int outIndex;
  Quaternion quat;
  getQuatFromString (serialRead, out quat, out outIndex);

  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];
}