Propeller DCM code

Well here it is, the first DCM propeller code (that I have seen).  This is based on the wealth of work put forth by Bill Premerlani and others on the Ardupilot/matrix pilot side.  I must admit that this is the first enormous coding project that I have undertaken and as such represents a major work in progress.  Consider this V0.0001.

 

Hardware:


I am using my own custom board with two propeller chips and the following inertial hardware:

1 x Sparkfun 5dof board (The older green one)(analog)

1 x LISY300 gyro for the z-axis (analog)

1 x HMC5843 3-axis magnetometer

1 x MAX1270 ADC

1 x MPXV5004DP Dual pressure sensor (analog)

1 x MPXA6115 Barometric pressure sensor (analog)


I also have a Copernicus GPS mounted but currently unused.  Telemetry is handled by a 900MHz X-bee pro XSC.


So far the largest shortcoming of this code has turned out to be not keeping proper track of axis alignment. This will be the focus of the next release.  For the PI tuning gains I used the AMAZING Viewport software debugging environment.  I would highly recommend that anyone using the propeller look into this package (Free 30 day trial).


My tuning methodology was as follows:

Initialize the DCM matrix with an error in one axis.  The values can be computed via Premerlani's paper.  I chose to offset the matrix by ~10degrees


In the DCMupdate vector, make the off axis gyro signals zero, this will effectively turn your board into 1-dof.  I.E. if you are tuning the x-axis, then make the y and z gyro values zero.  Just make
sure you do not move it around a non-functioning axis.  Turn the kIa gain to zero and adjust the kPa
gain until you are satisfied with the settling time.  Then slowly increase the kIa gain to eliminate the steady-state offset. That's all there is to it!  This completely neglects the countless hours I spent trying to figure that out..


This tuning methodology works equally well for the z compass axis.

 

Once you have the PI gains set, you can adjust your gyro scaling by placing your board at a known angle (30 degree drafting triangles work well here), waiting for the DCM to stabilize and then rapidly removing the block and making the board level. Overshoot? Reduce the gains. Undershoot? Make them larger.  You can also adjust them analytically if you know the correct scaling signal, mine is a bit funky due to the ADC setup. And my X and Y axis seem to be slightly different. IDG-3200 here I come.

 

The DCM matrix implementation is done using fixed point arithmetic.  I chose to scale everything by 2^14 so as to reduce the chance of overflow errors when multiplying.  Please feel free to change these values but do so knowing that you may run into overflow issues within the matrix multiplication routine.  I also chose to scale the accelerometer readings to 1g = 2^14 as a legacy decision.  If this is changed, know that the associated kPa and kIa gains will need to be retuned.  


I was hoping that we (me and whoever else) could start working on this in a more distributed fashion.  Once the code is verified we would need to add support for GPS headings, velocity corrections, wind estimation and the lot.  How should we move forward on this?!


-Matt

Tags: DCM, Magnetometer, PI, Parallax, Propeller

Views: 686

Attachments:

▶ Reply to This

Replies to This Discussion

Permalink Reply by Matthew Eckman on March 31, 2011 at 7:23am
Great Work. Any new progress? I am currently making an autonomous UAV using a Propeller and the Wii MotionPlus and Nunchuck as the IMU. At the moment I am using a basic Kalman Filter and PI loop, and I am interested in trying other methods.
Permalink Reply by William Premerlani on March 31, 2011 at 3:29pm

Hi Matt,

Very nice work. It certainly is, as you said, "enormous". I wish you well, and hope that a community grows to help you continue to develop it.

Could you post a picture of your board? I would like to see it.

The pressure sensors are a good idea.

I like the way you are determining the PI drift compensation feedback gains. Very nice.

Regarding the gyro scaling, what I do with the UAVDevBoard is to run the "roll-pitch-yaw demo" without any yaw reference. (No magnetometer). Recently, we added an "integrate and dump" technique to the sampling, as well as a high rate of sampling, that has lowered the uncompensated drift rate to less than 1 degree per minute, so we can measure the gyro scaling by spinning the board on an old record player that conveniently lets you remove the center spindle. I spin the board 10 or 20 revolutions, and then look at the accumulated yaw error. This lets me estimate the gyro gains to better than 1% accuracy. I revise the gains and repeat until I can spin the board 20 times with zero accumulated error.

I assume that the gains for the roll and pitch axes are the same, but they could be measured the same way if you are willing to build something to hold your board on its side while you test it.

Best regards,

Bill

Permalink Reply by Bot Thoughts on September 9, 2011 at 8:26am

Very cool (digging up old thread). Any developments on this over the last year?

Permalink Reply by Navigator on January 22, 2012 at 11:31am

Hi Bill,

It is great work, I am working on a bit different project, I want to use DCM to get correct orientation(Yaw Pitch Roll) of Smartphone, I  have Gyro, Accelerometer and Compass in Smartphone, I am using compass instead of GPS for Yaw correction,

The problem I am facing is that when I change only pitch value(When I move mobile up/down), I get correction in pitch value but my roll value is distorted (It should be near zero), I get abrupt change of -180 to 180 in roll value, I am using atan2 to get the roll value. 

I get the correction when pitch value is less than 90 but above 90 it distorts the roll value, How Can I overcome this problem.

Best regards

Navigator.

Permalink Reply by William Premerlani on January 22, 2012 at 5:32pm

Hi Navigator,

There are two related problems that you might be running into:

1. If you are treating the magnetometer as a tilt compensated compass, it will not work for pitch values near 90 degrees. A better way to do it is to use the magnetometer measurement as a reference vector instead. Use the direction cosine matrix to transform the magnetometer vector from body frame to earth frame, which works in any orientation, even with 90 degree pitch. Then, extract the horizontal component of the magnetometer earth frame vector, and take the cross product of it with the known horizontal component of the earth's magnetic field. The result is a rotation error vector that you transform back into the body frame, and use it to compensate for gyro drift. That is technique we are using in MatrixPilot, it works for any orientation. A combination of two reference vectors (magnetometer and accelerometer) will provide a 3 axis lock.

2. If you are using Euler angles to represent orientation, they do not work for 90 degree pitch. There is an effect known as "gimbal lock" that throws off the roll. It is better to use either an angle and rotation axis representation, or quaternions.

From your description of the problem, it sounds like you are simply running into gimbal lock.

Best regards,

Bill Premerlani

Permalink Reply by Navigator on January 23, 2012 at 3:19am

Hi Bill,

Thank you very much for your explanation, Now I understand where is the problem (gimbal Lock), I have gone through IMUdraft2.pdf, but I could not understand properly how I can use an angle and rotation axis representation to find the three angles. I have also seen the Matrixpilot code (by PaulBizard attached) there also roll , pitch and yaw are calculated using rotation matrix elements  and using function atan2.

Best regards

Navigator

Attachments:
Permalink Reply by Navigator on January 24, 2012 at 10:48am

Hi Bill,

I am still waiting for some response from you, Can you please a little bit explain that how I can represents the orientation using and angle and rotation axis instead of Euler angles. (Sorry I am not so good at Math)

regards

navigator

Permalink Reply by Bot Thoughts on January 24, 2012 at 11:55am

I wished I'd done/understood more linear algebra. I've been spending a lot of time with this algorithm and code variants lately.

If it helps, in MatrixPilot (e.g., older version 2.5.1) there's a function, mag_drift(), in rmat.c, which may have some hints for us on how to do this.

If you haven't, try reading through these a few times:

http://gentlenav.googlecode.com/files/DCMDraft2.pdf

http://www.starlino.com/wp-content/uploads/data/dcm_tutorial/Starli...

Looking through mag_drift() I kind of get what's going on at least up to the point of calculating error. Still trying to grasp the rest. I'd been looking at v3.2.1 with delay compensation; v2.5.1 doesn't have it and may be easier to grasp. I'm looking at that now.

Best of luck.

Permalink Reply by Bot Thoughts on January 24, 2012 at 11:05pm

It turns out that after calculating error, the rest of the code is intended to automatically null out the magnetometer offsets. "Note that the yaw compensation takes only six lines, it is a lot easier to do in terms of direction cosines than it is in terms of tilt-compensating the magnetometer in terms of Euler angles" from http://gentlenav.googlecode.com/files/MagnetometerOffsetNulling.pdf

Permalink Reply by William Premerlani on January 25, 2012 at 7:21pm

Hi Navigator,

Most of the theory of using rotation matrices in MatrixPilot is explained here. There are two fundamental concepts:

1. The rotation matrix can be used to transform vectors between body and earth frame of reference.

2. Reference vectors can be used to detect gyro drift. The cross product of two reference vectors that should be aligned produces a rotation error vector that can be used directly to adjust gyro drift.

For more information, see the wikipedia entry on rotation matrices.

Best regards,

Bill Premerlani

RSS

Social Networking

Top Content 

1 ArduCopter-3.0 ready for wider use

ArduCopter-3.0 ready for wider use
2 APM Project vs DJI NAZA and WOOKONG

APM Project vs DJI NAZA and WOOKONG
3 APM 2.5 with NEO-6M ublox gps (not 3DR LEA-6)

APM 2.5 with NEO-6M ublox gps (not 3DR LEA-6)
4 Flying 3D Spline Curves between Waypoints with ArduCopter 3.0

Flying 3D Spline Curves between Waypoints with ArduCopter 3.0
5 APM Copter 2.9.1.b has been released

APM Copter 2.9.1.b has been released
6 10k autonomous flight - telemetry guided flight, and other APM tests!

10k autonomous flight - telemetry guided flight, and other APM tests!
7 MAAT final version is being tested

MAAT final version is being tested
8 Getting ready for Arducopter 3.0

Getting ready for Arducopter 3.0
9 Help Tuning Arduplane 2.73 With A Large Slow Airframe

Help Tuning Arduplane 2.73 With A Large Slow Airframe
10 How to succeed without really trying

How to succeed without really trying

Contests

Season Two of the Trust Time Trial (T3) Contest has now begun. The fourth round is an accuracy round for multicopters, which requires contestants to fly a cube. The deadline is April 14th.

A list of all T3 contests is here. 

Groups

Advertisement

© 2013   Created by Chris Anderson.   Powered by

Badges  |  Report an Issue  |  Terms of Service