Simple Quaternion Library?
michaellangford99
Posts: 19
in Propeller 1
Hello,
I was wondering if anyone knows of a Quaternion library for use in an IMU, or how to make one. In my current setup, my gyro will lose 90 degrees if I tilt it 90 in pitch, then roll it 90, and then put it back down, because it is just integrating the rate. This would not happen if a Quaternion was used. Even though I am Kalman filtering my gyro and accelerometer, I will still get some data loss for a few iterations. I am making a quadcopter, and though the rotations described would probably never happen, anything remotely like it could severely mess the flight up.
I was wondering if anyone knows of a Quaternion library for use in an IMU, or how to make one. In my current setup, my gyro will lose 90 degrees if I tilt it 90 in pitch, then roll it 90, and then put it back down, because it is just integrating the rate. This would not happen if a Quaternion was used. Even though I am Kalman filtering my gyro and accelerometer, I will still get some data loss for a few iterations. I am making a quadcopter, and though the rotations described would probably never happen, anything remotely like it could severely mess the flight up.
Comments
https://github.com/ZiCog/madgwick.js
Includes Madgwick's and Mahony's AHRS accel, gyro and compass fusion code in C and Javascript.
From my limited experiments it works quite well with out the compass inputs.
The C could be usable on the Prop as is, if it fits. Else you will have to port that to Spin.
Didn't someone already do this in Spin ?
It's not a quaternion, but it's easy enough to get the angles out using ArcSin.
http://forums.parallax.com/discussion/131022/propeller-dcm-now-with-source/p1
And there's also: https://github.com/parallaxinc/Flight-Controller
It uses a quaternion for the updates, then converts to a matrix to do the drift compensation. It's still in development, but it's flyable now. I'm hoping since it's open source, no one will mind me posting it early.
https://github.com/srlm-io/anzhelka/blob/master/software/spin/src/block_moment.spin
PUB Calculate 'One iteration of the calculations 'Should be called from a repeat loop... 'Writes object local variables, does not write to address fp.FInterpret(@MOMENT_BLOCK_INSTRUCTIONS) {{AZM_MATH MOMENT_BLOCK 'q star: q_1 = 0 - q_1 q_2 = 0 - q_2 q_3 = 0 - q_3 'Moment Block, first Quat Mul q_tilde_0 = ((q_d_0*q_0) - (q_d_1*q_1)) - ((q_d_2*q_2) - (q_d_3*q_3)) q_tilde_1 = ((q_d_0*q_1) + (q_d_1*q_0)) + ((q_d_2*q_3) - (q_d_3*q_2)) q_tilde_2 = ((q_d_0*q_2) - (q_d_1*q_3)) + ((q_d_2*q_0) + (q_d_3*q_1)) q_tilde_3 = ((q_d_0*q_3) + (q_d_1*q_2)) - ((q_d_2*q_1) + (q_d_3*q_0)) alpha = 2 * (q_tilde_0 arc_c 0) t_1 = (alpha / 2) sin 0 ...Resulted in:
This could then be run in a special version of F32 that would chug though the math instruction at full assembly speed. In my testing I found that much of the slowness of F32 was the Spin overhead, and so if you can put your math operations back to back things get alot faster.
https://github.com/srlm-io/anzhelka/blob/master/software/spin/lib/F32_CMD.spin
I don't really suggest that you use the code as is, but maybe it will provide some inspiration. Also in that repo is a pretty handy doc walking through the implementation of a quadrotor control system.
https://github.com/srlm-io/anzhelka/raw/master/doc/reports/quadrotor_mathematics.pdf
maybe this is helpfull
Reinhard