I hope some of this helps…
Arduino:
- Sends quaternion data (X, Y, Z, W) to serial port as a string and terminates with a ‘/n’.
I used the Adafruit library for this.
Processing:
-
Reads quaternion data and stores it in an array
readQuatData(); // Stored in q[] -
Convert quaternions to Euler angles
quatToEuler_1(q, Euler); -
Rotate
rotateEuler(); -
Draw to canvas
I also displayed data to screen and axis of the BNO055, Euclidean formulas and Processing display to help me visualize all of this. The screen follows the orientation of my device!
Note:
I also had code to convert to rotate matrix from quaternions.
rotateMatrix();
This site was very useful:
//**************************************************************************************************
// Example #7 EuclideanSpace - Mathematics and Computing
//
// Can be tough to navigate!
//
// https://www.euclideanspace.com/maths/geometry/rotations/conversions/index.htm
// https://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToEuler/index.htm
// http://www.euclideanspace.com/maths/standards/index.htm
// https://www.euclideanspace.com/maths/geometry/rotations/index.htm
Once I defined my co-ordinate systems and mapped them I just had to piece it all together.
I had to make lots of notes and wrap my head around the different co-ordinate systems and mapping\converting them from one to another.
I wrote all of my own code with the exception of the working formulas from the website.
Some notes:
//**************************************************************************************************
// GLV Notes
// Things to consider:
// 1) Axis of device; in this case BNO055
//
// 2) Conversion of Quaternion to Euler
//
// 3) Convert from one coordinate system to another
//
// 4) Coordinate system of code used; this varies widely!
//
// 4) Convert from device > world > display
// BNO055 > Euclidian > Processing
//
// Attempt to map BNO055 to Euclidean to Processing:
// B E P
// +X P/A -Z R/B +Z
// +Y R/B -X P/A -X
// +Z Y/H +Y Y/H -Y
//
//**************************************************************************************************
// Coordinate systems - GLV hand stitched
//
// // 1) BNO055 (RHS) x, y, z:
//
// +z P
// * +x
// * *
// * *
// * *
// R * *
// +y * * * * * *
//
// 2) Euclidean (RHS) x', y', z':
//
// +y'(+z) P
// * -z'(+x)
// * *
// * *
// * *
// R * *
// -x'(+y) * * * * * *
// Processing (LHS) x", y", z": using scale (1, 1, 1) default
//
// -y" P
// * -z"
// * *
// * *
// * *
// R * *
// -x" * * * * * *
//
// Processing (RHS): using scale(1, 1, -1)
//
// -y" P
// * z"
// * *
// * *
// * *
// R * *
// -x" * * * * * *
//
//
//**************************************************************************************************
// Eucidean mapping:
//
// BNO055 QE Processing
// X att Z att Z att
// Y roll X roll Y head
// Z head Y head X roll
// Euclidean:
// Heading = rotation about y axis
// Attitude = rotation about z axis
// Bank = rotation about x axis
//
// Heading applied first
// Attitude applied second
// Bank applied last
//****************************************************
After all was said and done I can go back and remap things differently one of these days.
