Calibration of the drone

Hi everyone

I am building a quadcopter drone, with Arduino Nano, which has the sole purpose of flying vertically, without remote control, but will have the MPU6050 which will have to autocalibrate, that is, always keep it horizontally.

I tried to make a sketch of the program, putting too much “if”, which is totally unacceptable, but I don’t know what kind of algorithm there may be behind the autocalibration of the drone, in the sense:
If the drone is skewed on the X axis by 40 degrees, how long should I turn on the right 2 motors and also at what speed?
The same thing for the Y axis and also with degrees less than 0

And I leave you below the components and the code that I am using for the drone.

If anyone has some kind of algorithm or code that they have already been able to try I would be really grateful if you could help me with this problem I am having.
Thanks for the attention

Componenti-Drone-Volo-Verticale (4).pdf (245 KB)

Componenti-Drone-Volo-Verticale (4).pdf (245 KB)

Programma_Drone.ino (12 KB)

Why does it have to autocalibrate? An initial calibration is sufficient. You can do this in a separate code.

Edit: You are talking about stabilizing not calibration correct?
You are talking about a closed loop system. You should read up on control systems.

So: the drone could start, in many cases, crooked or upside down and the drone has to get into the right position, being horizontal / straight, to start flying high

And I wish that in 2-3 seconds, no more, the drone was straight, not crooked

I think you really need to learn some basic control systems. Stabilizing a drone (getting it to the right position) is more than just a lot of if statements.

Here, watch this video:

I saw a PID controller library for Arduino. I have never used it because I have never needed to do such a project. I have only had control systems in the university.

Here are more videos from Brian Douglas that will help if you don't understand.