% Attitude Dynamics Equations, with magnetic field computation, magnetic torque.
% Darren Pais
% Quarter revolution - Aerospace Matrix matrix.
% Feb 12, 2007

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

function [data]=attitude(emuno) 
% Mass moments Kg m^2
Ix=.23689;
Iy=.28393;
Iz=.35307;

%Ix=.0015212;
%Iy=.001448;
%Iz=.001507;

% Magnetic Dipole Moment
mu=emuno*10^3*10^-3 ; % 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;
% Roll Pitch Yaw angles of BRF wrt MRF
phi_o=0; theta_o=0; psi_o=0;

% Simulation Parameters

% Simulations time in mins for a half orbit
t_mts=24;
% Convert to seconds
t_max=t_mts*60;
%---------------------

%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) ;
        
        %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
        
        %M= [cos(theta)*cos(psi)+sin(phi)*sin(theta)*sin(psi) cos(phi)*sin(psi) -sin(theta)*cos(psi)+sin(phi)*sin(psi)*cos(theta);
        %    -cos(theta)*sin(psi)+sin(phi)*sin(theta)*cos(psi)  cos(phi)*cos(psi)  sin(theta)*sin(psi)+sin(phi)*cos(psi)*cos(theta);
        %    cos(phi)*sin(theta)  -sin(phi)  cos(phi)*cos(theta) ]; %this is the psi->theta->phi transform
          
        M= [cos(theta)*cos(psi)  cos(theta)*sin(psi)  -sin(theta);  
            sin(phi)*sin(theta)*cos(psi)-cos(phi)*sin(psi)  sin(phi)*sin(theta)*sin(psi)+cos(phi)*cos(psi)  sin(phi)*cos(theta);
            cos(phi)*sin(theta)*cos(psi)+sin(phi)*sin(psi)  cos(phi)*sin(theta)*sin(psi)-sin(phi)*cos(psi)  cos(phi)*cos(theta)];

        
        transform=M*[cos(THETA) 0 -sin(THETA) ; 0 -1 0 ; -sin(THETA) 0 -cos(THETA)];
        
 
        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
    
        w_xyz=[wx wy wz] ;
        w_123=w_xyz+n*[0 1 0]*M;
        
        w1=w_123(1) ;
        w2=w_123(2) ;
        w3=w_123(3) ;
        
        %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) = w1+ tan(y(2))*(w2*sin(y(1))+w3*cos(y(1)) ) ; %phidot
                dy(2) = w2*cos(y(1)) - w3*sin(y(1))  ;                        %thetadot
                dy(3) = ( w2*sin(y(1))+w3*cos(y(1)) )/cos(y(2)) ; %psidot
            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 \theta deg');
subplot 223
plot(data(:,7)/60,data(:,6)*180/pi) ;
xlabel('Time in mts');
ylabel('Yaw angle \psi deg');
subplot 224
plot(data(:,7)/60,data(:,8)) ;
xlabel('Time in mts');
ylabel('B offset from Body x');


end