% Attitude Dynamics Equations, Torque Free, with magnetic field computation.
% Darren Pais
% Here we start at the pole and go to the equator. Field angle is the angle
% between the magnetic field and the inertial body Z axis.
% October 12, 2006

%---------------------
% Term Definitions

function [data]=attitude5(emuno) 
% Mass moments Kg m^2
Ix=0.23689 ; 
Iy=0.28393 ; 
Iz=0.35307 ; 
%Ix=.0015212;
%Iy=.001448;
%Iz=.001507;
% Magnetic Dipole Moment
mu=emuno;  % A m^2

% Initial Moments about body CG projected in <b1 b2 b3>
M1=0;
M2=0;
M3=0;

% Orbit parameter n for polar circular orbit
alt_km=500 ; a=6378+alt_km;
n=sqrt(3.986*10^5/a^3);

% Initial Conditions

% Angular Velocity vector projected in <b1 b2 b3>
wx_o=0; wy_o=0 ;wz_o=0 ; %0.05236;
% Roll Pitch Yaw angles of BRF wrt MRF
phi_o=0; theta_o=0; psi_o=0;

% Simulation Parameters

% Simulations time in mins for orbit
T=2*pi*sqrt(a^3/398600); %secs
orbit_frac=1/4; 
% Convert to seconds
t_max=T*orbit_frac;
%---------------------

%Initialize Data
wx=wx_o;wy=wy_o;wz=wz_o;t=0;
phi=phi_o; theta=theta_o; psi=psi_o;
data(1,:)=[wx wy wz phi theta psi 0 NaN] ;

%numerically intgrate to get angular velocities at each second (1 sec step)
    
    for i=1:1:t_max
        wx=data(i,1); wy=data(i,2); wz=data(i,3);
        phi=data(i,4); theta=data(i,5); psi=data(i,6);
        
        
        %determine magnetic field vector in orbit in XYZ frame
        b_XYZ=get_XYZ_field2(alt_km,i,orbit_frac) ;
        
        %quarter orbit time period
        r=6378+alt_km;
        T=t_max;
        
        %determine planar elevation angle
        THETA=i/T*pi/2 ;
        
        %determine magnetic field vector in moving xyz frame
        %b_xyz=[sin(THETA) 0 -cos(THETA) ; 0 1 0 ; -cos(THETA) 0 -sin(THETA)]*b_XYZ';
        
        
        %determine magnetic field vector in body b1b2b3 frame
               
        transform=[cos(theta)*cos(psi)   sin(phi)*sin(theta)*cos(psi)+cos(phi)*sin(psi)   -cos(phi)*sin(theta)*cos(psi)+sin(phi)*sin(psi) ;
              -cos(theta)*sin(psi)  -sin(phi)*sin(theta)*sin(psi)+cos(phi)*cos(psi)  cos(phi)*sin(theta)*sin(psi)+sin(phi)*cos(psi) ;
              sin(theta)            -sin(phi)*cos(theta)                             cos(phi)*cos(theta)                            ;] *  [cos(THETA) 0 -sin(THETA) ; 0 -1 0 ; -sin(THETA) 0 -cos(THETA)];
        
        %b_123=b_XYZ' ; 
        b_123=transform*b_XYZ' ; 
        
        %Compute Magnetic Moment Vector assuming magnets are along b3 axis.
        M_vec=mu*cross([0 0 1],b_123) ;
        M1=M_vec(1) ;M2=M_vec(2) ;M3=M_vec(3) ;        
        
        %compute angular velocity of BRF wrt MRF
    
        w1=wx + n*sin(phi)*sin(theta)*cos(psi)+n*cos(phi)*sin(psi) ;
        w2=wy - n*sin(phi)*sin(theta)*sin(psi)+n*cos(phi)*cos(psi) ;
        w3=wz-n*sin(phi)*cos(theta) ;
        
        %numerically intgrate to get euler angles at next time step
     
        [t,Y]=get_angles(w1,w2,w3,phi,theta,psi,i);
        
        phi=Y(end,1); theta=Y(end,2); psi=Y(end,3);
        
        %numerically intgrate to get angular velocity values at next time step
        
        [t,Y] = ode45(@ang_vels,[i-1 i],[data(i,1) data(i,2) data(i,3)]);
        wx=Y(end,1) ; wy=Y(end,2) ; wz=Y(end,3) ; t=i ;
        
        
        %determine angle between (b3) axis and the magnetic field vector
        b_123_norm = b_123/sqrt((b_123(1))^2+(b_123(2))^2+(b_123(3))^2);
        angle=acosd( dot(b_123_norm,[0;0;1]) ) ;
        
        %store data at time step
        data(i+1,:)=[wx wy wz phi theta psi t angle] ;
    end


% Euler Angles Definition System ODE
    function [t,Y]=get_angles(w1,w2,w3,phi,theta,psi,i)
        [t,Y] = ode45(@euler_angles,[i-1 i],[phi theta psi]);
            function dy = euler_angles(t,y)
                dy = zeros(3,1);    % a column vector
                dy(1) = cos(y(3))/cos(y(2))*w1 - sin(y(3))/cos(y(2))*w2;
               dy(2) = sin(y(3))*w1+cos(y(3))*w2;
               dy(3) = -sin(y(2))*cos(y(3))/cos(y(2))*w1 + sin(y(2))*sin(y(3))/cos(y(2))*w2 + w3 ;
            end
    end


% Angular Velocities Definition System ODE
    function dy = ang_vels(t,y)
        dy = zeros(3,1);    % a column vector
        dy(1) = 1/Ix* (M1-(Iz-Iy)*y(2)*y(3)) ;
        dy(2) = 1/Ix* (M2-(Ix-Iz)*y(1)*y(3)) ;
        dy(3) = 1/Ix* (M3-(Iy-Ix)*y(1)*y(2)) ;
    end

% Plotting Commands
subplot 221
plot(data(:,7)/60,data(:,4)*180/pi) ;
xlabel('Time in mts');
ylabel('Roll angle phi deg');
subplot 222
plot(data(:,7)/60,data(:,5)*180/pi) ;
xlabel('Time in mts');
ylabel('Pitch angle phi deg');
subplot 223
plot(data(:,7)/60,data(:,6)*180/pi) ;
xlabel('Time in mts');
ylabel('Yaw angle phi deg');
subplot 224
plot(data(:,7)/60,data(:,8)) ;
xlabel('Time in mts');
ylabel('B offset from Body x');


end