This code would had been too long to post here directly, hence the link:
-Currently employs HC-05 as transceiver
-Sensor as 10DOF (using Accelerometer+Magnetometer), you can replace your code for MPU6050 in the code.
-Loop time 4ms, i.e. 250 Hz
-Simple Clean Code ;)
If you have any suggestions/bug related to my code, please let me know.
If any questions, which you should have, let me know. Will be happy to help.
This is an old video, based on the code, but without any stabilization implemented: