Discussions

I've tried several sketches with the ArduIMU V3, the stock DCM version and FreeIMU and Martin's DMP.  The stock DCM works for me, but I want to DIY.  Right now I have FreeIMU loaded and I changed the quaternion sketch so it will work with ArduIMU Test.  The FreeIMU quaternion sketch is supposed to work with the processing file FreeIMU cube and it does.  The key to working with the cube program is how the cube program uses the quaternions.  I borrowed the conversion from quaternions to euler and stuck it into the sketch. 

As long as I stay within one quadrant, it works great.  As soon as I rotate too far, then pitch might become yaw and roll might become pitch, etc.  So, I looked at how the processing sketch converts the quaternions and I am confused.  It looks like at the bottom of the processing sketch there are 4 functions and I'm only using the void function in my sketch. I'm trying to figure out where "a" and "b" come from and how "conj" will translate to my arduino sketch. If you can help me understand this, then I will share my sketch. Thanks.

// See Sebastian O.H. Madwick report 
// "An efficient orientation filter for inertial and intertial/magnetic sensor arrays" Chapter 2 Quaternion representationvoid quaternionToEuler(float [] q, float [] euler) {
  euler[0] = atan2(2 * q[1] * q[2] - 2 * q[0] * q[3], 2 * q[0]*q[0] + 2 * q[1] * q[1] - 1); // psi
  euler[1] = -asin(2 * q[1] * q[3] + 2 * q[0] * q[2]); // theta
  euler[2] = atan2(2 * q[2] * q[3] - 2 * q[0] * q[1], 2 * q[0] * q[0] + 2 * q[3] * q[3] - 1); // phi
}float [] quatProd(float [] a, float [] b) {
  float [] q = new float[4];
  
  q[0] = a[0] * b[0] - a[1] * b[1] - a[2] * b[2] - a[3] * b[3];
  q[1] = a[0] * b[1] + a[1] * b[0] + a[2] * b[3] - a[3] * b[2];
  q[2] = a[0] * b[2] - a[1] * b[3] + a[2] * b[0] + a[3] * b[1];
  q[3] = a[0] * b[3] + a[1] * b[2] - a[2] * b[1] + a[3] * b[0];
  
return q;
}// returns a quaternion from an axis angle representation
float [] quatAxisAngle(float [] axis, float angle) {
float [] q = new float[4];
  
float halfAngle = angle / 2.0;
float sinHalfAngle = sin(halfAngle);
  q[0] = cos(halfAngle);
  q[1] = -axis[0] * sinHalfAngle;
  q[2] = -axis[1] * sinHalfAngle;
  q[3] = -axis[2] * sinHalfAngle;
  
return q;
}// return the quaternion conjugate of quat
float [] quatConjugate(float [] quat) {
float [] conj = new float[4];
  
  conj[0] = quat[0];
  conj[1] = -quat[1];
  conj[2] = -quat[2];
  conj[3] = -quat[3];
  
return conj;
}