Basic 4 plots and animation are working. Now onto rc_pilot testing!

Plotting/helper_functions/animate_simulink_data_single_vehicle.m

+function [] = animate_simulink_data_single_vehicle(simData, params)
+%animate Animates the multirotor
+    % Inputs:
+    %           simData
+    % Outputs:
+    %           Animation Plot
+
+    %% Some hardcoding before we smooth things out
+%     params.num_vehicles = 1;
+    %using_load = true;
+    
+    
+    %% Process Input into simpler names
+    T0 = simData.tout;
+    X0 = [simData.q1_X];
+    anim_FPS = params.anim_FPS;
+    
+    %% Interpolate to get requested display rate
+    deltaT = 1 / anim_FPS;
+    t = T0(1) : deltaT : T0(end);
+
+    load_ind = params.num_vehicles + 1;
+    
+    % Get Data for Each Vehicle
+    for quad = 1:params.num_vehicles
+        %% Obtain X,Y,Z from state 
+        X{quad} = interp1(T0,X0(:,(quad-1)*12 + 1),t);
+        Y{quad} = interp1(T0,X0(:,(quad-1)*12 + 2),t);
+        Z{quad} = interp1(T0,X0(:,(quad-1)*12 + 3),t);
+
+        %% Obtain Roll, Pitch, Yaw from state
+        Phi{quad} =   interp1(T0,X0(:,(quad-1)*12 + 7),t);
+        Theta{quad} = interp1(T0,X0(:,(quad-1)*12 + 8),t);
+        Psi{quad} =   interp1(T0,X0(:,(quad-1)*12 + 9),t);
+        
+        %% Process mounting points
+        r_Pi_P{quad} = params.multirotors{quad}.r_Pi_P;
+    end   
+   
+    %% Set Graph Limits
+    cubeSideLength = 1.5;
+    
+    % X Graph Limits
+    xUlim = cubeSideLength;
+    xLlim = -cubeSideLength;
+
+    % Y Graph Limtis
+    yUlim = cubeSideLength;
+    yLlim = -cubeSideLength;
+    
+    % Z Graph Limits
+    zUlim = cubeSideLength;
+    zLlim = -cubeSideLength;
+%     zUlim = -1;
+%     zLlim = -2;
+
+    %% Set up the graph
+    figure(100); clf;
+    axis([xLlim xUlim yLlim yUlim zLlim zUlim]);
+    ax = gca;
+    xlabel('X');
+    ylabel('Y');
+    zlabel('Z');
+    % Flip Z Axis and Y Axis
+    set(gca, 'Zdir', 'reverse');
+    set(gca, 'Ydir', 'reverse');
+    grid on;
+    movegui(gcf,'north');
+    
+    %% Draw Multirotor  
+    % Quadrotor in Cross Configuration
+    qw = 0.25; %Quad Width 
+    qh = 0.04;  %Quad Height  
+    quadFrontColor = [1 0 0];
+    quadBackColor = [0 0 0];
+    multiRotor_T = eye(4);
+    
+    for quad = 1 : params.num_vehicles
+        mr{quad} = drawQuad(ax,qw,qh,quadFrontColor,quadBackColor);
+        set(mr{quad},'Matrix',multiRotor_T);
+    end
+    
+    %% Change Viewpoint
+    view(-75,50);
+    
+    %% Animate
+    tic
+    for i=1:length(t)   
+        title(['t = ' num2str( t(i),'%2.1f') ' s']);     
+        
+        for quad = 1 : params.num_vehicles
+            % Update Multirotor
+            multiRotor_T = makehgtform('zrotate',Psi{quad}(i)) *...
+                           makehgtform('yrotate',Theta{quad}(i)) *...
+                           makehgtform('xrotate',Phi{quad}(i));
+            multiRotor_T(1:3,4) = [X{quad}(i); Y{quad}(i); Z{quad}(i)];
+            set(mr{quad},'Matrix',multiRotor_T);
+
+        end
+        
+        %Draws a point tracing the quad's path
+        plot3(X{1}(i), Y{1}(i), Z{1}(i), 'c.')
+            
+        % Display the Animation and sleep to maintain the display FPS
+        drawnow;
+        sleepTime = deltaT - toc;
+        if (sleepTime > 0)
+            java.lang.Thread.sleep((sleepTime)*1000);
+        end
+        
+        tic;
+%         %recording
+%         if strcmp(name, 'none') ~= 1 
+%             frame = getframe;
+%             writeVideo(writerObj,frame);
+%         end
+    end
+    
+    %close video object
+%     if strcmp(name, 'none') ~= 1 
+%         close(writerObj);
+%     end
+end
+

Plotting/helper_functions/plot_orientation.m

-function [] = plot_orientation(T,X,Xd,title_string,location)
+function [] = plot_orientation(T,X,Xd,title_string,location,lw,fs)
 
     figure();
 

Plotting/helper_functions/plot_orientation_rates.m

-function [] = plot_orientation_rates(T,X,Xd, simData)
+function [] = plot_orientation_rates(T,X,Xd,title_string,location,lw,fs)
 
     figure(); 
     
-        subplot(3,1,1);
-     plot(T,X(:,10));
+    subplot(3,1,1);
+    plot(T,X(:,10));
     hold on;
-    plot(T,simData.q1_phi_dot_des,'r--');
+    plot(T,Xd(:,10),'r--');
     hold off;
     ylabel('p (rad/s)');
     xlabel('Time (s)');
@@ -15,7 +15,7 @@ function [] = plot_orientation_rates(T,X,Xd, simData)
     subplot(3,1,2);
     plot(T,X(:,11));
     hold on;
-    plot(T,simData.q1_theta_dot_des,'r--');
+    plot(T,Xd(:,11),'r--');
     hold off;
     ylabel('q (rad/s)');
     xlabel('Time (s)');
@@ -24,11 +24,43 @@ function [] = plot_orientation_rates(T,X,Xd, simData)
     subplot(3,1,3);
     plot(T,X(:,12));
     hold on;
-    plot(T,simData.q1_psi_dot_des,'r--');
+    plot(T,Xd(:,12),'r--');
     hold off;
     ylabel('r (rad/s)');
     xlabel('Time (s)');
     legend('Actual','Desired');
 
+%     figure(); 
+%     
+%         subplot(3,1,1);
+%      plot(T,X(:,10));
+%     hold on;
+%     plot(T,simData.q1_phi_dot_des,'r--');
+%     hold off;
+%     ylabel('p (rad/s)');
+%     xlabel('Time (s)');
+%     title('Attitude Rates');
+%     legend('Actual','Desired');  
+%     
+%     subplot(3,1,2);
+%     plot(T,X(:,11));
+%     hold on;
+%     plot(T,simData.q1_theta_dot_des,'r--');
+%     hold off;
+%     ylabel('q (rad/s)');
+%     xlabel('Time (s)');
+%     legend('Actual','Desired');
+% 
+%     subplot(3,1,3);
+%     plot(T,X(:,12));
+%     hold on;
+%     plot(T,simData.q1_psi_dot_des,'r--');
+%     hold off;
+%     ylabel('r (rad/s)');
+%     xlabel('Time (s)');
+%     legend('Actual','Desired');
+
+    movegui(location);
+
 end
 

singleVehicleSim.m

@@ -7,9 +7,14 @@
 %% Add Necessary Folders to MATLAB Path (add everything)
 addpath(genpath(pwd));
 
-%% Set up Parameters
+%% Set up Parameters (comment/uncomment tests below)
+
+% Test 1: Square Trajectory
 traj_num = 2;
 params = setUpParameters_OneQuad(traj_num);
+
+
+
 params.num_vehicles = 1;
 
 %% Load Top Level Simulink Model (helps Simulink load ahead of time)
@@ -21,12 +26,15 @@ simData = sim('singlequadrotor','StartTime','0','StopTime',num2str(params.tFinal
 
 
 %% Plot
-% TODO plot quadrotor stuff, not load stuff
+lw = 1;
+fs = 12;
+
+plot_position(simData.tout, simData.q1_X, simData.q1_Xd,'Position','northwest',lw,fs);
+plot_velocity(simData.tout, simData.q1_X, simData.q1_Xd,'Velocity','northeast',lw,fs);
+plot_orientation(simData.tout, simData.q1_X, simData.q1_Xd, 'Orientation','southwest',lw,fs);
+plot_orientation_rates(simData.tout, simData.q1_X, simData.q1_Xd, 'Orientation Rate','southeast',lw,fs);
+
 
-plot_position(simData.tout, simData.q1_X, simData.q1_Xd);
-plot_velocity(simData.tout, simData.q1_X, simData.q1_Xd, simData);
-plot_orientation(simData.tout, simData.q1_X, simData.q1_Xd, simData);
-plot_orientation_rates(simData.tout, simData.q1_X, simData.q1_Xd, simData);
 %plot_FF(simData.tout, simData.q1_X, simData.q1_Xd, simData);
 %plot_FF_2(simData.tout, simData.q1_X, simData.q1_Xd, simData);
 %plot_acceleration(simData.tout, simData.q1_X, simData.q1_Xd, simData);
@@ -36,7 +44,7 @@ plot_orientation_rates(simData.tout, simData.q1_X, simData.q1_Xd, simData);
 % plot_force_torque_des(simData.tout, simData.q1_X, simData.q1_Xd, simData);
 
 %% Animate
-animate_simulink_data(simData, params, false);
+animate_simulink_data_single_vehicle(simData, params);