Merge remote-tracking branch 'origin/groundStation-dev-comp_graph-commands' into gui-dev

.gitmodules

 [submodule "groundStation/src/vrpn"]
 	path = groundStation/src/vrpn
 	url = https://github.com/vrpn/vrpn
+	ignore = dirty

ci-build.sh

@@ -3,4 +3,5 @@
 set -e
 
 # Quad Libraries and Boot image
-cd quad && make boot
+(cd quad && make)
+(cd groundStation && make) 

common/common.txt

-The common files are:
- - setcontrol.h
- - setcontrol.c

common/examples_c

-#include "packet.h"
-#include "setcontrol.h"
-#include "commands.h"
-
-#include <sys/types.h>
-
-
-/* This example is close to how the ground station will handle things. It
- * shouldn't be too hard to adapt it to work on the quad side.
- */
-
-static int next_msg_id = 0;
-
-int SendSetControl()
-{
-	struct metadata m;
-	struct controller_message cm;
-
-	cm.id = ROLL_ID;        /* Roll controller */
-	cm.value_id = KI_ID;    /* I coefficient */
-	cm.value = 42.00f;      /* An appropriate value */
-
-	uint8_t data[64];
-	m.msg_id = next_msg_id++;
-	/* Fills in the rest of the metadata automatically */
-	if (EncodeSetcontrol(&m, data, 64, &cm) < 0) {
-		return -1;
-	}
-
-	uint8_t packet[128];
-	ssize_t length;
-	if ((length = EncodePacket(packet, 128, &m, data)) < 0) {
-		return -1;
-	}
-
-	/* Now you can send the packet
-	 * sendThePacket(packet, length);
-	 */
-
-	return 0;
-}
-
-
-/* Process incoming packets */
-int receive_packet(uint8_t * packet, size_t length)
-{
-	struct metadata m;
-	uint8_t data[128];
-
-	/* Will fail if checksum is bad, etc */
-	if (DecodePacket(&m, data, 128, packet, length) < 0) {
-		return -1;
-	}
-
-	/* Let's pretend that the callbacks have signature
-	 * void cb(
-	 *     const struct metadata * m,
-	 *     const uint8_t * data);
-	 *
-	 * Call the callback.
-	 */
-	(*MessageTypes[m.msg_type].functionPtr)(&m, data);
-
-	return 0;
-}
-
-/* An example of a setcontrol callback. Arguments
- * aren't important, as long as we have access
- * to the packet bytes and the length
- */
-void cb_setcontrol(const struct metadata *m, const uint8_t *data)
-{
-	struct controller_message cm;
-	DecodeSetcontrol(&cm, m, data);
-
-	/* cm now has populated controller ID, value ID, and value */
-}

controls/DataAnalysisTool/Tool/DataAnalysis.m

@@ -8,14 +8,14 @@
 % window.
 %
 %fname = 'sampleLogFileWithMarker.txt';  
-fname = 'testies_v2.txt';  
+fname = '';  
 
 % PLOTTING SWITCHES - set them to 0 or 1 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 %
 plot            = 1;    % to choose plotting
 separatePlot    = 1;    % to generate separatePlots
 multiPlot       = 1;    % to generate multiPlot
-subPlot         = 0;    % to generate subPlots
+subPlot         = 1;    % to generate subPlots
 clearFigs       = 0;    % to close all the plots (needed only by the GUI)
 
 % DATA TO PLOT %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@@ -25,27 +25,28 @@ clearFigs       = 0;    % to close all the plots (needed only by the GUI)
 % seperatePlots. If this is empty but "plot" switch is 1, all the columns
 % will be plotted using seperatePlots.
 %
-separateData = {'PID_roll_vel'};
+separateData = {'X pos PID_Correction', 'Altitude PID_Correction'};
                                                 
 % MULTIDATA
 % write names of the data headers that you want to plot using multiPlots.
 % If this is empty but "plot" switch is 1, the data headers in separateData
 % will be plotted using multiPlots.
 %
-multiData = {}; 
+multiData = {'X pos PID_Correction', 'Altitude PID_Correction'}; 
 
 % SUBDATA
 % write names of the data headers that you want to plot using subPlots.
 % If this is empty but "plot" switch is 1, the data headers in multiData
 % will be plotted using subPlots.
 %
-subData = {};
+subData = {'X pos PID_Correction', 'Altitude PID_Correction'};
+
 
 % COLOR FOR PLOTTING %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 %
 color       = 'r';      % one character for color of the plotting line
 marker      = '';       % one character for the marker of the plotting line
-style       = ':';      % one character for the style of the plotting line
+style       = '-';      % one character for the style of the plotting line
 backgnd     = [1 1 1];  % rgb array for background color of the plot
 
 
@@ -94,10 +95,9 @@ expData = parse_log(params.file.pathName, params);
 % plot the data accoriding to the plotting parameters set
 %
 plot_data(expData, params.plotting);
-
 %% creating the main structure to be stored in the workspace %%%%%%%%%%%%%
 %
-main.params     = params;
-main.expData    = expData;
-clearvars -except main;
-save main main;
\ No newline at end of file
+% main.params     = params;
+% main.expData    = expData;
+% clearvars -except main;
+% save main main;
\ No newline at end of file

controls/DataAnalysisTool/Tool/parse_log.m

@@ -43,6 +43,7 @@ identifier = string(1);
 if (regexp(identifier,'%'))
     foundHeaders = 1;
     % this is a line of headers; extract headers:
+    string = strrep(string,' ', '_');
     headers = strsplit(string);
     headers{1} = strrep(headers{1},'%', '');
     numOfHeaders = length(headers);
@@ -126,17 +127,20 @@ if exist('params', 'var')
                         end
                     else
                         for i = 1:length(params.plotting.separateData)
+                            params.plotting.separateData{i} = strrep(params.plotting.separateData{i},' ', '_');
                             eval(['loggedData.' params.plotting.separateData{i} '.params.plot = 1;']);
                         end
                     end
                     
                 else
                     for i = 1:length(params.plotting.multiData)
+                        params.plotting.multiData{i} = strrep(params.plotting.multiData{i},' ', '_');
                         eval(['loggedData.' params.plotting.multiData{i} '.params.plot = 1;']);
                     end
                 end
             else
                 for i = 1:length(params.plotting.subData)
+                    params.plotting.subData{i} = strrep(params.plotting.subData{i},' ', '_');
                     eval(['loggedData.' params.plotting.subData{i} '.params.plot = 1;']);
                 end
             end
@@ -150,11 +154,13 @@ if exist('params', 'var')
                     end
                 else
                     for i = 1:length(params.plotting.separateData)
+                        params.plotting.separateData{i} = strrep(params.plotting.separateData{i},' ', '_');
                         eval(['loggedData.' params.plotting.separateData{i} '.params.plot = 1;']);
                     end
                 end
             else
                 for i = 1:length(params.plotting.multiData)
+                    params.plotting.multiData{i} = strrep(params.plotting.multiData{i},' ', '_');
                     eval(['loggedData.' params.plotting.multiData{i} '.params.plot = 1;']);
                 end
             end
@@ -167,6 +173,7 @@ if exist('params', 'var')
                 end
             else
                 for i = 1:length(params.plotting.separateData)
+                    params.plotting.separateData{i} = strrep(params.plotting.separateData{i},' ', '_');
                     eval(['loggedData.' params.plotting.separateData{i} '.params.plot = 1;']);
                 end
             end

controls/DataAnalysisTool/Tool/plot_data.m

@@ -6,6 +6,8 @@ function plot_data(expData, plotParams)
 % check plotting switch
 if (~plotParams.plot) 
     disp('No plotting');
+elseif(~plotParams.multiPlot && ~plotParams.subPlot && ~plotParams.separatePlot)
+    disp('No plotting, select a plotting style');
 else
     %% check separate plotting switch
     if(plotParams.separatePlot)
@@ -74,7 +76,7 @@ else
         if(isempty(plotParams.subData)) % if none mentioned, use headers mentioned for multi plotting
             if(isempty(plotParams.multiData)) % if no headers mentioned for multi plotting, use headers mentioned for separate plotting
                 if(isempty(plotParams.separateData)) % if no headers mentioned for separate plotting, use all of the headers
-                    headers = fieldNames(expData);
+                    headers = fieldnames(expData);
                 else
                     headers = plotParams.separateData;
                 end

controls/DataAnalysisTool/Tool/plot_multi.m

@@ -13,6 +13,7 @@ function plot_multi(expData, useMarker, varargin)
 numOfHeaders = 0;
 headers = {};
 for i = 1:length(varargin)
+    varargin{i} = strrep(varargin{i},' ', '_');
     if(~isPlotCharString(varargin{i}))
         numOfHeaders = numOfHeaders + 1;
         headers{numOfHeaders} = varargin{i};
@@ -144,13 +145,15 @@ for i = 1:numOfHeaders
     end
 end
 
-ylabel(yAxisLabel);
+ylabel(yAxisLabel,'Interpreter', 'none');
 
 % constructing statement for inserting the legend
-legendString = ['legend('''];
+legendString = ['legend('];
 for i = 1:numOfHeaders
     if(i == numOfHeaders)
-        legendString = strcat(legendString,headers{i},''',''Location'',''NorthWest'');');
+        legendString = strcat(legendString,headers{i},'''}',',''Interpreter'',''none'',''Location'',''NorthWest'');');
+    elseif(i == 1)
+        legendString = strcat(legendString,'{''',headers{i},''',''');
     else
         legendString = strcat(legendString,headers{i},''',''');
     end

controls/DataAnalysisTool/Tool/plot_separate.m

@@ -25,6 +25,7 @@ end
 % bulding the plot statement that will be executed
 for i = 1:length(varargin)
     
+    varargin{i} = strrep(varargin{i},' ', '_');
     dataHeader = varargin{i};
     
     % continue to next iteration if current argument is a plotting character string
@@ -49,19 +50,19 @@ for i = 1:length(varargin)
         plotCharString = buildPlotCharString(eval(['expData.' dataHeader '.params']));
         plotString = strcat(plotString,',''', plotCharString, ''');');
     end
-    
+
     % plotting data and making it look good
     figure;
     whitebg(gcf,eval(['expData.' dataHeader '.params.backgnd']));   % setting background
     eval(plotString);   % plotting
-    title(dataHeader);  % setting title
+    title(dataHeader, 'Interpreter', 'none');  % setting title
     xlabel(['Time (' time.unit ')']);   % setting the x-axis label
     if(eval(['isempty(expData.' dataHeader '.unit)']))
         yAxisLabel = dataHeader;
     else
         yAxisLabel = [dataHeader ' (' eval(['expData.' dataHeader '.unit']) ')'];
     end
-    ylabel(yAxisLabel); % setting y-axis label
+    ylabel(yAxisLabel,'Interpreter', 'none'); % setting y-axis label
     xlim([0, time.data(end)]); % setting x-axis limits
     grid ON;    % setting grid lines on the graph
     % adding markers

controls/DataAnalysisTool/Tool/plot_sub.m

@@ -41,6 +41,8 @@ for i = 1:length(varargin)
         continue;
     end
     
+    varargin{i} = strrep(varargin{i},' ', '_');
+    
     % keeping track of the number of headers
     currHeaderIndex = currHeaderIndex + 1;
     
@@ -75,14 +77,14 @@ for i = 1:length(varargin)
     
     %% plotting data and making it look pretty
     eval(plotString);
-    title(varargin{i});
+    title(varargin{i},'Interpreter','none');
     xlabel(['Time (' time.unit ')']);
     if(eval(['isempty(expData.' varargin{i} '.unit)']))
         yAxisLabel = varargin{i};
     else
         yAxisLabel = [varargin{i} ' (' eval(['expData.' varargin{i} '.unit']) ')'];
     end
-    ylabel(yAxisLabel);
+    ylabel(yAxisLabel,'Interpreter','none');
     xlim([0,time.data(end)]);
     grid ON;
     set(gca,'Color',eval(['expData.' varargin{i} '.params.backgnd']));

controls/model/.gitignore

+slprj/
+test_model_grt_rtw/
+test_model_sfun.mexw64
\ No newline at end of file

controls/model/c_controller.c

 #include "c_controller.h"
-#include "quad_files/control_algorithm.c"
-#include "quad_files/computation_graph.c"
+#include "control_algorithm.c"
+#include "computation_graph.c"
 double c_controller(int vrpn_id, double vrpn_ts, double set_x, double set_y, double set_z, double set_yaw,
         double cur_x, double cur_y, double cur_z,
         double cur_phi, double cur_theta, double cur_psi,

controls/model/c_controller.h

 #ifndef C_CONTROLLER_H
 #define C_CONTROLLER_H
-#include "quad_files/computation_graph.h"
-#include "quad_files/graph_blocks/node_pid.h"
-#include "quad_files/graph_blocks/node_bounds.h"
-#include "quad_files/graph_blocks/node_constant.h"
-#include "quad_files/graph_blocks/node_mixer.h"
-#include "quad_files/PID.h"
-#include "quad_files/control_algorithm.h"
+#include "computation_graph.h"
+#include "node_pid.h"
+#include "node_bounds.h"
+#include "node_constant.h"
+#include "node_mixer.h"
+#include "PID.h"
+#include "control_algorithm.h"
 
 double c_controller(int vrpn_id, double vrpn_ts, double set_x, double set_y, double set_z, double set_yaw,
         double cur_x, double cur_y, double cur_z,

controls/model/loggingAnalysis/logAnalysis.m

@@ -32,10 +32,10 @@ yaw_command_model_data = yaw_command_model.signals.values(indices_40ms);
 yaw_value_model_data = yaw_value_model.signals.values(indices_40ms);
 
 % Pull duty cycle commands from model
-PWM0_model = motorCommands.signals.values(indices_40ms, 1);
-PWM1_model = motorCommands.signals.values(indices_40ms, 2);
-PWM2_model = motorCommands.signals.values(indices_40ms, 3);
-PWM3_model = motorCommands.signals.values(indices_40ms, 4);
+PWM0_model = motorCommands.signals.values(indices_5ms, 1);
+PWM1_model = motorCommands.signals.values(indices_5ms, 2);
+PWM2_model = motorCommands.signals.values(indices_5ms, 3);
+PWM3_model = motorCommands.signals.values(indices_5ms, 4);
 
 %% Plot x control structure
 
@@ -165,7 +165,7 @@ legend('Log', 'Model', 'location', 'northwest');
 %% Plot PWM Commands
 figure(5); subplot(2, 2, 1);
 stairs(time, PWM0,'.-'); hold on; grid minor;
-stairs(time_model_40ms, PWM0_model, '.-'); hold off;
+stairs(time_model_5ms, PWM0_model, '.-'); hold off;
 title('PWM0 Value');
 xlabel('Time (s)');
 ylabel('PWM0 Command');
@@ -173,7 +173,7 @@ legend('Log', 'Model', 'location', 'northwest');
 
 subplot(2, 2, 2);
 stairs(time, PWM1,'.-'); hold on; grid minor;
-stairs(time_model_40ms, PWM1_model, '.-'); hold off;
+stairs(time_model_5ms, PWM1_model, '.-'); hold off;
 title('PWM1 Value');
 xlabel('Time (s)');
 ylabel('PWM1 Command');
@@ -181,7 +181,7 @@ legend('Log', 'Model', 'location', 'northwest');
 
 subplot(2, 2, 3);
 stairs(time, PWM2,'.-'); hold on; grid minor;
-stairs(time_model_40ms, PWM2_model, '.-'); hold off;
+stairs(time_model_5ms, PWM2_model, '.-'); hold off;
 title('PWM2 Value');
 xlabel('Time (s)');
 ylabel('PWM2 Command');
@@ -189,7 +189,7 @@ legend('Log', 'Model', 'location', 'northwest');
 
 subplot(2, 2, 4);
 stairs(time, PWM3,'.-'); hold on; grid minor;
-stairs(time_model_40ms, PWM3_model, '.-'); hold off;
+stairs(time_model_5ms, PWM3_model, '.-'); hold off;
 title('PWM3 Value');
 xlabel('Time (s)');
 ylabel('PWM3 Command');

controls/model/modelParameters.m

-temp = 0;
+temp = 1;
 % Log Analysis Toggle    
     logAnalysisToggle = 1;          % 1 for log analysis, 0 for normal operation
 
@@ -181,7 +181,7 @@ elseif logAnalysisToggle == 1 && temp == 1
     % window.
     %
     %fname = 'sampleLogFile.txt';  
-    fname = 'sampleLogFile.txt';
+    fname = '';
     fpath = '';
     
     if(isempty(fname))
@@ -208,8 +208,8 @@ elseif logAnalysisToggle == 1 && temp == 1
     y_error = timeseries(y_setpoint - y_position, time);
 
     % Determine z position error
-    z_setpoint = dataStruct.Z_Setpoint_Constant.data;
-    z_position = dataStruct.VRPN_Z_Constant.data;
+    z_setpoint = dataStruct.Alt_Setpoint_Constant.data;
+    z_position = dataStruct.VRPN_Alt_Constant.data;
     z_error = timeseries(z_setpoint - z_position, time);
 
     % Determine pitch error
@@ -243,10 +243,10 @@ elseif logAnalysisToggle == 1 && temp == 1
     yawrate_error = timeseries(yawrate_setpoint - yawrate_value, time);
 
     % Pull motor commands from log
-    x_command = dataStruct.Pitch_Rate_PID_Correction;
-    y_command = dataStruct.Roll_Rate_PID_Correction;
-    z_command = dataStruct.Altitude_PID_Correction;
-    yaw_command = dataStruct.Yaw_Rate_PID_Correction;
+    x_command = dataStruct.Pitch_Rate_PID_Correction.data;
+    y_command = dataStruct.Roll_Rate_PID_Correction.data;
+    z_command = dataStruct.Altitude_PID_Correction.data;
+    yaw_command = dataStruct.Yaw_Rate_PID_Correction.data;
     
     % Determine signal mix PWM values
     PWM0 = dataStruct.Signal_Mixer_PWM_0.data;

controls/model/parse_log_model.m

@@ -43,6 +43,7 @@ identifier = string(1);
 if (regexp(identifier,'%'))
     foundHeaders = 1;
     % this is a line of headers; extract headers:
+    string = strrep(string,' ', '_');
     headers = strsplit(string);
     headers{1} = strrep(headers{1},'%', '');
     numOfHeaders = length(headers);
@@ -106,6 +107,5 @@ for i = 1:numOfHeaders
         
 end
 
-
 end
 

controls/model/quad_files/.gitignore

-# Don't track the copied files here
-*.h
-*.c
\ No newline at end of file

controls/model/quad_files/copy_files.bat

-copy ..\..\..\quad\computation_graph\src\computation_graph.c computation_graph.c
-copy ..\..\..\quad\computation_graph\src\computation_graph.h computation_graph.h
-
-copy ..\..\..\quad\sw\modular_quad_pid\src\control_algorithm.h control_algorithm.h
-copy ..\..\..\quad\sw\modular_quad_pid\src\control_algorithm.c control_algorithm.c
-
-copy ..\..\..\quad\sw\modular_quad_pid\src\PID.h PID.h
-copy ..\..\..\quad\sw\modular_quad_pid\src\type_def.h type_def.h
-copy ..\..\..\quad\sw\modular_quad_pid\src\sensor_processing.h sensor_processing.h
-copy ..\..\..\quad\sw\modular_quad_pid\src\log_data.h log_data.h
-copy ..\..\..\quad\sw\modular_quad_pid\src\quadposition.h quadposition.h
-copy ..\..\..\quad\sw\modular_quad_pid\src\util.h util.h
-copy ..\..\..\quad\sw\modular_quad_pid\src\timer.h timer.h
-copy ..\..\..\groundStation\src\backend\commands.h commands.h
-copy ..\..\..\groundStation\src\backend\callbacks.h callbacks.h
-
-mkdir graph_blocks
-copy ..\..\..\quad\computation_graph\src\graph_blocks\node_constant.h graph_blocks\node_constant.h
-copy ..\..\..\quad\computation_graph\src\graph_blocks\node_constant.c graph_blocks\node_constant.c
-copy ..\..\..\quad\computation_graph\src\graph_blocks\node_add.h graph_blocks\node_add.h
-copy ..\..\..\quad\computation_graph\src\graph_blocks\node_add.c graph_blocks\node_add.c
-copy ..\..\..\quad\sw\modular_quad_pid\src\graph_blocks\node_mixer.c graph_blocks\node_mixer.c
-copy ..\..\..\quad\sw\modular_quad_pid\src\graph_blocks\node_mixer.h graph_blocks\node_mixer.h
-copy ..\..\..\quad\sw\modular_quad_pid\src\graph_blocks\node_pid.c graph_blocks\node_pid.c
-copy ..\..\..\quad\sw\modular_quad_pid\src\graph_blocks\node_pid.h graph_blocks\node_pid.h
-copy ..\..\..\quad\sw\modular_quad_pid\src\graph_blocks\node_bounds.c graph_blocks\node_bounds.c
-copy ..\..\..\quad\sw\modular_quad_pid\src\graph_blocks\node_bounds.h graph_blocks\node_bounds.h
\ No newline at end of file