-
Notifications
You must be signed in to change notification settings - Fork 3
/
TranslationalAccelerations.m
53 lines (48 loc) · 1.59 KB
/
TranslationalAccelerations.m
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
function accelerations = TranslationalAccelerations(MotorThrust, Orientation, Velocity)
%#codegen
global m;
global g;
global rho;
global ModelCd0;
%calculate the angle of the model
Bank = Orientation(1);
Elevation = Orientation(2);
Heading = Orientation(3);
%set up the rotation matrix
Rx = [1,0,0; 0, cos(Bank), -sin(Bank); 0, sin(Bank), cos(Bank)];
Ry = [cos(Elevation), 0, sin(Elevation); 0,1,0; -sin(Elevation),0,cos(Elevation)];
Rz = [cos(Heading), -sin(Heading),0; sin(Heading), cos(Heading),0;0,0,1];
Rcombined = Rz*Ry*Rx;
%Calculate the Total Thrust From The Model
TotalThrust = sum(MotorThrust);
%TWR = TotalThrust/(m*g)
%Convert Thrust to Inertial Coordinates
TotalThrustInertial = Rcombined*([0,0,TotalThrust]');
%CalculateDrag Opposing Motion%
%THIS IS DUMMY DATA%
AreaVertical = (0.3*0.3);
AreaHorizontal = (0.3*0.01+0.1*0.1);
Sx = abs(AreaVertical*sin(Bank)+AreaHorizontal*cos(Bank));
Sy = abs(AreaVertical*sin(Elevation)+AreaHorizontal*cos(Elevation));
Sz = abs(AreaVertical*cos(Bank)*cos(Elevation)+AreaHorizontal*sin(Bank)*sin(Elevation));
vx = Velocity(1);
vy = Velocity(2);
vz = Velocity(3);
Dx = 0.5*rho*vx*abs(vx)*Sx*ModelCd0;
Dy = 0.5*rho*vy*abs(vy)*Sy*ModelCd0;
Dz = 0.5*rho*vz*abs(vz)*Sz*ModelCd0;
Drag = [Dx, Dy, Dz];
%END DUMMY DRAG
%Calculate the Thrust Forces on the model in inertial Coordinates
xThrust = TotalThrustInertial(1);
yThrust = TotalThrustInertial(2);
zThrust = TotalThrustInertial(3);
%Combine With Drag
xForces = -xThrust - Drag(1);
yForces = -yThrust - Drag(2);
zForces = zThrust - m*g-Drag(3);
xA = xForces/m;
yA = yForces/m;
zA = zForces/m;
accelerations = [xA, yA, zA];
end