 # Self-correction for UAV based on IMU. [math heavy]

Hello!
I’m making a drone for a school project, but I’m having trouble programming in the self correction mechanisms for the drone so that it won’t tip itself over.
Here is what I want to do:
Using a set of Euler vectors (v = <X,Y,Z>) that represent the drone’s orientation and are fed sequentially, build code for an N-rotor drone where N is variable, and all rotors are coplanar and equally spaced, that will correct the motors to make the drone tip along the desired axis and to a certain tilt between 0 and 10 degrees on the Y or Z axis.
here is the mapout:
Z component is pitch, positive Z is positive pitch.
Y component is Roll, negative Y is positive Roll.
X component is yaw. Positive x is positive yaw.
all of the euler angles are in degrees.
I use radians in the first paragraph.

here is my attempt:
the drone takes user input at the radio, with its aileron and elevator (pitch, roll) stick. I convert the position of the stick to a pair of polar coordinates (θ,R) and I add PI/2 to θ to get the axis to be rotated upon. For example, if you push the stick straight forward, the drone is expected to tilt on an axis perpindicular to the θ of the stick in order to move forward. I then take the sin(θ-m) where m is the motor rotation➀. What this does is make any rotor that is close to perpindicular to the axis of rotation differentiate more, and the propellors that are close to the axis change less. so if you move the stick forward, the moters that are closer to the front and the back are going to change speed many times more than the rotors on the sides. I then mulltiply sin(θ-m) with k*R where R is the distance from the stick, and k is the maximum percent change in power that I want to see.

So with the accelerometer, I take the expected distance that we want the motor to move sin(θ-m)*R,
and I’m trying to figure out the actual distance that the motor is from the axis using the euler angles. I’ll then subtract the actual from the expected to get the change in power that I need. The problem is that I have no clue how to convert the Y,Z components of the vectors to a distance using the angle that the rotor is from the “front” ➀. This is where I’m stuck.

If there is anything that you’d like illustrated about this post, tell me and I’ll tro to post a picture.

➀Imagine that the N rotors are on the face of the clock, with the front of the copter at the top. Each motor is a certain ammount of radians from the 12 o’ clock position. r

The problem is that I have no clue how to convert the Y,Z components of the vectors to a distance using the angle that the rotor is from the "front"

This is completely unclear. What vectors? What are Y and Z components?

Make a diagram with labels and post it.

jremington: This is completely unclear. What vectors? What are Y and Z components?

Make a diagram with labels and post it.

I tried my best.. The disk is the UAV and It's plane that is tipped upon. The line with a red dot is the arm with a motor D is what I need to find.

That is trigonometry. Sines and cosines.

Sorry, I still don't completely understand the question, the problem, or how you intend to solve it.

I understand that you want to tilt the copter by some angle, set by the joystick. My simplistic approach would be to decide what the final Euler angles would be and set those to be target values.

Use PID loops to make the current Euler angles equal to the target values, by varying motor thrust(s). You need three loops for the three angles.