Arduino Nano Based D.I.Y. Quadcopter Attitude Control Problem

Hello guys!

I'm making micro-sized quadcopter for embedded system eduction in Korea.

I've designed frame and nano-based FCU(Flight Control Unit).

It contains MPU6050 and HC-06 bluetooth module and four DC motor driver.

I think that theory is perfect.

All the hardware works well, and Control through bluetooth also works well!

But I can't control attitude of this micro-quadcopter.

I attach photo of FCU, video and source code from Github.

Here is photo of my FCU :

This is Arduino-Nano based FCU, and MCU use ATMega328P-AU(16MHz)

I've connected motor to PWM pin which is 6, 9, 10, 11.

I know that pin6 has different PWM frequency from other pins(9, 10, 11)

But, I've changed all PWM frequency to 977Hz

This is source code from Github

Github Source Code

And below is video of my quad

Watch Video!!!

Please help me!

IF YOU NEED this FCU, I will send it to your home by EMS or FeDEx

It's going to be very hard to diagnose the problem without debug output. At a minimum you should log the control inputs, the MPU6050 outputs and the resulting motor outputs. Start with the motors unplugged. Make sure that when the control inputs are centered the motor outputs depends on the tilt of the board. Roll left and the two left motors should get more power than the right motors. Similar for pitch. Then keep the board level and make sure the motors are reacting correctly to the control inputs.