‫So let's now go to our support file, and so here is your trajectory generator function that takes in
‫your time vector here.
‫I take the necessary initial constants from my init function from this one here.
‫So Python classes are built in such a way that if you put something in the Init function, then in other
‫functions you can take that stuff from the unit function.
‫Then here you have the high difference, final height minus initial height.
‫So the spiral starts at five meters and ends at twenty five metres in altitude.
‫Then twenty five minus five would give you 20 meters, which is the difference.
‫Height.
‫And while this alpha this is what goes into these cosine and sine functions, so you have two times
‫by times, frequency, times, time.
‫So if you have a spiral, then it essentially determines how many rotations you will have in one hundred
‫seconds.
‫And, well, if you remember, then you have nine trajectories.
‫This is your first trajectory.
‫So these are your equations for your spiral.
‫And if you remember, we also had to take their derivatives and their second derivative.
‫So this is your derivative part and then this is your second derivative part.
‫And we did this one in the lectures as well.
‫But then we also have different trajectories.
‫So we have trajectory to here, trajectory to is built in such a way that first of all, I create a
‫trajectory one and then somewhere in the middle, a little bit before that, I just overwrite the second
‫part.
‫So in trajectory to the first part of the trajectory would be the spiral that you have here.
‫But then the second part of that trajectory will be something else.
‫So that's why you see that I simply overwrite the elements starting from the one hundred and first element
‫up until the end.
‫And then you have its derivatives and second derivatives.
‫The same logic applies to trajectory three and trajectory four, where I overwrite the second part,
‫then trajectory five and trajectory six.
‫They are here.
‫They are like tornadoes and trajectory five, your radius increase linearly and then in trajectory six,
‫it increases with the power of two.
‫And so that's why trajectory six really looks like a tornado.
‫Then you have trajectory seven, you have trajectory eight and then you have trajectory nine.
‫This is the crown that I think I showed you one time in the lectures.
‫And, well, you can see that all these trajectories, they are inside the if statements.
‫So when you choose something in your init function, when you choose a trajectory, then it will give
‫you the right one.
‫So that's why you have these if and live statements.
‫Then here I calculate the difference of the X, Y and Z values, and here I add one more element to
‫it and they need that in order to construct our reference value vector for the yawing.
‫Also BPCI Subha and it's this one here and the code here is written in such a way that it remembers
‫how many rotations you have had.
‫So for example, if in a spiral you have rotated two times, then you will have gone not from a zero
‫to two pi radians two times, but you will have gone from zero to four pi radians.
‫And if you go in another direction then you will reference values become negative.
‫So if you start turning clockwise, then you go from zero to minus two pi radians or from zero to minus
‫four pi radians.
‫But this code remembers how many rotations you have had and then you just take all your vectors and
‫you give it to your main file.
‫And in the main file, these are your reference value vectors.
‫You see, you've got everything here.
‫You have X ref, X dot traffics, dot, dot, ref, the same thing for the wise and zis and also the
‫your angle reference vector and then this one here.
‫Well that's just the number of outer control loop iterations.
‫So when you go from zero to one hundred seconds with an interval of zero point four seconds, then it
‫essentially tells you how many elements you have in this factor here.
‫And I'm going to need that when I start running the outer loop and then here are my states.
‫All right, so I have u v w p q ah.
‫So these are the translational velocities in the body frame.
‫These are angular velocities in the body frame.
‫And these are your inertial frame, X, Y, Z states, and these are your inertial frame Fifita and
‫precise states following our convention.
‫That was our stop the Z, Y, X.
‫So we first rotate about the Z axis, right.
‫About the moving frame Z axis, then about the moving from Y axis and then about the moving frame X
‫axis.
‫And so all these are my initial states.
‫That's what I have at the very beginning.
‫So at the very beginning, I'm not going anywhere.
‫So that's why all my velocities are zero meters per second and zero radians per second.
‫And then these are my initial position values.
‫You can see that I start from X equals zero Y equals minus one meter.
‫So I'm not starting completely at the origin.
‫I'm a little bit shifted in the Y direction at the very beginning and then my initial altitude is zero
‫meters.
‫And then in terms of rule and pitch angles, these angles are zero radiance, so I'm not tilted in a
‫weird way or something.
‫However, my initial your angle is aligned with the first element of my reference, your angle vector.
‫So if my reference, your angle vector tells me that at the beginning I should be rotated by pi over
‫to radians, then at the very beginning I will also place my drone like that so that the drone would
‫be oriented like that as well in reality.
‫So normally if my body frame x axis is in this direction.
‫So this is my body from x axis here.
‫But if the first element of this vector is pi over to radians, then that means that I will take my
‫drone and I will rotate it in such a way that this body from x axis will point up.
‫And then if right now, if you look at this one, then your body from y axis would point up, then if
‫I rotate my body frame by prior to radians, then my body frame y axis will actually point to the left
‫because my body frame x axis would point up.
‫And then I take all my initial states and I'm going to put them in this array and I'm going to call
‫this area states and then I'm going to have your states total and I'm going to put states inside stepstool.
‫And the states tool, it will be accumulating all the states, so you can imagine that every year,
‫one second, all these states will update.
‫So I will have a new states vector.
‫However, I want to remember my previous states as well.
‫So I'm going to be collecting all the states throughout my time in this state's total vector.
‫And, well, this one is for the animation purposes.
‫And then here I have a vector for the angle reference values.
‫And then I also need some kind of a vector for the X, Y, Z velocities, so I'm going to need them
‫later once I start running my control loops.
‫And then I'm going to define my initial propellor angular velocity, so you see that they are three
‫thousand radians per second here.
‫So at the very beginning, my omega's, they are not at rest.
‫They are actually rotating.
‫They are rotating at three thousand radians per second.
‫However, this rotation will not produce enough thrust to overcome the force of gravity and therefore
‫your drone will not be flying anywhere with these rotations.
‫And well, here I take my thrust and drag factor and the length of the drone from the center of the
‫drone to the center of a propeller.
‫And based on this initial data here and based on these constants, I can compute my initial you want
‫you to Q3 and Q4 that I store in this vector.
‫And later on when I get my new you want you to use three four.
‫I will also store them in this vector.
‫But this vector will remember the previous controller inputs as well.
‫So I need them in order to be able to plot everything in the end is the same thing with this state's
‫total vector and the same thing here.
‫This Omega's bundle vector.
‫I want to collect all my omegle values throughout the time and then while this is again for the animation
‫purposes and now we can start our controller.