Go Down

Topic: 6DOF Attitude controller (Read 5891 times) previous topic - next topic


Paul S,

I successfully achieved floating point tx/rx by converting floating point values to integers. This required I adopt a set precision for the data, multiply by this order of magnitude, and add a value equal to the maximum absolute value of the data to remove the +/- signing requirement. I implemented the code that you presented in this discussion: http://forum.arduino.cc/index.php?topic=150896.0 , and likewise performed more math to back out my set-precision floats.

The next challenge is the integration with the control system, specifically, getting a servo to actuate according to the error quaternion. The code is getting verbose at this point, so I hope I may effectively outline the issue to you and receive further guidance:

According to your code, once the "SOP" is read, successive values are stored in an array until the "EOP is read, hence we received a packet. I am able to convert these values to variables while the code is populating the array prior to receipt of "EOP", but am not able to pull values from the array after the complete packet is received. The issue may not stem from this portion of code itself, since this code also works in conjunction with the Adafruit_10DOF library, communicating with the sensor via I2C. Which brings me to my second and potentially related issue and the motivation for this post.........

Although I converted these variables while the array is populating AND successfully generated the error quaternion during stable communication AND validated via Serial.print(the output values to the servo), I get no response from the servo when I implement the servo library's write() command.

Please advise.


I get no response from the servo when I implement the servo library's write() command.
Do you KNOW that the servo works? How is it powered? Lets rule out the stupid mistakes, first.


May 29, 2015, 04:27 am Last Edit: May 29, 2015, 02:50 pm by SpaceJockey
PaulS, I committed the classic mistake of not having a common ground between power supply and digital pin on the Arduino. Full speed ahead on more detailed experimentation.

Already uncovered one error from real world tests as to be expected. I'm pleased with the way the project is coming along and I hope to deliver the code soon.


I encountered some issues over the weekend that may stem from the quaternion to axis error transformation as described on page 3867 "Controller Synthesis" of the paper provided by jremington,


Equation 20 appears to show that the axis error utilized by the control system is simply the vector portion of the error quaternion without performing a transformation to axis-angles. This makes sense because all three vector components are scaled by the same sin(a/2), and is advantageous if possible since it avoids singularities when angular error approaches zero.

As a test of this approach, I performed a 1-D relative rotation about the x-axis, which should intuitively result in an x-axis error only. However, in testing the error is observed on more than one axis, or by an axis other than the x-axis.

The controller presented in the article uses the axis error to control the rotation rates directly, so I am confused as to why I am witnessing this behavior. Any ideas on how to proceed?



I made a program to test the hypothesis that a 1-D relative rotation should result in a single-axis response and I determined that yes, that is how the control system should respond. However, it is important to note that any rotation involving more than a single axis will result in response by all three axes.

So why did I observe different behavior in my implementation of the system?

Since all control systems operate on the premise of garbage in = garbage out, I revisited the quaternion inputs to my system, obtained via the Adafruit 9DOF sensor in conjunction with the Madgwick sensor fusion algorithm. Rotating my sensor in space, I noticed that for angles of rotation exceeding approximately 45 degrees, there was an axial mixing taking place within the vector portion of the quaternion. Yes they are strange numbers to begin with, and rotations are a confounding problem to visualize, but bear with me.

I performed the same test with quaternions obtained directly from the Bosch BNO055 absolute orientation sensor utilizing the Adafruit BNO055 library and not only were the quaternion values different, but they did not exhibit the axial mixing! Furthermore, the sensor readings were fairly resistant to disturbance.

I went back to Madgwick's Dissertation and apart from being impressed with his efforts, I was unable to find the source of error. All I care about is that I found a solution and can finally progress past this wicked problem.


As you are no doubt aware, rotations do not commute, and there are several different conventions for Euler angles, plus two possibilities for defining a positive rotation. 

The choice of angular system must be consistent when composing and decomposing a quaternion into an Euler system, and the data sheet for the BNO055 does not seem to state clearly which convention is used.

Most likely, Madgwick used a different convention for the definition of Euler angles.


BTW is this code available somewhere?  It might be fun to have a look sometime :)
[ I DO NOT respond to personal messages, I WILL delete them unread, use the forum please ]

Go Up