About a month ago, my son and I decided to ditch the packaged EM7180 board we were using for tilt angle monitoring and roll our own, since most of the IMUs are a black box and/or they integrate accelerometer data, which doesn't work reliably for very long on rockets. The solution we had been using also integrated accelerometer data and the output was unreliable about ten seconds after launch.
With a fresh new ST LSM6DSO32 6dof accel/gyro we mastered the basics with the gyro. We got stationary drift offset to almost nothing over five minutes and we were reading rotations and integrating them accurately into degrees at 500 hz on the bench. Then we hit a brick wall. If the rocket didn't spin, calculating tilt would be easy peasy, but with high rates of spin (or low rates of spin) you have no idea how to integrate your pitch and yaw data (e.g. if you pitched +5 degrees, then rolled/spun 180d, then pitched -5d, you are now sitting at 10d pitch - not -0d pitch). At first, we tried to use solutions solely based on integrating Euler Angles. Some seemed to work, but when stress tested the results were ugly. We tried everything we could, before jumping in the deep end of the pool with quaternions.
I spent a week reading every previous post on the board related to tilt, quaternions, and I sifted through thousands of lines of code. At some point in 2012, @UhClem said that "quaternions are your friend" in solving the tilt problem. I'm eternally grateful for David, and the dozens of others that have weighed in over the years (@jderimig @keithp @JimJarvis50 etc), but I'd also like to declare that while they are great at solving this problem, quaternions are not your friend. I spent about 20 hours tracing @keithp 's AltusMetrum code (thank you!!), but couldn't quite get things to work using subsets of code. That was probably a blessing, as it forced us to decompose each step and fully understand what was going on. (side note, HUGE props to Keith for making his code open source).
So, the punchline is that we finally got quaternions to work for us with our gyro and using the accelerometer to baseline tilt angles before launch. We haven't flown it yet, but I am amazed that I can put my board in a football, launch it up high, spin it, tumble it end-over-end, and when I bring it back to zero it has almost no drift. That is the amazing power of quaternions. Here is our summary of what we are doing and some sample code for future travelers trying to solve the same challenge with rockets. The code is franken-code, hacked together from many sources, and not optimized, but the quat code runs in less than 1.8 microseconds for me on a teensy 4.0, so I don't have much incentive to hone it. That said, we would appreciate any feedback, ideas, or advice that could make it better for others.
Basic logic flow:
1. On the pad (pre launch) sample the accelerometer, calculate tilt angles, and prepopulate the "master Quaternion" with the starting tilt (an over-simplified version in code below)
2. After launch, start integrating gyro rotations into the master quaternion with the following steps (for us sampling at 500x per second):
- Sample the gyro and convert raw values to degrees per second, proportionalized for the tiny amount of time that has gone by
- Convert the rotations into a half Euler angle and load it into a temporary quaternion
- Combine the temporary quaternion rotation into the master quaternion by multiplying the two together
- Normalize the master quaternion back below 1, as the integrations over time cause it to get too big
- last, convert the master quaternion back into angles that humans can understand.
- The resulting angX, angY, angZ are your roll, pitch, yaw. Watch pitch (AngY) and yaw (AngZ) values for max tilt lockout.
< Code in the next post >
With a fresh new ST LSM6DSO32 6dof accel/gyro we mastered the basics with the gyro. We got stationary drift offset to almost nothing over five minutes and we were reading rotations and integrating them accurately into degrees at 500 hz on the bench. Then we hit a brick wall. If the rocket didn't spin, calculating tilt would be easy peasy, but with high rates of spin (or low rates of spin) you have no idea how to integrate your pitch and yaw data (e.g. if you pitched +5 degrees, then rolled/spun 180d, then pitched -5d, you are now sitting at 10d pitch - not -0d pitch). At first, we tried to use solutions solely based on integrating Euler Angles. Some seemed to work, but when stress tested the results were ugly. We tried everything we could, before jumping in the deep end of the pool with quaternions.
I spent a week reading every previous post on the board related to tilt, quaternions, and I sifted through thousands of lines of code. At some point in 2012, @UhClem said that "quaternions are your friend" in solving the tilt problem. I'm eternally grateful for David, and the dozens of others that have weighed in over the years (@jderimig @keithp @JimJarvis50 etc), but I'd also like to declare that while they are great at solving this problem, quaternions are not your friend. I spent about 20 hours tracing @keithp 's AltusMetrum code (thank you!!), but couldn't quite get things to work using subsets of code. That was probably a blessing, as it forced us to decompose each step and fully understand what was going on. (side note, HUGE props to Keith for making his code open source).
So, the punchline is that we finally got quaternions to work for us with our gyro and using the accelerometer to baseline tilt angles before launch. We haven't flown it yet, but I am amazed that I can put my board in a football, launch it up high, spin it, tumble it end-over-end, and when I bring it back to zero it has almost no drift. That is the amazing power of quaternions. Here is our summary of what we are doing and some sample code for future travelers trying to solve the same challenge with rockets. The code is franken-code, hacked together from many sources, and not optimized, but the quat code runs in less than 1.8 microseconds for me on a teensy 4.0, so I don't have much incentive to hone it. That said, we would appreciate any feedback, ideas, or advice that could make it better for others.
Basic logic flow:
1. On the pad (pre launch) sample the accelerometer, calculate tilt angles, and prepopulate the "master Quaternion" with the starting tilt (an over-simplified version in code below)
2. After launch, start integrating gyro rotations into the master quaternion with the following steps (for us sampling at 500x per second):
- Sample the gyro and convert raw values to degrees per second, proportionalized for the tiny amount of time that has gone by
- Convert the rotations into a half Euler angle and load it into a temporary quaternion
- Combine the temporary quaternion rotation into the master quaternion by multiplying the two together
- Normalize the master quaternion back below 1, as the integrations over time cause it to get too big
- last, convert the master quaternion back into angles that humans can understand.
- The resulting angX, angY, angZ are your roll, pitch, yaw. Watch pitch (AngY) and yaw (AngZ) values for max tilt lockout.
< Code in the next post >