1 out of 1 members found this post helpful.
Did you find this post helpful?
|
Re: Quadcopter on an 18F25K20
The code is about 1400 lines at this point (probably could/should be shorter - plenty of white space) and definitely not streamlined and would probably offer a wealth of lessons on how not to comment your code. There's also some possible IP I might want to protect. However I can answer some of the questions. I will say the hardest part of the code was to figure out how to do everything with 16bit integers (there's a lot of special multiplications like '*/' and '**' I had to use).
It uses a MPU6050 (i.e. GY-521 module you can get on ebay for <2$ each), which is a 6 DOF accelerometer and a Honeywell HMC5883L magnetometer (GY-273 module - also cheap on ebay). If you anyone uses the GY-273 module, be careful as some are actually a QMC5883L which is a clone of the Honeywell version and requires a different address, but will still work ok.
I modeled a couple filters in Matlab, some using quaternions, others using a DCM (direct cosine matrix). I also looked at Madgwick and Mahony filters, which are types of complimentary filters typical for drones, and ended up throwing away both of those options also. Ultimately I settled on a variation of the DCM method that tracks just 2 vectors: up and east. Then I started writing the code in PBP using the Matlab code as the basis for the filter. The overall loop looks something like this:
- Take new gryo/accel/mag readings
- Turn on motors that need to be turned on, preload the highbyte of timer1 based on when the first motor needs to turn off, and start timer1
- Calculate the 3x3 transformation matrix based on the gyro readings. This is tricky as the DCM needs to be normalized (or pretty close), which is really hard to do with 16bit integer math and (I think) impossible to do quickly. This was challenging to figure out.
- Transform the Vz (up) and Vx (east) vectors using the 3x3 transformation matrix
- Calculate the Vza (up) and Vxa (east) vectors based on the accelerometer and magnetometer measurement. These vectors are not orthogonal, so I do a cross-product of Vza (accelerometer) with the magnetometer vector which gives me an east vector (Vxa) that is orthogonal to the Vza vector. Also, if the acceleration is <0.5g or >2g, or if the magnitude of the magnetic vector is <50% or >200% of the earth's expected field strength, OR if the cross-product result indicates Vza and the magnetometer vector are <7.5deg apart, the code considers the Vza and Vxa vectors invalid sets a variable "vbit" to 0.
- If vbit=1 (good accel and mag readings), blend 255/256th of the Vx and Vz vectors with 1/256th of the Vxa and Vza vectors
- Calculate a running average of the rotational velocities (informs the 'D' component of the PD control of the motors)
- Calculate motor gains based on RC throttle setting
- Adjust motor gains based on the running average rotational velocities and how far away the drone is from the Vz vector
- Adjust motor gains AGAIN based on roll/pitch inputs from the RC receiver
- Repeat
All the while, a low priority timer1 interrupt turns motors off when they need to turn off, and a high priority interrupt measures the ppm pulse durations coming in from the RC receiver on timer0, and timer3 is used to maintain a 200Hz filter rate. There's several vector normalizations that occur during the loop which are needed to make the math consistent and prevent compounding errors from round-off. There's a few functions that were necessary, like calibrating the magnetometer when it powers on (that was actually the most enjoyable part cause the code is "good enough" and I knocked that out in a couple hours).
Hopefully that answered some questions. If y'all have anymore, I can try to answer those also. At some point I might be able to post some snippets of the code that could be useful elsewhere.
Dave
Bookmarks