Hello all,
As i am new to this forum, short intruduction to myself:
I am a Mechatronics student at SDU in Sønderborg (Denmark). About a year ago me and a friend started developing interest in Quadcopters, and decided to build our own from scratch. In that process we wanted to program our own flightcontroller.
We chose to use two microcontrollers, one for the actual stabilizing, and one to read from the transmitter and later on controlling functions like "return to home" etc.
At this stage, we are using two Teensy 3.1 on the copter communicating over SPI, and one on our "base", receiving data over the XBee wireless connection (serial communication). As sensor we use the ADXL-345 accelerometer and the L3G4200D gyroscope on a GY-80 10 DOF sensor.:
We are programming the Teensys in C, using Kintes Design Studio.
The main angle reading and stabilization is done in an timed interrupt, currently running at 800Hz, looking as follows:
get_raw_data reads the x, y and z from both the accelerometer and the gyro and stores them in the struct "raw". Raw_filter takes the last 10 values, deletes the highest and the lowest (to filter spikes) and takes the average of the remaining, storing them in "filtered_raw". "calc_angle" then calculates the angle in degrees from both acc and gyro data. We checked the angle, and it works fine.
Now, to the most important (and probably hardest) part, the stabilization algorithm. It takes the calculated angles in the "orientation" struct, and calculates the P, I and D feedback with an offset from the remote, "input.pitch" and "input.roll".
This is the code in "stabilize":
We can see that the P, I and D feedback are generally working, and we can see that the quad is trying to stabilize. But it seems there is no value for P (which we tried tuning first) which would make the quad anything near stable. When starting at very low values and slowly increasing, the quad first does almost no reaction at all, and then starts oscillating forth and back. Weird thing is, stabilizing in one axis (quad tied in two ends, only two motors running) seems to work quite good (using both P and D feedback). But in two axis, the quad either oscillates very much or just drops one side as if it was not reacting on the error at all.
Not sure if there is something wrong with our P feedback, since it always oscillates using only P.
In the start we used two arduino nanos instead of the teensys, and where actually able to fly (using the same stabilization algorithm), but far from perfect (see First flight video). At that stage we assumed it was because of the lack of processing power (and feedback speed), so we switched to the teensy. But as that didnt help, it seems we have some mistake/fail in our code. So we hope that any of you guys here can give some suggestions on what might be wrong.
Thanks in advance,
Milan
Replies