Propeller DCM - Now with source
JasonDorie
Posts: 1,930
This is a Parallax Propeller chip running a partial implementation of Premerlani and Bizard's work on Direction Cosine Matrix computation for orientation sensing. (Check out their paper on DIYDrones) I'm hoping to use this in a quad-rotor without a GPS or magnetometer - I wanted to see how good the algorithm was at stabilizing pitch and roll. So far it looks very promising.
All the work is done on the Propeller itself. It reads the gyro and accelerometer values on one core, and computes the DCM from those readings on another core. At this point the code is still written in the high-level language, Spin. Since all the math is fixed point, there's a strong chance I won't actually have to write an assembly version - I haven't timed it yet, but my update loop is running at 200Hz, so it's at least that fast.
I'm hoping to code the flight control stuff next, and then I'll see if I can make my quad hover without user input.
Here's a video of it in action: [video]http://www.youtube.com/v/4bj2ieD4HA0[/video]
The attached code includes the C# visualization app with the initial implementation for the PC, as well as the current code on the Propeller. It's not finished, but I thought it might be useful for others.
All the work is done on the Propeller itself. It reads the gyro and accelerometer values on one core, and computes the DCM from those readings on another core. At this point the code is still written in the high-level language, Spin. Since all the math is fixed point, there's a strong chance I won't actually have to write an assembly version - I haven't timed it yet, but my update loop is running at 200Hz, so it's at least that fast.
I'm hoping to code the flight control stuff next, and then I'll see if I can make my quad hover without user input.
Here's a video of it in action: [video]http://www.youtube.com/v/4bj2ieD4HA0[/video]
The attached code includes the C# visualization app with the initial implementation for the PC, as well as the current code on the Propeller. It's not finished, but I thought it might be useful for others.
Comments
I am planning on doing some flight tests/hovering in the next few days, the end of the semester is quickly approaching and I have to finish this......I'll share my results and finished code when it's working sufficiently.
A strong lightweight cheap frame could easily be made from carbon graphite golf clubs, I purchased 4 from Salvation Army for $0.50 each!
The DCM solution is elegant because it solves the orientation problem as a whole, unlike the Kalman implementations kicking around that treat each axis independently. Those will work for a balance bot, but quickly accumulate errors when rotating on multiple axis because of gimbal lock. The DCM seems to be no more expensive, and possibly cheaper.
The golf clubs are a cool idea - I found cheap carbon in China, but not that cheap!
In the DCM object, in the Main method, where you call the gyro and accel readings:
repeat
Gx := Gyro.GetRX
Gy := Gyro.GetRY
Gz := Gyro.GetRZ
Ax := Gyro.GetAX
Ay := Gyro.GetAY
Az := Gyro.GetAZ
What exactly are the values? I am assuming the gyro values are deg/sec. What are the accel values? Raw? g's? or acceleration in m/s^2?
I am using the Wii MotionPlus with a Wii Nunchuck so if the sensor readings match then it should be plug and play.
This would be a lot better than the Kalman (too much floating point math).
Accelerometer values are 1g = 1.0 in 16.15 format.
There are a few places in the original paper that got glossed over a little - one of them being that the update matrix for applying the incremental rotations is composed of raw gyro values, not bothering to take sin or cos. At 200 loops per sec, even if something is rotating at 200 degrees per second, that's only 1 degree per update. Cos of that is basically 1.0, and sin of that is approximately the value in radians. It's inaccurate, but the renorm step cleans it all up.
The other is the error term - it takes a cross product and then feeds a scaled version of the resulting vector back in as Euler angles. Basically the vector represents an amount of error in each axis of rotation. I'm still wrapping my head around that one, but I suspect that the scale of components in the vector doesnt matter, and the vector itself represents how far you have to rotate along each of your 3 rotation axis to get back in line with the gravity vector.
I'm hoping to get some time in the next week or so to implement a control scheme based on it. I poked at it a little last night and it doesn't seem like it'll take too long to do.
I was trying to fine tune my Kalman today and was getting upset at the GyroZ axis (yaw) since the Kalman I have doesn't filter it, I have to use the raw data. Any little touch of the UAV made the GyroZ axis crazy and jump around. So I went back to basics and picked apart what the code is doing. I realized that the Wii MotionPlus has a fast mode and a slow mode for fast and slow rotations. I started to change that one value and by decreasing it I decreased the overall rate of measurement which in turn provided a more stable signal. Once I found a reasonable value for the rate, I then deleted the Kalman filter and applied the same value to the Gyro X and Y axis. It's working great so far! In essence I believe all I did was simulate slowing the sample rate of the MotionPlus. This is perfect for my Quad-Copter because it is not acrobatic, it is a slow moving autonomous robot. Also after physically manipulating the UAV and watching the gyro data and PID outputs, everything leveled out whenever I was holding it still. I didn't notice any drift/errors. This may all change once the propellers are on the motors, we'll see. Eliminating the 20+ lines of math operations for Kalman increased the overall speed of the PID loop as well.
One of the biggest problems I am having is trying to get the Wii Nunchuck accelerometer values accurate. The UAV needs to know it's instantaneous angle, i.e.- if the UAV needs to move forward then it needs to know to pitch until it is pitched by 20-30 degrees. I am getting drift errors during this part. Since I only have 2 weeks to finish this project I plan on using a ranged value as a set-point. So if the UAV is pitched/rolled within the specified range then all is fine. I don't think that will cause a problem, again this won't be doing flips or sharp maneuvers.
I've attached a picture of our UAV. Most of it is done, just some wiring issues and we still need to mount the Geiger counter.
In the picture, center on the top is 5 Sharp IR sensors; then the IP Camera on the side; ADC chips, Wii Nunchuck and MotionPlus, and a digital compass is on an expansion board; then the propeller board; underneath is the Spinneret and wireless router and one more Sharp IR sensor. On the ring is the batteries, ESC's, motors, power switch, and start button.
I wasn't a big influence on the physical design (I suggested a traditional frame). From calculations it should fly....if the blue ring doesn't cause too much loss of efficiency and if my code is sufficient.
I am using orientation sensor VN100 from Vectornav:
http://www.vectornav.com/
to make internal measurement unit (IMU) which work as the same of your project "Propeller DCM".
My experience in using OpenGl is very limited, so, I would be most thankful if you kindly modify the my attached file vectornavform1 to work as similar to "Propeller DCM".
Also, I wonderful about using PictureBox to contian OpenGL output.
Thank you and look for your reply.
This is really great. I've been learning about the propeller and programming in spin the last few weeks, and this project really grabbed my attention. I've had a look at your code (I can actually read the spin portion of it! - the PASM not so much :P ) and have seen the video. I have a Protoboard, and I would really like to try it myself. Would it be possible to get a sketch or description of the connections required on the protoboard, and the components - I assume the ITG-3200 / ADXL-345 breakout boards are from sparkfun, but I couldn't make out what the rest of the components on the protoboard were from looking at your video.
I have the sparkfun board that is the IMU3000 with the ADXL-345 chip on the same board. I will be attempting to get your code to work on that board soon. It has a mode wear you can just read the raw gyro and acc values (without doing it's fusion thing that no one seems to be able to get working properly yet anyway). Thanks for sharing your code.
This is excellent. I had implemented DCM in Spin a few months ago, but I used a fixed-point object from OBEX for the math, and I couldn't get it to update faster than 75Hz. It would appear that your approach to fixed-point is much faster. I was waiting for the MPU-6000 chip to solve the speed problem, but now maybe I can do without it. Thanks for sharing!
Jasons DCM code does not contain conversion to euler angles (pitch,roll, yaw). At least I could not see this part. I think he did this in the visualisation software on PC.
I think fixed-point Atan2 function may not fast enough for 200Hz loop
I did do the visualization software on the PC, but I just used the matrix directly - No conversion to Euler required.
I left in the code that does initial configuration of the ADXL345 chip, as comments, so you can compare it with the corresponding code for the MMA8451QT.
On the Builders Forum http://forums.parallax.com/showthread.php?133372-Ken-s-QuadCopter-Build-Log-(now-includes-videos) there is a log of building QuadCopters.
Jason has sent me his latest code and I am in the process of building my Quad and getting it working. My posts are now on Kens build log.
thanks for posting that code Jason. :-)
Found a couple of bugs in the itg-3200-pasm file.
(1) In the ADXLReadValues "sub". The last read doesn't NAK, which upsets the ITG gryo read as the conversation with the accel isn't finished.
mov i2cMask, #%10000000 test i2cTestCarry, #0 wc ' Clear the carry flag to make reads auto-increment call #i2cRead rol i2cMask, #16 ' This device reads low/high byte [b]test i2cTestCarry, #1 wc ' Set the carry flag to tell it we're done[/b] call #i2cRead 'Sign extend the 15th bit test i2cData, i2cWordReadMask wc muxc i2cData, i2cWordMask mov iaZ, i2cData(2) The i2cStop command is missing from the bottom of both read sections.
Just thought I'd share. Now if I can get mine to be stable as in your video....
Attached updated code.
Cheers again,
James
I'll have to take a look at your changes in detail - I know that the code I have works (it's what I used to post the DCM demo), but it may be that it only *mostly* works without these changes. :-)
Thanks for sharing the fixes. Hope to see you flying soon!
Jason
I used your code to test the ITG-3200 some months ago and it works.
Now want to test DCM with ITG-3200 + ADXL 345.
Please can you add to your program , a sketch of the connections you use (with pin numbers).
I fear to make a mistake and damage a component.
Thanks for help.
Jean Paul
I have pins 16 & 17 connected like this:
16 - SDA
17 - SCL
The other two pins are power and ground. I'm using the ITG3200 and ADXL345 breakout boards from SparkFun, so there are only four wires to connect for each board, and they're both connected to the same set of four pins. As long as you get the "power" line correct you're unlikely to damage them.
Jason
Have only to change the COM number.
Hope some day to see this help flying a quadcopter.
Jean Paul
http://www.pololu.com/catalog/product/1265
The best part is the black friday sale $30.00
http://www.pololu.com/blackfriday2011
http://www.youtube.com/watch?v=Ge8L9f6rzA0&feature=related
Jean Paul
That's a very good deal - I ordered one myself.
I tested this with an MPU-6050, which is circuit-compatible with the ITG-3200.
The control and data registers for the 60X0 chips have different addresses and different layout from the ITG-3200. Register maps for the chips are now (finally!) available here:
http://invensense.com/mems/catalog.html
Have you seen this thread on Quadcopters etc http://forums.parallax.com/showthread.php?133372-Ken-Cluso99-W9GFO-JasonD-s-QuadCopter-Build-Log-(updated-info-ELEV-8-availability)
It is a long thread, so you may want to look at the end to see Jason's quad flying.