diff --git a/Readme.md b/Readme.md
index c45a5060c8a0260f0ba1e5ec09634c5bebeef253..a7949d3dd34199adac9d7828a08255af73be7042 100644
--- a/Readme.md
+++ b/Readme.md
@@ -31,6 +31,8 @@ MicroCART has 3 areas of development:
 ## Documentation
 [How to demo the quadcopter](documentation/how_to_demo.md)  
 [How to charge the LiPo batteries](documentation/how_to_charge_lipo.md)  
+[Continuous Integration (automatic build process) FAQ](documentation/ci_faq.md)  
+[How to document things on Gitlab](documentation/how_to_document_things_on_gitlab.md)  
 
 # Stable Releases
 To browse stable releases from previous teams, view the [Tags](/../tags).
\ No newline at end of file
diff --git a/controls/model/logAnalysis.m b/controls/model/logAnalysis.m
index c4f3a1f365deb000fe399bc2a851bd16989a20ff..06511cabce98b8521383109b8eaa3852429f6f23 100644
--- a/controls/model/logAnalysis.m
+++ b/controls/model/logAnalysis.m
@@ -37,16 +37,20 @@ PWM1_model = motorCommands.signals.values(indices_5ms, 2);
 PWM2_model = motorCommands.signals.values(indices_5ms, 3);
 PWM3_model = motorCommands.signals.values(indices_5ms, 4);
 
-%Pull accelerometer readings from model
+% Pull accelerometer readings from model
 pitch_accel = angle_IMU_reading.signals.values(2, 1, indices_5ms);
 pitch_accel = reshape(pitch_accel, [length(pitch_accel) , 1] );
 roll_accel = angle_IMU_reading.signals.values(1, 1, indices_5ms);
 roll_accel = reshape(roll_accel, [length(roll_accel) , 1] );
 
+% Pull mahony filter data
+pitch_accel_mahony = mahony_reading.signals.values(indices_5ms, 2);
+roll_accel_mahony = mahony_reading.signals.values(indices_5ms, 1);
+
 %% Plot x control structure
 
 % Plot lateral controller output
-figure(1); subplot(2, 2, 1);
+figure(1); ax1 = subplot(2, 2, 1);
 stairs(time, pitch_setpoint, '.-'); hold on; grid minor;
 stairs(time_model_40ms, pitch_setpoint_model_data, '.-'); hold off;
 title('Lateral Controller Output');
@@ -55,7 +59,7 @@ ylabel('\theta (rad)');
 legend('Log', 'Model', 'location', 'northwest');
 
 % Plot pitch controller output
-subplot(2, 2, 2);
+ax2 = subplot(2, 2, 2);
 stairs(time, pitchrate_setpoint,'.-'); hold on; grid minor;
 stairs(time_model_5ms, pitchrate_setpoint_model_data, '.-'); hold off;
 title('Pitch Controller Output');
@@ -64,16 +68,16 @@ ylabel('d\theta/dt (rad/s)');
 legend('Log', 'Model', 'location', 'northwest');
 
 % Plot x controller command
-subplot(2, 2, 3);
+ax3 = subplot(2, 2, 3);
 stairs(time, x_command, '.-'); hold on; grid minor;
 stairs(time_model_5ms, x_command_model_data, '.-'); hold off;
-title('X Command');
+title('Pitch Rate Controller Output');
 xlabel('Time (s)');
 ylabel('Command');
 legend('Log', 'Model', 'location', 'northwest');
 
 % Plot x position
-subplot(2, 2, 4);
+ax4 = subplot(2, 2, 4);
 stairs(time, x_position, '.-'); hold on; grid minor;
 stairs(time_model_40ms, x_position_model_data, '.-'); hold off;
 title('X Position');
@@ -81,10 +85,12 @@ xlabel('Time (s)');
 ylabel('Position (m)');
 legend('Log', 'Model', 'location', 'northwest');
 
+linkaxes([ax1, ax2, ax3, ax4], 'x');
+
 %% Plot y control structure
 
 % Plot longitude controller output
-figure(2); subplot(2, 2, 1);
+figure(2); ax1 = subplot(2, 2, 1);
 stairs(time, roll_setpoint, '.-'); hold on; grid minor;
 stairs(time_model_40ms, roll_setpoint_model_data, '.-'); hold off;
 title('Longitude Controller Output ');
@@ -93,7 +99,7 @@ ylabel('\phi (rad)');
 legend('Log', 'Model', 'location', 'northwest');
 
 % Plot roll controller output
-subplot(2, 2, 2);
+ax2 = subplot(2, 2, 2);
 stairs(time, rollrate_setpoint,'.-'); hold on; grid minor;
 stairs(time_model_5ms, rollrate_setpoint_model_data, '.-'); hold off;
 title('Roll Controller Output');
@@ -102,7 +108,7 @@ ylabel('d\phi/dt (rad/s)');
 legend('Log', 'Model', 'location', 'northwest');
 
 % Plot y controller command
-subplot(2, 2, 3);
+ax3 = subplot(2, 2, 3);
 stairs(time, y_command, '.-'); hold on; grid minor;
 stairs(time_model_5ms, y_command_model_data, '.-'); hold off;
 title('Y Command');
@@ -111,7 +117,7 @@ ylabel('Command');
 legend('Log', 'Model', 'location', 'northwest');
  
 % Plot y position
-subplot(2, 2, 4);
+ax4 = subplot(2, 2, 4);
 stairs(time, y_position, '.-'); hold on; grid minor;
 stairs(time_model_40ms, y_position_model_data, '.-'); hold off;
 title('Y Position');
@@ -119,10 +125,12 @@ xlabel('Time (s)');
 ylabel('Position (m)');
 legend('Log', 'Model', 'location', 'northwest');
 
+linkaxes([ax1, ax2, ax3, ax4], 'x');
+
 %% Plot z control structure
 
 % Plot z controller command
-figure(3); subplot(2, 1, 1);
+figure(3); ax1 = subplot(2, 1, 1);
 stairs(time, z_command, '.-'); hold on; grid minor;
 stairs(time_model_5ms, z_command_model_data, '.-'); hold off;
 title('Z Command');
@@ -131,7 +139,7 @@ ylabel('Command');
 legend('Log', 'Model', 'location', 'northwest');
 
 % Plot z position
-subplot(2, 1, 2);
+ax2 = subplot(2, 1, 2);
 stairs(time, z_position, '.-'); hold on; grid minor;
 stairs(time_model_40ms, z_position_model_data, '.-'); hold off;
 title('Z Position');
@@ -139,10 +147,12 @@ xlabel('Time (s)');
 ylabel('Position (m)');
 legend('Log', 'Model', 'location', 'northwest');
 
+linkaxes([ax1, ax2], 'x');
+
 %% Plot yaw control structure
 
 % Plot yaw controller output
-figure(4); subplot(2, 2, 1);
+figure(4); ax1 = subplot(2, 2, 1);
 stairs(time, yawrate_setpoint,'.-'); hold on; grid minor;
 stairs(time_model_40ms, yawrate_setpoint_model_data, '.-'); hold off;
 title('Yaw Controller Output');
@@ -151,7 +161,7 @@ ylabel('d\psi/dt (rad/s)');
 legend('Log', 'Model', 'location', 'northwest');
 
 % Plot yaw controller command
-subplot(2, 2, 2);
+ax2 = subplot(2, 2, 2);
 stairs(time, yaw_command, '.-'); hold on; grid minor;
 stairs(time_model_5ms, yaw_command_model_data, '.-'); hold off;
 title('Yaw Command');
@@ -160,7 +170,7 @@ ylabel('Command');
 legend('Log', 'Model', 'location', 'northwest');
 
 % Plot yaw position
-subplot(2, 2, 3);
+ax3 = subplot(2, 2, 3);
 stairs(time, yaw_value, '.-'); hold on; grid minor;
 stairs(time_model_40ms, yaw_value_model_data, '.-'); hold off;
 title('Yaw Position');
@@ -168,10 +178,12 @@ xlabel('Time (s)');
 ylabel('Value (rad)');
 legend('Log', 'Model', 'location', 'northwest');
 
+linkaxes([ax1, ax2, ax3], 'x');
+
 %% Plot PWM Commands
 figure(5); ax1 = subplot(2, 2, 1);
 stairs(time, PWM0,'.-'); hold on; grid minor;
-stairs(time_model_5ms, PWM0_model, '.-'); hold off;
+%stairs(time_model_5ms, PWM0_model, '.-'); hold off;
 title('PWM0 Value');
 xlabel('Time (s)');
 ylabel('PWM0 Command');
@@ -179,7 +191,7 @@ legend('Log', 'Model', 'location', 'northwest');
 
 ax2 = subplot(2, 2, 2);
 stairs(time, PWM1,'.-'); hold on; grid minor;
-stairs(time_model_5ms, PWM1_model, '.-'); hold off;
+%stairs(time_model_5ms, PWM1_model, '.-'); hold off;
 title('PWM1 Value');
 xlabel('Time (s)');
 ylabel('PWM1 Command');
@@ -187,7 +199,7 @@ legend('Log', 'Model', 'location', 'northwest');
 
 ax3 = subplot(2, 2, 3);
 stairs(time, PWM2,'.-'); hold on; grid minor;
-stairs(time_model_5ms, PWM2_model, '.-'); hold off;
+%stairs(time_model_5ms, PWM2_model, '.-'); hold off;
 title('PWM2 Value');
 xlabel('Time (s)');
 ylabel('PWM2 Command');
@@ -195,7 +207,7 @@ legend('Log', 'Model', 'location', 'northwest');
     
 ax4 = subplot(2, 2, 4);
 stairs(time, PWM3,'.-'); hold on; grid minor;
-stairs(time_model_5ms, PWM3_model, '.-'); hold off;
+%stairs(time_model_5ms, PWM3_model, '.-'); hold off;
 title('PWM3 Value');
 xlabel('Time (s)');
 ylabel('PWM3 Command');
@@ -205,29 +217,34 @@ linkaxes([ax1, ax2, ax3, ax4], 'xy');
 
 %% Plot output of complimentary filter
 
-figure(8); subplot(2, 1, 1);
+figure(8); ax1 = subplot(2, 1, 1);
 stairs(time, pitch_measured_VRPN * (180/pi), '.-'); hold on; grid minor;
-stairs(time, pitch_measured_IMU * (180/pi), '.-');
-stairs(time_model_5ms, pitch_accel * (180/pi), '.-'); hold off;
+stairs(time, (pitch_measured_IMU + 0.02) * (180/pi), '.-');
+%stairs(time_model_5ms, pitch_accel * (180/pi), '.-');
+stairs(time_model_5ms, pitch_accel_mahony * (180/pi), '.-'); hold off;
 title('Pitch Complementary Filter Output');
 xlabel('Time (s)');
 ylabel('Pitch Angle (degrees)');
 legend('VRPN','IMU', 'Model', 'location', 'northwest');
 
-subplot(2, 1, 2);
+ax2 = subplot(2, 1, 2);
 stairs(time, roll_measured_VRPN * (180/pi), '.-'); hold on; grid minor;
 stairs(time, roll_measured_IMU * (180/pi), '.-');
-stairs(time_model_5ms, roll_accel * (180/pi), '.-'); hold off;
+%stairs(time_model_5ms, roll_accel * (180/pi), '.-');
+stairs(time_model_5ms, roll_accel_mahony * (180/pi), '.-'); hold off;
 title('Roll Complementary Filter Output');
 xlabel('Time (s)');
 ylabel('Roll Angle (degrees)');
 legend('VRPN','IMU', 'Model', 'location', 'northwest');
 
+linkaxes([ax1, ax2], 'x');
+
 %% Plot VRPN Position
 
 figure(9); ax1 = subplot(3, 1, 1);
 stairs(time, x_position, '.-'); hold on; grid minor;
-stairs(time_model_40ms, x_position_model_data, '.-');
+%stairs(time_model_40ms, x_position_model_data, '.-');
+stairs(time, x_setpoint, '.-');
 title('X position');
 xlabel('Time (s)');
 ylabel('X position');
@@ -235,7 +252,9 @@ legend('X position', 'X position model', 'X setpoint');
 
 ax2 = subplot(3, 1, 2);
 stairs(time, y_position, '.-'); hold on; grid minor;
-stairs(time_model_40ms, y_position_model_data, '.-');
+%stairs(time_model_40ms, y_position_model_data, '.-');
+stairs(time, x_setpoint, '.-');
+
 title('Y position');
 xlabel('Time (s)');
 ylabel('Y position');
@@ -250,3 +269,45 @@ ylabel('Z position');
 legend('Z position', 'Z position model', 'Y setpoint');
 
 linkaxes([ax1, ax2, ax3], 'x');
+
+%% Plot Gyro Data
+figure(10); ax1 = subplot(3, 1, 1);
+stairs(time, raw_gyro_data_x, '.-'); hold on; grid minor;
+title('gyro x');
+xlabel('Time (s)');
+ylabel('p (rad/s)');
+
+ax2 = subplot(3, 1, 2);
+stairs(time, raw_gyro_data_y, '.-'); hold on; grid minor;
+title('gyro y');
+xlabel('Time (s)');
+ylabel('q (rad/s)');
+
+ax3 = subplot(3, 1, 3);
+stairs(time, raw_gyro_data_z, '.-'); hold on; grid minor;
+title('gyro z');
+xlabel('Time (s)');
+ylabel('r (rad/s)');
+
+linkaxes([ax1, ax2, ax3], 'x');
+
+%% Plot Accel Data
+figure(11); ax1 = subplot(3, 1, 1);
+stairs(time, raw_accel_data_x, '.-'); hold on; grid minor;
+title('accel x');
+xlabel('Time (s)');
+ylabel('accel x (g)');
+
+ax2 = subplot(3, 1, 2);
+stairs(time, raw_gyro_data_y, '.-'); hold on; grid minor;
+title('accel y');
+xlabel('Time (s)');
+ylabel('accel y (g)');
+
+ax3 = subplot(3, 1, 3);
+stairs(time, raw_gyro_data_z, '.-'); hold on; grid minor;
+title('accel z');
+xlabel('Time (s)');
+ylabel('accel (z)');
+
+linkaxes([ax1, ax2, ax3], 'x');
\ No newline at end of file
diff --git a/controls/model/modelParameters.m b/controls/model/modelParameters.m
index 20dad9510c22a96dec30005120ada0f9c7aebd34..3ad190cf93f61fe0b5149b9e8f70b9ab297e75a4 100644
--- a/controls/model/modelParameters.m
+++ b/controls/model/modelParameters.m
@@ -18,6 +18,7 @@
     rhx = 0.16;                     % X-axis distance from center of mass to a rotor hub
     rhy = 0.16;                     % Y-axis distance from center of mass to a rotor hub
     rhz = 0.03;                     % Z-axis distance from center of mass to a rotor hub
+    r_oc = [0; 0; 0];               % Vector from origin to center of mass
     Rm = 0.2308;                    % Motor resistance
     Kq = 96.3422;                   % Motor torque constant
     Kv = 96.3422;                   % Motor back emf constant
@@ -31,6 +32,8 @@
     x_controlled_o = 0;             % Equilibrium lateral controller output
     y_controlled_o = 0;             % Equilibrium longitudinal controller output
     yaw_controlled_o = 0;           % Equilibrium yaw controller output
+    Kp_mahony = 0.2;                % Proportional term for mahony filter
+    Ki_mahony = 0.001;              % Integral term for mahony filter
     
     % Determine Initial Conditions
     
@@ -114,7 +117,7 @@ if logAnalysisToggle == 1
 
     % Determine pitch error
     pitch_setpoint = dataStruct.X_pos_PID_Correction.data;
-    pitch_value = dataStruct.Pitch_Constant.data;
+    pitch_value = dataStruct.Pitch_trim_add_Sum.data;
     pitch_error = timeseries(pitch_setpoint - pitch_value, time);
 
     % Determine roll error
@@ -175,10 +178,10 @@ if logAnalysisToggle == 1
     
     % Create time series object for z command
     throttle_command = timeseries(dataStruct.RC_Throttle_Constant.data, time);
-    z_command = dataStruct.RC_Throttle_Constant.data;
+    %z_command = dataStruct.RC_Throttle_Constant.data;
     
     % Pull the measurements from the complimentary filter
-    pitch_measured_IMU = dataStruct.Pitch_Constant.data;
+    pitch_measured_IMU = dataStruct.Pitch_trim_add_Sum.data;
     roll_measured_IMU = dataStruct.Roll_Constant.data;
     IMU_angle_arr = ...
         [roll_measured_IMU, pitch_measured_IMU];
@@ -188,37 +191,37 @@ if logAnalysisToggle == 1
     pitch_measured_VRPN = dataStruct.VRPN_Pitch_Constant.data;
     roll_measured_VRPN = dataStruct.VRPN_Roll_Constant.data;
     
-    % Set height_controlled_o to initial throttle command
-    height_controlled_o = dataStruct.RC_Throttle_Constant.data(1);
-    
-    % Set rotor speed initial conditions to there calculated values based on
-    % initial throttle control
-    omega0_o = (sqrt(((height_controlled_o ...
-    - Pmin)/(Pmax - Pmin)* Vb - Rm * If) * 4 * ...
-    Rm * Kv^2 * Kq * Kd + 1) - 1) / (2 * Rm *  ...
-    Kv * Kq * Kd);
-    omega1_o = omega0_o;
-    omega2_o = omega0_o;
-    omega3_o = omega0_o;
-    
-    % Set initial positions
-    x_o = x_position(1);
-    y_o = y_position(1);
-    z_o = z_position(1);
-    
-    % Set initial velocities
-    x_vel_o = (x_position(1) - x_position(2))/(time(1) - time(2));
-    y_vel_o = (y_position(1) - y_position(2))/(time(1) - time(2));
-    z_vel_o = (z_position(1) - z_position(2))/(time(1) - time(2));
-    
-    % Equilibrium angles
-    roll_o = roll_measured_VRPN(1);
-    pitch_o = pitch_measured_VRPN(1);
-    yaw_o = 0;
-    
-    % Equilibrium angular rates
-    rollrate_o = (roll_measured_VRPN(1) - roll_measured_VRPN(2))/(time(1) - time(2));
-    pitchrate_o = (pitch_measured_VRPN(1) - pitch_measured_VRPN(2))/(time(1) - time(2));
-    yawrate_o = 0;
+%     % Set height_controlled_o to initial throttle command
+%     height_controlled_o = dataStruct.RC_Throttle_Constant.data(1);
+%     
+%     % Set rotor speed initial conditions to there calculated values based on
+%     % initial throttle control
+%     omega0_o = (sqrt(((height_controlled_o ...
+%     - Pmin)/(Pmax - Pmin)* Vb - Rm * If) * 4 * ...
+%     Rm * Kv^2 * Kq * Kd + 1) - 1) / (2 * Rm *  ...
+%     Kv * Kq * Kd);
+%     omega1_o = omega0_o;
+%     omega2_o = omega0_o;
+%     omega3_o = omega0_o;
+%     
+%     % Set initial positions
+%     x_o = x_position(1);
+%     y_o = y_position(1);
+%     z_o = z_position(1);
+%     
+%     % Set initial velocities
+%     x_vel_o = (x_position(1) - x_position(2))/(time(1) - time(2));
+%     y_vel_o = (y_position(1) - y_position(2))/(time(1) - time(2));
+%     z_vel_o = (z_position(1) - z_position(2))/(time(1) - time(2));
+%     
+%     % Equilibrium angles
+%     roll_o = roll_measured_VRPN(1);
+%     pitch_o = pitch_measured_VRPN(1);
+%     yaw_o = 0;
+%     
+%     % Equilibrium angular rates
+%     rollrate_o = (roll_measured_VRPN(1) - roll_measured_VRPN(2))/(time(1) - time(2));
+%     pitchrate_o = (pitch_measured_VRPN(1) - pitch_measured_VRPN(2))/(time(1) - time(2));
+%     yawrate_o = 0;
     
 end
\ No newline at end of file
diff --git a/controls/model/test_model.slx b/controls/model/test_model.slx
index 56971125dc1d50445e083b3e0f8b6d3187957d3b..62dc0a21547c1eff7b1e558e74d80030e2955325 100644
Binary files a/controls/model/test_model.slx and b/controls/model/test_model.slx differ
diff --git a/documentation/how_to_document_things_on_gitlab.md b/documentation/how_to_document_things_on_gitlab.md
new file mode 100644
index 0000000000000000000000000000000000000000..8bafcf718f00ad0fdfb5e91ca2c70041ae21fed9
--- /dev/null
+++ b/documentation/how_to_document_things_on_gitlab.md
@@ -0,0 +1,34 @@
+# Documentation on Gitlab
+We can use Gitlab to host our documentation by exploiting the fact that Gitlab
+automatically renders Markdown (.md) files as HTML in the browser, making it
+easy to read and show images.
+
+# Organization
+We will be using the top-level `README.md` as our main page for documentation.
+You might notice that this page is automatically rendered on our [main Gitlab
+project page](/../).
+
+Ideally, all documentation should be reachable from links on this main page.
+For documentation that is whole-project encompassing, those links should go
+on the main page itself. For documentation that is sub-topic specific (quad,
+ground station, controls), add an appropriate link on the sub topic's README.md
+page. See the Quad section for an example.
+
+Ideally, all Markdown (.md) files should go inside some `doc` or `documentation`
+folder (with exception to the READMEs).
+
+# Editing
+If you are familiar with Markdown and Git, you can just edit the documentation
+from within your normal Git workflow.
+
+However, Gitlab also makes is relatively easy to edit Markdown pages for
+documentation right in the browser. To edit a Markdown page, select "Edit" at
+the top right. This will bring up an editor that you can use to make changes.
+To preview the final project, use the "Preview" tab at the top left. To achieve
+desired formatting, you will need to use the Markdown syntax - see the [Gitlab
+Markdown reference](https://docs.gitlab.com/ee/user/markdown.html).
+
+To create a new file within Gitlab, go to the "Repository" tab and then "Files"
+tab. As you navigate through our files, you will see a "+" (plus sign) button.
+From here, you can create a new Markdown file, upload a new file, or create a
+folder (say if you need to create some `doc` folder).
diff --git a/documentation/images/.gitkeep b/documentation/images/.gitkeep
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/groundStation/gui/MicroCART/MicroCART.pro b/groundStation/gui/MicroCART/MicroCART.pro
index 54734fd0f176853baa306db8e55f2929797eea87..e650809dadcff4eea45dd1649c1c3072d45ffecb 100644
--- a/groundStation/gui/MicroCART/MicroCART.pro
+++ b/groundStation/gui/MicroCART/MicroCART.pro
@@ -26,3 +26,26 @@ HEADERS  += mainwindow.h \
     controlworker.h
 
 FORMS    += mainwindow.ui
+
+INCLUDEPATH += $$PWD/../../../quad/inc
+DEPENDPATH += $$PWD/../../../quad/inc
+
+win32:CONFIG(release, debug|release): LIBS += -L$$PWD/../../../quad/lib/release/ -lgraph_blocks
+else:win32:CONFIG(debug, debug|release): LIBS += -L$$PWD/../../../quad/lib/debug/ -lgraph_blocks
+else:unix: LIBS += -L$$PWD/../../../quad/lib/ -lgraph_blocks
+
+win32-g++:CONFIG(release, debug|release): PRE_TARGETDEPS += $$PWD/../../../quad/lib/release/libgraph_blocks.a
+else:win32-g++:CONFIG(debug, debug|release): PRE_TARGETDEPS += $$PWD/../../../quad/lib/debug/libgraph_blocks.a
+else:win32:!win32-g++:CONFIG(release, debug|release): PRE_TARGETDEPS += $$PWD/../../../quad/lib/release/graph_blocks.lib
+else:win32:!win32-g++:CONFIG(debug, debug|release): PRE_TARGETDEPS += $$PWD/../../../quad/lib/debug/graph_blocks.lib
+else:unix: PRE_TARGETDEPS += $$PWD/../../../quad/lib/libgraph_blocks.a
+
+win32:CONFIG(release, debug|release): LIBS += -L$$PWD/../../../quad/lib/release/ -lcomputation_graph
+else:win32:CONFIG(debug, debug|release): LIBS += -L$$PWD/../../../quad/lib/debug/ -lcomputation_graph
+else:unix: LIBS += -L$$PWD/../../../quad/lib/ -lcomputation_graph
+
+win32-g++:CONFIG(release, debug|release): PRE_TARGETDEPS += $$PWD/../../../quad/lib/release/libcomputation_graph.a
+else:win32-g++:CONFIG(debug, debug|release): PRE_TARGETDEPS += $$PWD/../../../quad/lib/debug/libcomputation_graph.a
+else:win32:!win32-g++:CONFIG(release, debug|release): PRE_TARGETDEPS += $$PWD/../../../quad/lib/release/computation_graph.lib
+else:win32:!win32-g++:CONFIG(debug, debug|release): PRE_TARGETDEPS += $$PWD/../../../quad/lib/debug/computation_graph.lib
+else:unix: PRE_TARGETDEPS += $$PWD/../../../quad/lib/libcomputation_graph.a
diff --git a/groundStation/gui/MicroCART/MicroCART.pro.user b/groundStation/gui/MicroCART/MicroCART.pro.user
index 03f704131d619d5228554eb5694df9f8558c3461..1dbd232a082163317c643a0b53fd6b2f093385cd 100644
--- a/groundStation/gui/MicroCART/MicroCART.pro.user
+++ b/groundStation/gui/MicroCART/MicroCART.pro.user
@@ -1,10 +1,10 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <!DOCTYPE QtCreatorProject>
-<!-- Written by QtCreator 3.2.2, 2017-03-24T14:09:55. -->
+<!-- Written by QtCreator 4.0.1, 2017-04-11T14:10:46. -->
 <qtcreator>
  <data>
   <variable>EnvironmentId</variable>
-  <value type="QByteArray">{9578df81-eac2-4831-8e1a-80abf90f9c0f}</value>
+  <value type="QByteArray">{ec588c71-c0cc-43f4-8233-a07fa24de8ad}</value>
  </data>
  <data>
   <variable>ProjectExplorer.Project.ActiveTarget</variable>
@@ -40,6 +40,7 @@
    <value type="bool" key="EditorConfiguration.ScrollWheelZooming">true</value>
    <value type="bool" key="EditorConfiguration.ShowMargin">false</value>
    <value type="int" key="EditorConfiguration.SmartBackspaceBehavior">0</value>
+   <value type="bool" key="EditorConfiguration.SmartSelectionChanging">true</value>
    <value type="bool" key="EditorConfiguration.SpacesForTabs">true</value>
    <value type="int" key="EditorConfiguration.TabKeyBehavior">0</value>
    <value type="int" key="EditorConfiguration.TabSize">8</value>
@@ -60,12 +61,12 @@
   <valuemap type="QVariantMap">
    <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Desktop</value>
    <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Desktop</value>
-   <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">{199b6d60-acb7-4eec-805c-8e68ddaba3f6}</value>
+   <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">{c6f8ca21-0eb9-4188-b2e8-fae8725afa1b}</value>
    <value type="int" key="ProjectExplorer.Target.ActiveBuildConfiguration">0</value>
    <value type="int" key="ProjectExplorer.Target.ActiveDeployConfiguration">0</value>
    <value type="int" key="ProjectExplorer.Target.ActiveRunConfiguration">0</value>
    <valuemap type="QVariantMap" key="ProjectExplorer.Target.BuildConfiguration.0">
-    <value type="QString" key="ProjectExplorer.BuildConfiguration.BuildDirectory">/home/jake/MicroCART_17-18/groundStation/gui/build-MicroCART-Desktop-Debug</value>
+    <value type="QString" key="ProjectExplorer.BuildConfiguration.BuildDirectory">/home/jake/Microcart_17-18/groundStation/gui/build-MicroCART-Desktop-Debug</value>
     <valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.0">
      <valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.0">
       <value type="bool" key="ProjectExplorer.BuildStep.Enabled">true</value>
@@ -73,9 +74,10 @@
       <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
       <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">QtProjectManager.QMakeBuildStep</value>
       <value type="bool" key="QtProjectManager.QMakeBuildStep.LinkQmlDebuggingLibrary">false</value>
-      <value type="bool" key="QtProjectManager.QMakeBuildStep.LinkQmlDebuggingLibraryAuto">true</value>
       <value type="QString" key="QtProjectManager.QMakeBuildStep.QMakeArguments"></value>
       <value type="bool" key="QtProjectManager.QMakeBuildStep.QMakeForced">false</value>
+      <value type="bool" key="QtProjectManager.QMakeBuildStep.SeparateDebugInfo">false</value>
+      <value type="bool" key="QtProjectManager.QMakeBuildStep.UseQtQuickCompiler">false</value>
      </valuemap>
      <valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.1">
       <value type="bool" key="ProjectExplorer.BuildStep.Enabled">true</value>
@@ -124,7 +126,7 @@
     <value type="bool" key="Qt4ProjectManager.Qt4BuildConfiguration.UseShadowBuild">true</value>
    </valuemap>
    <valuemap type="QVariantMap" key="ProjectExplorer.Target.BuildConfiguration.1">
-    <value type="QString" key="ProjectExplorer.BuildConfiguration.BuildDirectory">/home/jake/MicroCART_17-18/groundStation/gui/build-MicroCART-Desktop-Release</value>
+    <value type="QString" key="ProjectExplorer.BuildConfiguration.BuildDirectory">/home/jake/Microcart_17-18/groundStation/gui/build-MicroCART-Desktop-Release</value>
     <valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.0">
      <valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.0">
       <value type="bool" key="ProjectExplorer.BuildStep.Enabled">true</value>
@@ -132,9 +134,10 @@
       <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
       <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">QtProjectManager.QMakeBuildStep</value>
       <value type="bool" key="QtProjectManager.QMakeBuildStep.LinkQmlDebuggingLibrary">false</value>
-      <value type="bool" key="QtProjectManager.QMakeBuildStep.LinkQmlDebuggingLibraryAuto">true</value>
       <value type="QString" key="QtProjectManager.QMakeBuildStep.QMakeArguments"></value>
       <value type="bool" key="QtProjectManager.QMakeBuildStep.QMakeForced">false</value>
+      <value type="bool" key="QtProjectManager.QMakeBuildStep.SeparateDebugInfo">false</value>
+      <value type="bool" key="QtProjectManager.QMakeBuildStep.UseQtQuickCompiler">false</value>
      </valuemap>
      <valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.1">
       <value type="bool" key="ProjectExplorer.BuildStep.Enabled">true</value>
@@ -198,6 +201,11 @@
    <value type="int" key="ProjectExplorer.Target.DeployConfigurationCount">1</value>
    <valuemap type="QVariantMap" key="ProjectExplorer.Target.PluginSettings"/>
    <valuemap type="QVariantMap" key="ProjectExplorer.Target.RunConfiguration.0">
+    <value type="bool" key="Analyzer.QmlProfiler.AggregateTraces">false</value>
+    <value type="bool" key="Analyzer.QmlProfiler.FlushEnabled">false</value>
+    <value type="uint" key="Analyzer.QmlProfiler.FlushInterval">1000</value>
+    <value type="QString" key="Analyzer.QmlProfiler.LastTraceFile"></value>
+    <value type="bool" key="Analyzer.QmlProfiler.Settings.UseGlobalSettings">true</value>
     <valuelist type="QVariantList" key="Analyzer.Valgrind.AddedSuppressionFiles"/>
     <value type="bool" key="Analyzer.Valgrind.Callgrind.CollectBusEvents">false</value>
     <value type="bool" key="Analyzer.Valgrind.Callgrind.CollectSystime">false</value>
@@ -236,12 +244,13 @@
     <valuelist type="QVariantList" key="PE.EnvironmentAspect.Changes"/>
     <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">MicroCART</value>
     <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
-    <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">Qt4ProjectManager.Qt4RunConfiguration:/home/jake/MicroCART_17-18/groundStation/gui/MicroCART/MicroCART.pro</value>
+    <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">Qt4ProjectManager.Qt4RunConfiguration:/home/jake/Microcart_17-18/groundStation/gui/MicroCART/MicroCART.pro</value>
+    <value type="bool" key="QmakeProjectManager.QmakeRunConfiguration.UseLibrarySearchPath">true</value>
     <value type="QString" key="Qt4ProjectManager.Qt4RunConfiguration.CommandLineArguments"></value>
     <value type="QString" key="Qt4ProjectManager.Qt4RunConfiguration.ProFile">MicroCART.pro</value>
     <value type="bool" key="Qt4ProjectManager.Qt4RunConfiguration.UseDyldImageSuffix">false</value>
-    <value type="bool" key="Qt4ProjectManager.Qt4RunConfiguration.UseTerminal">false</value>
     <value type="QString" key="Qt4ProjectManager.Qt4RunConfiguration.UserWorkingDirectory"></value>
+    <value type="QString" key="Qt4ProjectManager.Qt4RunConfiguration.UserWorkingDirectory.default">/home/jake/Microcart_17-18/groundStation/gui/build-MicroCART-Desktop-Debug</value>
     <value type="uint" key="RunConfiguration.QmlDebugServerPort">3768</value>
     <value type="bool" key="RunConfiguration.UseCppDebugger">false</value>
     <value type="bool" key="RunConfiguration.UseCppDebuggerAuto">true</value>
@@ -258,10 +267,10 @@
  </data>
  <data>
   <variable>ProjectExplorer.Project.Updater.FileVersion</variable>
-  <value type="int">16</value>
+  <value type="int">18</value>
  </data>
  <data>
   <variable>Version</variable>
-  <value type="int">16</value>
+  <value type="int">18</value>
  </data>
 </qtcreator>
diff --git a/groundStation/gui/MicroCART/controlworker.cpp b/groundStation/gui/MicroCART/controlworker.cpp
index 5c526c77f87b2e51031bef3eb667d55f06746457..3201d0ef341e73bde79b7e7ccc490e1ef7a5b4ce 100644
--- a/groundStation/gui/MicroCART/controlworker.cpp
+++ b/groundStation/gui/MicroCART/controlworker.cpp
@@ -1,6 +1,130 @@
 #include "controlworker.h"
+#include "frontend_nodes.h"
+#include "frontend_param.h"
+#include "graph_blocks.h"
 
-controlworker::controlworker(QObject *parent) :
-    QObject(parent)
+ControlWorker::ControlWorker(QObject *parent) :
+    QObject(parent), conn(NULL)
 {
+
+}
+
+ControlWorker::~ControlWorker()
+{
+    disconnectBackend();
+}
+
+void ControlWorker::connectBackend()
+{
+    conn = ucart_backendConnect();
+}
+
+void ControlWorker::disconnectBackend()
+{
+    if (conn) {
+         ucart_backendDisconnect(conn);
+         conn = NULL;
+    }
+}
+
+void ControlWorker::getNodes()
+{
+    if (conn) {
+        frontend_node_data * nd = NULL;
+        size_t num_nodes = 0;
+        frontend_getnodes(conn, &nd, &num_nodes);
+        QStringList nodes;
+        QStringList const_block_nodes;
+        for (size_t i = 0; i < num_nodes; i++) {
+            nodes.append(QString(nd[i].name));
+            if (nd[i].type == BLOCK_CONSTANT) {
+                const_block_nodes.append(nd[i].name);
+            }
+        }
+        frontend_free_node_data(nd, num_nodes);
+        emit(gotNodes(nodes));
+        emit(gotConstantBlocks(const_block_nodes));
+    }
+}
+
+void ControlWorker::getParams(QString node)
+{
+    if (conn) {
+        frontend_node_data * nd = NULL;
+        size_t num_nodes = 0;
+        frontend_getnodes(conn, &nd, &num_nodes);
+
+        for (size_t i = 0; i < num_nodes; i++) {
+            if (QString(nd[i].name) == node) {
+                QStringList params;
+
+                /* Get type definition */
+                const struct graph_node_type * type = blockDefs[nd[i].type];
+                /* Iterate through param names to append, and then emit signal */
+                for (ssize_t j = 0; j < type->n_params; j++) {
+                    params.append(QString(type->param_names[j]));
+                }
+                emit(gotParams(params));
+            }
+        }
+        frontend_free_node_data(nd, num_nodes);
+    }
+}
+
+
+
+void ControlWorker::getParamValue(QString node, QString param)
+{
+    if (conn) {
+        frontend_node_data * nd = NULL;
+        size_t num_nodes = 0;
+        frontend_getnodes(conn, &nd, &num_nodes);
+
+        for (size_t i = 0; i < num_nodes; i++) {
+            if (QString(nd[i].name) == node) {
+                frontend_param_data pd;
+                pd.block = nd[i].block;
+
+                /* Get the type definition, then iterate through to find the param */
+                const struct graph_node_type * type = blockDefs[nd[i].type];
+                for (ssize_t j = 0; j < type->n_params; j++) {
+                    /* Found param */
+                    if (QString(type->param_names[j]) == param) {
+                        /* Set pd.param and finish the job */
+                        pd.param = j;
+                        frontend_getparam(conn, &pd);
+                        emit(gotParamValue(node, param, pd.value));
+                    }
+                }
+            }
+        }
+        frontend_free_node_data(nd, num_nodes);
+    }
+}
+
+void ControlWorker::setParamValue(QString node, QString param, float value)
+{
+    if (conn) {
+        frontend_node_data * nd = NULL;
+        size_t num_nodes = 0;
+        frontend_getnodes(conn, &nd, &num_nodes);
+
+        for (size_t i = 0; i < num_nodes; i++) {
+            if (QString(nd[i].name) == node) {
+                frontend_param_data pd;
+                pd.block = nd[i].block;
+
+                const struct graph_node_type * type = blockDefs[nd[i].type];
+                for (ssize_t j = 0; j < type->n_params; j++) {
+                    if (QString(type->param_names[j]) == param) {
+                        pd.param = j;
+                        pd.value = value;
+                        frontend_setparam(conn, &pd);
+                        emit(paramSet(node, param));
+                    }
+                }
+            }
+        }
+        frontend_free_node_data(nd, num_nodes);
+    }
 }
diff --git a/groundStation/gui/MicroCART/controlworker.h b/groundStation/gui/MicroCART/controlworker.h
index 4635a1fe725098b30e489a2047fa91abde94dfc4..58939cd8d2067a625e0fd4a934f05cacb866b4d8 100644
--- a/groundStation/gui/MicroCART/controlworker.h
+++ b/groundStation/gui/MicroCART/controlworker.h
@@ -2,18 +2,30 @@
 #define CONTROLWORKER_H
 
 #include <QObject>
+#include <QStringList>
 #include "frontend_common.h"
 
-class controlworker : public QObject
+class ControlWorker : public QObject
 {
     Q_OBJECT
 public:
-    explicit controlworker(QObject *parent = 0);
+    explicit ControlWorker(QObject *parent = 0);
+    ~ControlWorker();
 
 signals:
+    void gotNodes(QStringList nodes);
+    void gotParams(QStringList params);
+    void gotParamValue(QString node, QString param, float value);
+    void gotConstantBlocks(QStringList blocks);
+    void paramSet(QString node, QString param);
 
 public slots:
-
+    void connectBackend();
+    void disconnectBackend();
+    void getNodes();
+    void getParams(QString node);
+    void getParamValue(QString node, QString param);
+    void setParamValue(QString node, QString name, float value);
 
 private:
     struct backend_conn * conn;
diff --git a/groundStation/gui/MicroCART/mainwindow.cpp b/groundStation/gui/MicroCART/mainwindow.cpp
index 8df17de0b2c2edf292607a0e3cdc8d0526d30db3..8eed5261fc0bee1fec1a916e4bbfac41eef739a1 100644
--- a/groundStation/gui/MicroCART/mainwindow.cpp
+++ b/groundStation/gui/MicroCART/mainwindow.cpp
@@ -4,18 +4,30 @@
 #include <QFileDialog>
 #include <QThread>
 #include <QTimer>
+#include <QRegExp>
+#include <QProcessEnvironment>
 
 #include "wrappers.h"
 #include "trackerworker.h"
+#include "controlworker.h"
+
+#include "graph_blocks.h"
 
 MainWindow::MainWindow(QWidget *parent) :
     QMainWindow(parent),
     ui(new Ui::MainWindow),
     backendPid(0),
-    backendPipe(-1)
+    backendPipe(-1),
+    setpointList(new QStandardItemModel(this))
 {
     ui->setupUi(this);
 
+    /* Set up environment variables */
+    findChild<QLineEdit *>("socketPath")->setText(QProcessEnvironment::systemEnvironment().value("UCART_SOCKET"));
+
+    /* Idiot lights */
+    findChild<QLabel *>("noGraphWarning1")->setStyleSheet("QLabel {color : red; }");
+
     /* Create a thread for workers */
     QThread* workerThread = new QThread(this);
 
@@ -30,13 +42,30 @@ MainWindow::MainWindow(QWidget *parent) :
     connect(trackerWorker, SIGNAL (finished(float, float, float, float, float, float)),
             this, SLOT (updateTracker(float, float, float, float, float, float)));
 
+    /* Create another worker for the control graph */
+    QThread * cwThread = new QThread(this);
+    ControlWorker * controlWorker = new ControlWorker();
+    controlWorker->moveToThread(cwThread);
+
+    /* Connect signals from control worker */
+    connect(controlWorker, SIGNAL (gotNodes(QStringList)), this, SLOT (newNodes(QStringList)));
+    connect(controlWorker, SIGNAL (gotParams(QStringList)), this, SLOT (newParams(QStringList)));
+    connect(controlWorker, SIGNAL (gotParamValue(QString, QString, float)), this, SLOT (newParamValue(QString, QString, float)));
+    connect(controlWorker, SIGNAL (gotConstantBlocks(QStringList)), this, SLOT (newConstantBlocks(QStringList)));
+    connect(controlWorker, SIGNAL (paramSet(QString, QString)), controlWorker, SLOT (getParamValue(QString, QString)));
+
+    /* Signals to control worker */
+    connect(findChild<QPushButton *>("pbControlRefresh"), SIGNAL (clicked()), controlWorker, SLOT (getNodes()));
+    connect(findChild<QComboBox *>("nodeSelect"), SIGNAL (currentIndexChanged(QString)), controlWorker, SLOT (getParams(QString)));
+    connect(this, SIGNAL (getParamValue(QString, QString)), controlWorker, SLOT (getParamValue(QString, QString)));
+    connect(this, SIGNAL (setParamValue(QString, QString, float)), controlWorker, SLOT (setParamValue(QString, QString, float)));
+
     /* Connect and disconnect from backend when signals emitted */
     connect(this, SIGNAL (connectWorkers()), trackerWorker, SLOT (connectBackend()));
     connect(this, SIGNAL (disconnectWorkers()), trackerWorker, SLOT (disconnectBackend()));
+    connect(this, SIGNAL (connectWorkers()), controlWorker, SLOT (connectBackend()));
+    connect(this, SIGNAL (disconnectWorkers()), controlWorker, SLOT (disconnectBackend()));
 
-    /* Create other workers and add them to the worker thread, then connect them */
-
-    /* Now we can activate the slots of the workers with impunity and not block the UI thread */
     /* Connect refresh button and refresh timer to tracker worker */
     QTimer * trackerTimer = new QTimer(this);
     connect(trackerTimer, SIGNAL(timeout()), trackerWorker, SLOT(process()));
@@ -45,9 +74,18 @@ MainWindow::MainWindow(QWidget *parent) :
     /* Start the things */
     trackerTimer->start(300);
     workerThread->start();
+    cwThread->start();
+
+    /* Connect the setpointlist to the model */
+    findChild<QListView *>("setpointList")->setModel(setpointList);
+
+    /* Connect various things that can result in sending setpoints */
+    connect(findChild<QPushButton *>("pbSendSetpoint"), SIGNAL (clicked()), this, SLOT (sendSetpoints()));
+    connect(findChild<QLineEdit *>("xSetpoint"), SIGNAL (returnPressed()), this, SLOT (sendSetpoints()));
+    connect(findChild<QLineEdit *>("ySetpoint"), SIGNAL (returnPressed()), this, SLOT (sendSetpoints()));
+    connect(findChild<QLineEdit *>("zSetpoint"), SIGNAL (returnPressed()), this, SLOT (sendSetpoints()));
 
-    /* Create a timer to poll stdout and populate virtual console */
-    QTimer * consoleTimer = new QTimer(this);
+    connect(findChild<QListView *>("setpointList"), SIGNAL (doubleClicked(QModelIndex)), this, SLOT (sendSelectedSetpoint()));
 }
 
 MainWindow::~MainWindow()
@@ -61,9 +99,7 @@ void MainWindow::updateConsole()
         char buf[256];
         size_t len = 0;
         len = readBackend(backendPipe, buf, len);
-        printf("%d\n", len);
         if (len > 0) {
-            printf("From pipe: %s", buf);
             QLineEdit * con = findChild<QLineEdit *>("vConsole");
             con->setText(con->text().append(buf));
         }
@@ -72,12 +108,12 @@ void MainWindow::updateConsole()
 
 void MainWindow::updateTracker(float x, float y, float z, float p, float r, float yaw)
 {
-    findChild<QLineEdit *>("xLineEdit")->setText(QString::number(x));
-    findChild<QLineEdit *>("yLineEdit")->setText(QString::number(y));
-    findChild<QLineEdit *>("zLineEdit")->setText(QString::number(z));
-    findChild<QLineEdit *>("pLineEdit")->setText(QString::number(p));
-    findChild<QLineEdit *>("rLineEdit")->setText(QString::number(r));
-    findChild<QLineEdit *>("yLineEdit_2")->setText(QString::number(yaw));
+    findChild<QLineEdit *>("xActual")->setText(QString::number(x));
+    findChild<QLineEdit *>("yActual")->setText(QString::number(y));
+    findChild<QLineEdit *>("zActual")->setText(QString::number(z));
+    findChild<QLineEdit *>("pitchActual")->setText(QString::number(p));
+    findChild<QLineEdit *>("rollActual")->setText(QString::number(r));
+    findChild<QLineEdit *>("yawActual")->setText(QString::number(yaw));
 }
 
 void MainWindow::on_pbStart_clicked()
@@ -117,3 +153,147 @@ void MainWindow::on_chooseBackend_clicked()
          tr("Path to Backend Executable"));
     findChild<QLineEdit *>("backendPath")->setText(backendPath);
 }
+
+
+void MainWindow::newNodes(QStringList blocks)
+{
+    QComboBox * select = findChild<QComboBox *>("nodeSelect");
+    select->clear();
+    select->addItems(blocks);
+
+    this->findChild<QLabel *>("noGraphWarning1")->setVisible(false);
+    this->findChild<QLabel *>("noGraphWarning2")->setVisible(false);
+    this->findChild<QWidget *>("noGraphWarningLine")->setVisible(false);
+}
+
+
+void MainWindow::newConstantBlocks(QStringList blocks)
+{
+    QComboBox * xSelect = findChild<QComboBox *>("xSetpointSelect");
+    xSelect->clear();
+    xSelect->addItems(blocks);
+
+    QComboBox * ySelect = findChild<QComboBox *>("ySetpointSelect");
+    ySelect->clear();
+    ySelect->addItems(blocks);
+
+    QComboBox * zSelect = findChild<QComboBox *>("zSetpointSelect");
+    zSelect->clear();
+    zSelect->addItems(blocks);
+
+    for (ssize_t i = 0; i < blocks.size(); i++) {
+        if (blocks[i].contains("setpoint", Qt::CaseInsensitive) || blocks[i].contains("sp", Qt::CaseInsensitive)) {
+            if (blocks[i].contains("x ", Qt::CaseInsensitive)) {
+                xSelect->setCurrentIndex(i);
+            }
+            if (blocks[i].contains("y ", Qt::CaseInsensitive)) {
+                ySelect->setCurrentIndex(i);
+            }
+            if (blocks[i].contains("z ", Qt::CaseInsensitive) || blocks[i].contains("alt", Qt::CaseInsensitive)) {
+                zSelect->setCurrentIndex(i);
+            }
+        }
+    }
+}
+
+void MainWindow::newParams(QStringList params)
+{
+    QComboBox * select = findChild<QComboBox *>("paramSelect");
+    select->clear();
+    select->addItems(params);
+}
+
+void MainWindow::newParamValue(QString node, QString param, float val)
+{
+    findChild<QLineEdit *>("paramValue")->setText(QString::number(val));
+
+    /* Update the nav page setpoints if it's a setpoint paramvalue */
+    if (node == findChild<QComboBox *>("xSetpointSelect")->currentText()) {
+        findChild<QLineEdit *>("xSetpoint")->setText(QString::number(val));
+    } else if (node == findChild<QComboBox *>("ySetpointSelect")->currentText()) {
+        findChild<QLineEdit *>("ySetpoint")->setText(QString::number(val));
+    } else if (node == findChild<QComboBox *>("zSetpointSelect")->currentText()) {
+        findChild<QLineEdit *>("zSetpoint")->setText(QString::number(val));
+    }
+}
+
+void MainWindow::on_paramSelect_currentIndexChanged(const QString &arg1)
+{
+    emit(getParamValue(findChild<QComboBox *>("nodeSelect")->currentText(), arg1));
+}
+
+void MainWindow::on_paramValue_returnPressed()
+{
+    emit (setParamValue(findChild<QComboBox *>("nodeSelect")->currentText(),
+                        findChild<QComboBox *>("paramSelect")->currentText(),
+                        findChild<QLineEdit *>("paramValue")->text().toFloat()));
+}
+
+void MainWindow::sendSetpoints()
+{
+    emit (setParamValue(findChild<QComboBox *>("xSetpointSelect")->currentText(),
+                        blockDefs[BLOCK_CONSTANT]->param_names[0],
+                        findChild<QLineEdit *>("xSetpoint")->text().toFloat()));
+
+    emit (setParamValue(findChild<QComboBox *>("ySetpointSelect")->currentText(),
+                        blockDefs[BLOCK_CONSTANT]->param_names[0],
+                        findChild<QLineEdit *>("ySetpoint")->text().toFloat()));
+
+    emit (setParamValue(findChild<QComboBox *>("zSetpointSelect")->currentText(),
+                        blockDefs[BLOCK_CONSTANT]->param_names[0],
+                        findChild<QLineEdit *>("zSetpoint")->text().toFloat()));
+}
+
+void MainWindow::on_pbAppendSetpoint_clicked()
+{
+    QString str("[" + findChild<QLineEdit *>("xSetpoint")->text() + ", "+
+            findChild<QLineEdit *>("ySetpoint")->text() + ", " +
+            findChild<QLineEdit *>("zSetpoint")->text() + "]");
+
+    setpointList->appendRow(new QStandardItem(str));
+}
+
+void MainWindow::on_pbNextSetpoint_clicked()
+{
+    QListView * listView = findChild<QListView *>("setpointList");
+    if (listView->currentIndex().isValid() && setpointList->index(listView->currentIndex().row() + 1, 0).isValid()) {
+        listView->setCurrentIndex(setpointList->index(listView->currentIndex().row() + 1, 0));
+    } else {
+        listView->setCurrentIndex(setpointList->index(0, 0));
+    }
+    sendSelectedSetpoint();
+}
+
+
+void MainWindow::sendSelectedSetpoint()
+{
+    if (findChild<QListView *>("setpointList")->currentIndex().isValid()) {
+        QRegExp regex("\\[(.*), (.*), (.*)\\]");
+        int row = findChild<QListView *>("setpointList")->currentIndex().row();
+
+        regex.indexIn(setpointList->item(row)->text());
+        findChild<QLineEdit *>("xSetpoint")->setText(regex.cap(1));
+        findChild<QLineEdit *>("ySetpoint")->setText(regex.cap(2));
+        findChild<QLineEdit *>("zSetpoint")->setText(regex.cap(3));
+
+        sendSetpoints();
+    }
+}
+
+void MainWindow::on_pbActualToSetpoint_clicked()
+{
+    findChild<QLineEdit *>("xSetpoint")->setText(findChild<QLineEdit *>("xActual")->text());
+    findChild<QLineEdit *>("ySetpoint")->setText(findChild<QLineEdit *>("yActual")->text());
+    findChild<QLineEdit *>("zSetpoint")->setText(findChild<QLineEdit *>("zActual")->text());
+}
+
+void MainWindow::on_pbDeleteSetpoint_clicked()
+{
+    if (findChild<QListView *>("setpointList")->currentIndex().isValid()) {
+        setpointList->removeRow(findChild<QListView *>("setpointList")->currentIndex().row());
+    }
+}
+
+void MainWindow::on_socketPath_returnPressed()
+{
+}
diff --git a/groundStation/gui/MicroCART/mainwindow.h b/groundStation/gui/MicroCART/mainwindow.h
index 042e5f73c30988c1581b28b221f2d7e76d2faed3..95dc0261e7241780af96e026696240382d52fb6d 100644
--- a/groundStation/gui/MicroCART/mainwindow.h
+++ b/groundStation/gui/MicroCART/mainwindow.h
@@ -2,6 +2,8 @@
 #define MAINWINDOW_H
 
 #include <QMainWindow>
+#include <QStringList>
+#include <QStandardItemModel>
 
 namespace Ui {
 class MainWindow;
@@ -18,6 +20,8 @@ public:
 signals:
     void connectWorkers();
     void disconnectWorkers();
+    void getParamValue(QString node, QString param);
+    void setParamValue(QString node, QString param, float value);
 
 private slots:
     void on_pbStart_clicked();
@@ -32,11 +36,34 @@ private slots:
 
     void updateConsole();
 
+    void newNodes(QStringList blocks);
+    void newParams(QStringList params);
+    void newParamValue(QString node, QString param, float val);
+    void newConstantBlocks(QStringList blocks);
+
+    void on_paramSelect_currentIndexChanged(const QString &arg1);
+
+    void on_paramValue_returnPressed();
+
+    void on_pbAppendSetpoint_clicked();
+
+    void on_pbNextSetpoint_clicked();
+
+    void sendSetpoints();
+    void sendSelectedSetpoint();
+
+    void on_pbActualToSetpoint_clicked();
+
+    void on_pbDeleteSetpoint_clicked();
+
+    void on_socketPath_returnPressed();
+
 private:
     Ui::MainWindow *ui;
     pid_t backendPid;
     int backendPipe;
     int backendState;
+    QStandardItemModel * setpointList;
 };
 
 #endif // MAINWINDOW_H
diff --git a/groundStation/gui/MicroCART/mainwindow.ui b/groundStation/gui/MicroCART/mainwindow.ui
index 6aa4020ccc5570ac35d950988f8d8c3057689dc4..2931a125f4b689f91261e140f168b56c8d20355a 100644
--- a/groundStation/gui/MicroCART/mainwindow.ui
+++ b/groundStation/gui/MicroCART/mainwindow.ui
@@ -6,393 +6,705 @@
    <rect>
     <x>0</x>
     <y>0</y>
-    <width>798</width>
-    <height>581</height>
+    <width>874</width>
+    <height>596</height>
    </rect>
   </property>
   <property name="windowTitle">
    <string>MainWindow</string>
   </property>
   <widget class="QWidget" name="centralWidget">
-   <widget class="QTabWidget" name="tabWidget">
-    <property name="geometry">
-     <rect>
-      <x>0</x>
-      <y>10</y>
-      <width>801</width>
-      <height>471</height>
-     </rect>
-    </property>
-    <property name="sizePolicy">
-     <sizepolicy hsizetype="Maximum" vsizetype="Maximum">
-      <horstretch>0</horstretch>
-      <verstretch>0</verstretch>
-     </sizepolicy>
-    </property>
-    <property name="currentIndex">
-     <number>2</number>
-    </property>
-    <widget class="QWidget" name="backend">
-     <attribute name="title">
-      <string>Connection</string>
-     </attribute>
-     <widget class="QWidget" name="verticalLayoutWidget">
-      <property name="geometry">
-       <rect>
-        <x>0</x>
-        <y>0</y>
-        <width>761</width>
-        <height>341</height>
-       </rect>
+   <layout class="QVBoxLayout" name="verticalLayout_4">
+    <item>
+     <widget class="QTabWidget" name="tabWidget">
+      <property name="currentIndex">
+       <number>0</number>
       </property>
-      <layout class="QVBoxLayout" name="verticalLayout">
-       <item>
-        <layout class="QHBoxLayout" name="horizontalLayout">
-         <item>
-          <widget class="QPushButton" name="pbStart">
-           <property name="enabled">
-            <bool>false</bool>
-           </property>
-           <property name="text">
-            <string>Start</string>
-           </property>
-          </widget>
-         </item>
-         <item>
-          <widget class="QPushButton" name="pbConnect">
-           <property name="enabled">
-            <bool>true</bool>
-           </property>
-           <property name="text">
-            <string>Connect</string>
-           </property>
-          </widget>
-         </item>
-         <item>
-          <spacer name="horizontalSpacer">
-           <property name="orientation">
-            <enum>Qt::Horizontal</enum>
-           </property>
-           <property name="sizeHint" stdset="0">
-            <size>
-             <width>40</width>
-             <height>20</height>
-            </size>
-           </property>
-          </spacer>
-         </item>
-         <item>
-          <widget class="QPushButton" name="pbStop">
-           <property name="enabled">
-            <bool>false</bool>
-           </property>
-           <property name="text">
-            <string>Stop/Disconnect</string>
-           </property>
-          </widget>
-         </item>
-        </layout>
-       </item>
-       <item>
-        <layout class="QHBoxLayout" name="horizontalLayout_2">
-         <item>
-          <widget class="QPushButton" name="chooseBackend">
-           <property name="text">
-            <string>Choose Backend</string>
-           </property>
-          </widget>
-         </item>
-         <item>
-          <widget class="QLineEdit" name="backendPath"/>
-         </item>
-        </layout>
-       </item>
-       <item>
-        <spacer name="verticalSpacer">
-         <property name="orientation">
-          <enum>Qt::Vertical</enum>
-         </property>
-         <property name="sizeHint" stdset="0">
-          <size>
-           <width>20</width>
-           <height>40</height>
-          </size>
-         </property>
-        </spacer>
-       </item>
-       <item>
-        <widget class="QTextEdit" name="vConsole">
-         <property name="enabled">
-          <bool>true</bool>
-         </property>
-         <property name="readOnly">
-          <bool>true</bool>
-         </property>
-        </widget>
-       </item>
-      </layout>
+      <widget class="QWidget" name="backend">
+       <attribute name="title">
+        <string>Backend</string>
+       </attribute>
+       <layout class="QVBoxLayout" name="verticalLayout">
+        <item>
+         <layout class="QFormLayout" name="formLayout_3">
+          <item row="0" column="0">
+           <widget class="QLabel" name="socketPathLabel">
+            <property name="text">
+             <string>UCART_SOCKET_PATH</string>
+            </property>
+           </widget>
+          </item>
+          <item row="0" column="1">
+           <widget class="QLineEdit" name="socketPath">
+            <property name="enabled">
+             <bool>false</bool>
+            </property>
+           </widget>
+          </item>
+         </layout>
+        </item>
+        <item>
+         <layout class="QHBoxLayout" name="horizontalLayout_8">
+          <item>
+           <widget class="QPushButton" name="chooseBackend">
+            <property name="enabled">
+             <bool>false</bool>
+            </property>
+            <property name="text">
+             <string>Choose Backend</string>
+            </property>
+           </widget>
+          </item>
+          <item>
+           <widget class="QLineEdit" name="backendPath_2">
+            <property name="enabled">
+             <bool>false</bool>
+            </property>
+           </widget>
+          </item>
+         </layout>
+        </item>
+        <item>
+         <layout class="QHBoxLayout" name="horizontalLayout_7">
+          <item>
+           <widget class="QPushButton" name="pbStart">
+            <property name="enabled">
+             <bool>false</bool>
+            </property>
+            <property name="text">
+             <string>Start</string>
+            </property>
+           </widget>
+          </item>
+          <item>
+           <widget class="QPushButton" name="pbConnect">
+            <property name="enabled">
+             <bool>true</bool>
+            </property>
+            <property name="text">
+             <string>Connect</string>
+            </property>
+           </widget>
+          </item>
+          <item>
+           <spacer name="horizontalSpacer_2">
+            <property name="orientation">
+             <enum>Qt::Horizontal</enum>
+            </property>
+            <property name="sizeHint" stdset="0">
+             <size>
+              <width>40</width>
+              <height>20</height>
+             </size>
+            </property>
+           </spacer>
+          </item>
+          <item>
+           <widget class="QPushButton" name="pbStop">
+            <property name="enabled">
+             <bool>false</bool>
+            </property>
+            <property name="text">
+             <string>Stop/Disconnect</string>
+            </property>
+           </widget>
+          </item>
+         </layout>
+        </item>
+        <item>
+         <spacer name="verticalSpacer">
+          <property name="orientation">
+           <enum>Qt::Vertical</enum>
+          </property>
+          <property name="sizeHint" stdset="0">
+           <size>
+            <width>20</width>
+            <height>155</height>
+           </size>
+          </property>
+         </spacer>
+        </item>
+        <item>
+         <widget class="QTextEdit" name="vConsole">
+          <property name="enabled">
+           <bool>true</bool>
+          </property>
+          <property name="readOnly">
+           <bool>true</bool>
+          </property>
+         </widget>
+        </item>
+       </layout>
+      </widget>
+      <widget class="QWidget" name="controlGraph">
+       <attribute name="title">
+        <string>Controller Graph</string>
+       </attribute>
+       <layout class="QVBoxLayout" name="verticalLayout_2">
+        <item>
+         <widget class="QLabel" name="graphImage">
+          <property name="text">
+           <string>Refresh to display controller graph</string>
+          </property>
+         </widget>
+        </item>
+        <item>
+         <spacer name="verticalSpacer_2">
+          <property name="orientation">
+           <enum>Qt::Vertical</enum>
+          </property>
+          <property name="sizeHint" stdset="0">
+           <size>
+            <width>20</width>
+            <height>159</height>
+           </size>
+          </property>
+         </spacer>
+        </item>
+        <item>
+         <widget class="Line" name="line_2">
+          <property name="orientation">
+           <enum>Qt::Horizontal</enum>
+          </property>
+         </widget>
+        </item>
+        <item>
+         <widget class="QPushButton" name="pbControlRefresh">
+          <property name="text">
+           <string>Refresh Controller Graph</string>
+          </property>
+         </widget>
+        </item>
+        <item>
+         <widget class="Line" name="line">
+          <property name="orientation">
+           <enum>Qt::Horizontal</enum>
+          </property>
+         </widget>
+        </item>
+        <item>
+         <layout class="QHBoxLayout" name="horizontalLayout_6">
+          <item>
+           <widget class="QLabel" name="label">
+            <property name="sizePolicy">
+             <sizepolicy hsizetype="Fixed" vsizetype="Minimum">
+              <horstretch>0</horstretch>
+              <verstretch>0</verstretch>
+             </sizepolicy>
+            </property>
+            <property name="text">
+             <string>Node:</string>
+            </property>
+           </widget>
+          </item>
+          <item>
+           <widget class="QComboBox" name="nodeSelect">
+            <property name="sizePolicy">
+             <sizepolicy hsizetype="Preferred" vsizetype="Fixed">
+              <horstretch>0</horstretch>
+              <verstretch>0</verstretch>
+             </sizepolicy>
+            </property>
+           </widget>
+          </item>
+          <item>
+           <widget class="QLabel" name="label_2">
+            <property name="sizePolicy">
+             <sizepolicy hsizetype="Fixed" vsizetype="Preferred">
+              <horstretch>0</horstretch>
+              <verstretch>0</verstretch>
+             </sizepolicy>
+            </property>
+            <property name="text">
+             <string>Param:</string>
+            </property>
+           </widget>
+          </item>
+          <item>
+           <widget class="QComboBox" name="paramSelect"/>
+          </item>
+          <item>
+           <widget class="QLineEdit" name="paramValue">
+            <property name="sizePolicy">
+             <sizepolicy hsizetype="Preferred" vsizetype="Fixed">
+              <horstretch>0</horstretch>
+              <verstretch>0</verstretch>
+             </sizepolicy>
+            </property>
+           </widget>
+          </item>
+         </layout>
+        </item>
+        <item>
+         <layout class="QHBoxLayout" name="horizontalLayout_4">
+          <item>
+           <widget class="QLabel" name="label_4">
+            <property name="sizePolicy">
+             <sizepolicy hsizetype="Fixed" vsizetype="Minimum">
+              <horstretch>0</horstretch>
+              <verstretch>0</verstretch>
+             </sizepolicy>
+            </property>
+            <property name="text">
+             <string>X Setpoint</string>
+            </property>
+           </widget>
+          </item>
+          <item>
+           <widget class="QComboBox" name="xSetpointSelect">
+            <property name="sizePolicy">
+             <sizepolicy hsizetype="Preferred" vsizetype="Fixed">
+              <horstretch>0</horstretch>
+              <verstretch>0</verstretch>
+             </sizepolicy>
+            </property>
+           </widget>
+          </item>
+          <item>
+           <widget class="QLabel" name="label_5">
+            <property name="sizePolicy">
+             <sizepolicy hsizetype="Fixed" vsizetype="Preferred">
+              <horstretch>0</horstretch>
+              <verstretch>0</verstretch>
+             </sizepolicy>
+            </property>
+            <property name="text">
+             <string>Y Setpoint</string>
+            </property>
+           </widget>
+          </item>
+          <item>
+           <widget class="QComboBox" name="ySetpointSelect"/>
+          </item>
+          <item>
+           <widget class="QLabel" name="label_3">
+            <property name="sizePolicy">
+             <sizepolicy hsizetype="Fixed" vsizetype="Preferred">
+              <horstretch>0</horstretch>
+              <verstretch>0</verstretch>
+             </sizepolicy>
+            </property>
+            <property name="text">
+             <string>Z Setpoint</string>
+            </property>
+           </widget>
+          </item>
+          <item>
+           <widget class="QComboBox" name="zSetpointSelect"/>
+          </item>
+         </layout>
+        </item>
+       </layout>
+      </widget>
+      <widget class="QWidget" name="navigation">
+       <attribute name="title">
+        <string>Navigation</string>
+       </attribute>
+       <layout class="QVBoxLayout" name="verticalLayout_3">
+        <item>
+         <widget class="QLabel" name="noGraphWarning1">
+          <property name="font">
+           <font>
+            <pointsize>32</pointsize>
+            <weight>75</weight>
+            <bold>true</bold>
+            <underline>true</underline>
+           </font>
+          </property>
+          <property name="text">
+           <string>NO CONTROL GRAPH LOADED!</string>
+          </property>
+          <property name="alignment">
+           <set>Qt::AlignCenter</set>
+          </property>
+         </widget>
+        </item>
+        <item>
+         <widget class="QLabel" name="noGraphWarning2">
+          <property name="text">
+           <string>These controls won't work right unless the correct control graph is loaded.</string>
+          </property>
+          <property name="alignment">
+           <set>Qt::AlignCenter</set>
+          </property>
+         </widget>
+        </item>
+        <item>
+         <widget class="Line" name="noGraphWarningLine">
+          <property name="orientation">
+           <enum>Qt::Horizontal</enum>
+          </property>
+         </widget>
+        </item>
+        <item>
+         <layout class="QHBoxLayout" name="horizontalLayout_3">
+          <item>
+           <layout class="QVBoxLayout" name="verticalLayout_7">
+            <item>
+             <widget class="QLabel" name="label_6">
+              <property name="sizePolicy">
+               <sizepolicy hsizetype="Preferred" vsizetype="Preferred">
+                <horstretch>0</horstretch>
+                <verstretch>0</verstretch>
+               </sizepolicy>
+              </property>
+              <property name="text">
+               <string>Current Position</string>
+              </property>
+             </widget>
+            </item>
+            <item>
+             <layout class="QFormLayout" name="formLayout">
+              <property name="fieldGrowthPolicy">
+               <enum>QFormLayout::ExpandingFieldsGrow</enum>
+              </property>
+              <item row="0" column="0">
+               <widget class="QLabel" name="xLabel">
+                <property name="text">
+                 <string>X</string>
+                </property>
+               </widget>
+              </item>
+              <item row="0" column="1">
+               <widget class="QLineEdit" name="xActual">
+                <property name="enabled">
+                 <bool>false</bool>
+                </property>
+                <property name="sizePolicy">
+                 <sizepolicy hsizetype="Minimum" vsizetype="Fixed">
+                  <horstretch>0</horstretch>
+                  <verstretch>0</verstretch>
+                 </sizepolicy>
+                </property>
+               </widget>
+              </item>
+              <item row="1" column="0">
+               <widget class="QLabel" name="yLabel">
+                <property name="text">
+                 <string>Y</string>
+                </property>
+               </widget>
+              </item>
+              <item row="1" column="1">
+               <widget class="QLineEdit" name="yActual">
+                <property name="enabled">
+                 <bool>false</bool>
+                </property>
+                <property name="sizePolicy">
+                 <sizepolicy hsizetype="Minimum" vsizetype="Fixed">
+                  <horstretch>0</horstretch>
+                  <verstretch>0</verstretch>
+                 </sizepolicy>
+                </property>
+               </widget>
+              </item>
+              <item row="2" column="0">
+               <widget class="QLabel" name="zLabel">
+                <property name="text">
+                 <string>Z</string>
+                </property>
+               </widget>
+              </item>
+              <item row="2" column="1">
+               <widget class="QLineEdit" name="zActual">
+                <property name="enabled">
+                 <bool>false</bool>
+                </property>
+                <property name="sizePolicy">
+                 <sizepolicy hsizetype="Minimum" vsizetype="Fixed">
+                  <horstretch>0</horstretch>
+                  <verstretch>0</verstretch>
+                 </sizepolicy>
+                </property>
+               </widget>
+              </item>
+              <item row="3" column="0">
+               <widget class="QLabel" name="pLabel">
+                <property name="text">
+                 <string>P</string>
+                </property>
+               </widget>
+              </item>
+              <item row="3" column="1">
+               <widget class="QLineEdit" name="pitchActual">
+                <property name="enabled">
+                 <bool>false</bool>
+                </property>
+                <property name="sizePolicy">
+                 <sizepolicy hsizetype="Minimum" vsizetype="Fixed">
+                  <horstretch>0</horstretch>
+                  <verstretch>0</verstretch>
+                 </sizepolicy>
+                </property>
+               </widget>
+              </item>
+              <item row="4" column="0">
+               <widget class="QLabel" name="rLabel">
+                <property name="text">
+                 <string>R</string>
+                </property>
+               </widget>
+              </item>
+              <item row="4" column="1">
+               <widget class="QLineEdit" name="rollActual">
+                <property name="enabled">
+                 <bool>false</bool>
+                </property>
+                <property name="sizePolicy">
+                 <sizepolicy hsizetype="Minimum" vsizetype="Fixed">
+                  <horstretch>0</horstretch>
+                  <verstretch>0</verstretch>
+                 </sizepolicy>
+                </property>
+               </widget>
+              </item>
+              <item row="5" column="0">
+               <widget class="QLabel" name="yLabel_2">
+                <property name="text">
+                 <string>Y</string>
+                </property>
+               </widget>
+              </item>
+              <item row="5" column="1">
+               <widget class="QLineEdit" name="yawActual">
+                <property name="enabled">
+                 <bool>false</bool>
+                </property>
+                <property name="sizePolicy">
+                 <sizepolicy hsizetype="Minimum" vsizetype="Fixed">
+                  <horstretch>0</horstretch>
+                  <verstretch>0</verstretch>
+                 </sizepolicy>
+                </property>
+               </widget>
+              </item>
+             </layout>
+            </item>
+            <item>
+             <widget class="QPushButton" name="pbActualToSetpoint">
+              <property name="text">
+               <string>To Setpoint</string>
+              </property>
+             </widget>
+            </item>
+            <item>
+             <spacer name="verticalSpacer_4">
+              <property name="orientation">
+               <enum>Qt::Vertical</enum>
+              </property>
+              <property name="sizeHint" stdset="0">
+               <size>
+                <width>20</width>
+                <height>40</height>
+               </size>
+              </property>
+             </spacer>
+            </item>
+           </layout>
+          </item>
+          <item>
+           <layout class="QVBoxLayout" name="verticalLayout_5">
+            <item>
+             <widget class="QLabel" name="label_7">
+              <property name="text">
+               <string>Position Setpoints</string>
+              </property>
+             </widget>
+            </item>
+            <item>
+             <layout class="QFormLayout" name="formLayout_2">
+              <item row="0" column="0">
+               <widget class="QLabel" name="setpointLabel">
+                <property name="text">
+                 <string>X</string>
+                </property>
+               </widget>
+              </item>
+              <item row="0" column="1">
+               <widget class="QLineEdit" name="xSetpoint">
+                <property name="sizePolicy">
+                 <sizepolicy hsizetype="Minimum" vsizetype="Fixed">
+                  <horstretch>0</horstretch>
+                  <verstretch>0</verstretch>
+                 </sizepolicy>
+                </property>
+               </widget>
+              </item>
+              <item row="1" column="1">
+               <widget class="QLineEdit" name="ySetpoint">
+                <property name="sizePolicy">
+                 <sizepolicy hsizetype="Minimum" vsizetype="Fixed">
+                  <horstretch>0</horstretch>
+                  <verstretch>0</verstretch>
+                 </sizepolicy>
+                </property>
+               </widget>
+              </item>
+              <item row="2" column="1">
+               <widget class="QLineEdit" name="zSetpoint">
+                <property name="sizePolicy">
+                 <sizepolicy hsizetype="Minimum" vsizetype="Fixed">
+                  <horstretch>0</horstretch>
+                  <verstretch>0</verstretch>
+                 </sizepolicy>
+                </property>
+               </widget>
+              </item>
+              <item row="2" column="0">
+               <widget class="QLabel" name="setpointLabel_3">
+                <property name="text">
+                 <string>Z</string>
+                </property>
+               </widget>
+              </item>
+              <item row="1" column="0">
+               <widget class="QLabel" name="setpointLabel_2">
+                <property name="text">
+                 <string>Y</string>
+                </property>
+               </widget>
+              </item>
+             </layout>
+            </item>
+            <item>
+             <widget class="QPushButton" name="pbSendSetpoint">
+              <property name="text">
+               <string>Send to Quad</string>
+              </property>
+             </widget>
+            </item>
+            <item>
+             <widget class="Line" name="line_3">
+              <property name="orientation">
+               <enum>Qt::Horizontal</enum>
+              </property>
+             </widget>
+            </item>
+            <item>
+             <widget class="QPushButton" name="pbAppendSetpoint">
+              <property name="text">
+               <string>Append</string>
+              </property>
+             </widget>
+            </item>
+            <item>
+             <widget class="QPushButton" name="pbInsertSetpoint">
+              <property name="enabled">
+               <bool>false</bool>
+              </property>
+              <property name="text">
+               <string>Insert</string>
+              </property>
+             </widget>
+            </item>
+            <item>
+             <widget class="QPushButton" name="pbDeleteSetpoint">
+              <property name="enabled">
+               <bool>true</bool>
+              </property>
+              <property name="text">
+               <string>Delete</string>
+              </property>
+             </widget>
+            </item>
+            <item>
+             <spacer name="verticalSpacer_3">
+              <property name="orientation">
+               <enum>Qt::Vertical</enum>
+              </property>
+              <property name="sizeHint" stdset="0">
+               <size>
+                <width>20</width>
+                <height>40</height>
+               </size>
+              </property>
+             </spacer>
+            </item>
+           </layout>
+          </item>
+          <item>
+           <layout class="QVBoxLayout" name="verticalLayout_6">
+            <item>
+             <widget class="QLabel" name="label_8">
+              <property name="sizePolicy">
+               <sizepolicy hsizetype="Minimum" vsizetype="Preferred">
+                <horstretch>0</horstretch>
+                <verstretch>0</verstretch>
+               </sizepolicy>
+              </property>
+              <property name="text">
+               <string>Waypoints</string>
+              </property>
+             </widget>
+            </item>
+            <item>
+             <widget class="QListView" name="setpointList">
+              <property name="sizePolicy">
+               <sizepolicy hsizetype="Minimum" vsizetype="Expanding">
+                <horstretch>0</horstretch>
+                <verstretch>0</verstretch>
+               </sizepolicy>
+              </property>
+              <property name="editTriggers">
+               <set>QAbstractItemView::NoEditTriggers</set>
+              </property>
+              <property name="dragEnabled">
+               <bool>true</bool>
+              </property>
+              <property name="dragDropOverwriteMode">
+               <bool>false</bool>
+              </property>
+              <property name="dragDropMode">
+               <enum>QAbstractItemView::InternalMove</enum>
+              </property>
+              <property name="defaultDropAction">
+               <enum>Qt::MoveAction</enum>
+              </property>
+             </widget>
+            </item>
+            <item>
+             <widget class="QPushButton" name="pbNextSetpoint">
+              <property name="text">
+               <string>Send Next</string>
+              </property>
+             </widget>
+            </item>
+           </layout>
+          </item>
+          <item>
+           <spacer name="horizontalSpacer_3">
+            <property name="orientation">
+             <enum>Qt::Horizontal</enum>
+            </property>
+            <property name="sizeHint" stdset="0">
+             <size>
+              <width>40</width>
+              <height>20</height>
+             </size>
+            </property>
+           </spacer>
+          </item>
+         </layout>
+        </item>
+        <item>
+         <widget class="QPushButton" name="pbRefresh">
+          <property name="text">
+           <string>Refresh</string>
+          </property>
+         </widget>
+        </item>
+       </layout>
+      </widget>
      </widget>
-    </widget>
-    <widget class="QWidget" name="status">
-     <attribute name="title">
-      <string>Quad Status</string>
-     </attribute>
-     <widget class="QWidget" name="horizontalLayoutWidget_2">
-      <property name="geometry">
-       <rect>
-        <x>-1</x>
-        <y>9</y>
-        <width>451</width>
-        <height>226</height>
-       </rect>
-      </property>
-      <layout class="QHBoxLayout" name="horizontalLayout_3">
-       <item>
-        <layout class="QFormLayout" name="formLayout">
-         <property name="fieldGrowthPolicy">
-          <enum>QFormLayout::ExpandingFieldsGrow</enum>
-         </property>
-         <item row="0" column="0">
-          <widget class="QLabel" name="xLabel">
-           <property name="text">
-            <string>X</string>
-           </property>
-          </widget>
-         </item>
-         <item row="0" column="1">
-          <widget class="QLineEdit" name="xLineEdit">
-           <property name="enabled">
-            <bool>false</bool>
-           </property>
-          </widget>
-         </item>
-         <item row="1" column="0">
-          <widget class="QLabel" name="yLabel">
-           <property name="text">
-            <string>Y</string>
-           </property>
-          </widget>
-         </item>
-         <item row="1" column="1">
-          <widget class="QLineEdit" name="yLineEdit">
-           <property name="enabled">
-            <bool>false</bool>
-           </property>
-          </widget>
-         </item>
-         <item row="2" column="0">
-          <widget class="QLabel" name="zLabel">
-           <property name="text">
-            <string>Z</string>
-           </property>
-          </widget>
-         </item>
-         <item row="2" column="1">
-          <widget class="QLineEdit" name="zLineEdit">
-           <property name="enabled">
-            <bool>false</bool>
-           </property>
-          </widget>
-         </item>
-         <item row="3" column="0">
-          <widget class="QLabel" name="pLabel">
-           <property name="text">
-            <string>P</string>
-           </property>
-          </widget>
-         </item>
-         <item row="3" column="1">
-          <widget class="QLineEdit" name="pLineEdit">
-           <property name="enabled">
-            <bool>false</bool>
-           </property>
-          </widget>
-         </item>
-         <item row="4" column="0">
-          <widget class="QLabel" name="rLabel">
-           <property name="text">
-            <string>R</string>
-           </property>
-          </widget>
-         </item>
-         <item row="4" column="1">
-          <widget class="QLineEdit" name="rLineEdit">
-           <property name="enabled">
-            <bool>false</bool>
-           </property>
-          </widget>
-         </item>
-         <item row="5" column="0">
-          <widget class="QLabel" name="yLabel_2">
-           <property name="text">
-            <string>Y</string>
-           </property>
-          </widget>
-         </item>
-         <item row="5" column="1">
-          <widget class="QLineEdit" name="yLineEdit_2">
-           <property name="enabled">
-            <bool>false</bool>
-           </property>
-          </widget>
-         </item>
-        </layout>
-       </item>
-       <item>
-        <layout class="QFormLayout" name="formLayout_2"/>
-       </item>
-      </layout>
-     </widget>
-    </widget>
-    <widget class="QWidget" name="controlGraph">
-     <attribute name="title">
-      <string>Controller Graph</string>
-     </attribute>
-     <widget class="QWidget" name="verticalLayoutWidget_2">
-      <property name="geometry">
-       <rect>
-        <x>9</x>
-        <y>9</y>
-        <width>771</width>
-        <height>421</height>
-       </rect>
-      </property>
-      <layout class="QVBoxLayout" name="verticalLayout_2">
-       <item>
-        <widget class="QLabel" name="graphImage">
-         <property name="text">
-          <string>TextLabel</string>
-         </property>
-        </widget>
-       </item>
-       <item>
-        <spacer name="verticalSpacer_2">
-         <property name="orientation">
-          <enum>Qt::Vertical</enum>
-         </property>
-         <property name="sizeHint" stdset="0">
-          <size>
-           <width>20</width>
-           <height>40</height>
-          </size>
-         </property>
-        </spacer>
-       </item>
-       <item>
-        <widget class="Line" name="line">
-         <property name="orientation">
-          <enum>Qt::Horizontal</enum>
-         </property>
-        </widget>
-       </item>
-       <item>
-        <layout class="QHBoxLayout" name="horizontalLayout_6">
-         <item>
-          <widget class="QLabel" name="label">
-           <property name="sizePolicy">
-            <sizepolicy hsizetype="Fixed" vsizetype="Minimum">
-             <horstretch>0</horstretch>
-             <verstretch>0</verstretch>
-            </sizepolicy>
-           </property>
-           <property name="text">
-            <string>Node:</string>
-           </property>
-          </widget>
-         </item>
-         <item>
-          <widget class="QComboBox" name="nodeSelect">
-           <property name="sizePolicy">
-            <sizepolicy hsizetype="Preferred" vsizetype="Fixed">
-             <horstretch>0</horstretch>
-             <verstretch>0</verstretch>
-            </sizepolicy>
-           </property>
-          </widget>
-         </item>
-         <item>
-          <widget class="QLabel" name="label_2">
-           <property name="sizePolicy">
-            <sizepolicy hsizetype="Fixed" vsizetype="Preferred">
-             <horstretch>0</horstretch>
-             <verstretch>0</verstretch>
-            </sizepolicy>
-           </property>
-           <property name="text">
-            <string>Param:</string>
-           </property>
-          </widget>
-         </item>
-         <item>
-          <widget class="QComboBox" name="paramSelect"/>
-         </item>
-         <item>
-          <widget class="QLabel" name="label_3">
-           <property name="sizePolicy">
-            <sizepolicy hsizetype="Fixed" vsizetype="Preferred">
-             <horstretch>0</horstretch>
-             <verstretch>0</verstretch>
-            </sizepolicy>
-           </property>
-           <property name="text">
-            <string>Value:</string>
-           </property>
-          </widget>
-         </item>
-         <item>
-          <widget class="QLineEdit" name="paramValue">
-           <property name="sizePolicy">
-            <sizepolicy hsizetype="Preferred" vsizetype="Fixed">
-             <horstretch>0</horstretch>
-             <verstretch>0</verstretch>
-            </sizepolicy>
-           </property>
-          </widget>
-         </item>
-        </layout>
-       </item>
-       <item>
-        <widget class="Line" name="line_2">
-         <property name="orientation">
-          <enum>Qt::Horizontal</enum>
-         </property>
-        </widget>
-       </item>
-       <item>
-        <layout class="QHBoxLayout" name="horizontalLayout_5">
-         <item>
-          <widget class="QPushButton" name="pbControlRefresh">
-           <property name="text">
-            <string>Refresh Controller Graph</string>
-           </property>
-          </widget>
-         </item>
-        </layout>
-       </item>
-      </layout>
-     </widget>
-    </widget>
-   </widget>
-   <widget class="QPushButton" name="pbRefresh">
-    <property name="geometry">
-     <rect>
-      <x>10</x>
-      <y>480</y>
-      <width>88</width>
-      <height>34</height>
-     </rect>
-    </property>
-    <property name="text">
-     <string>Refresh</string>
-    </property>
-   </widget>
+    </item>
+   </layout>
   </widget>
   <widget class="QMenuBar" name="menuBar">
    <property name="geometry">
     <rect>
      <x>0</x>
      <y>0</y>
-     <width>798</width>
+     <width>874</width>
      <height>30</height>
     </rect>
    </property>
diff --git a/groundStation/gui/MicroCART/trackerworker.cpp b/groundStation/gui/MicroCART/trackerworker.cpp
index f5d3cda28a381f9c5e0b79a0ea9464c7896816c9..9a6830a7755f635ae15428fa4686c8a7f44abec1 100644
--- a/groundStation/gui/MicroCART/trackerworker.cpp
+++ b/groundStation/gui/MicroCART/trackerworker.cpp
@@ -32,6 +32,6 @@ void TrackerWorker::process()
         struct frontend_tracker_data td;
         frontend_track(conn, &td);
 
-        emit finished(td.height, td.lateral, td.longitudinal, td.pitch, td.roll, td.yaw);
+        emit finished(td.longitudinal, td.lateral, td.height, td.pitch, td.roll, td.yaw);
     }
 }
diff --git a/groundStation/src/frontend/frontend_nodes.h b/groundStation/src/frontend/frontend_nodes.h
index 6b0a340624e6245e5cb87129548f6ff90a6e21eb..fa7d57f14c6123ad04f6b07ab114a3deee2006aa 100644
--- a/groundStation/src/frontend/frontend_nodes.h
+++ b/groundStation/src/frontend/frontend_nodes.h
@@ -3,6 +3,10 @@
 
 #include "frontend_common.h"
 
+#ifdef __cplusplus
+extern "C" {
+#endif
+
 /* Get the block_id, type_id and name of
  * all of the nodes in the current comp_graph.
  *
@@ -32,4 +36,8 @@ void frontend_free_node_data(
 		struct frontend_node_data * nd,
 		size_t num_nodes);
 
+#ifdef __cplusplus
+}
+#endif
+
 #endif /* __FRONTEND_NODES_H */
diff --git a/groundStation/src/frontend/frontend_param.h b/groundStation/src/frontend/frontend_param.h
index 26854dd0d7ee104e20edb524a5459a2535794426..392fbd83bf6d02424d0f6d54bb0187c579105eab 100644
--- a/groundStation/src/frontend/frontend_param.h
+++ b/groundStation/src/frontend/frontend_param.h
@@ -3,6 +3,10 @@
 
 #include "frontend_common.h"
 
+#ifdef __cplusplus
+extern "C" {
+#endif
+
 /* Get the value of block.param
  *
  * Returns 0 on success, 1 on error
@@ -19,5 +23,9 @@ int frontend_setparam(
 		struct backend_conn * conn,
 		struct frontend_param_data * param_data);
 
+#ifdef __cplusplus
+}
+#endif
+
 
-#endif /* __FRONTEND_PARAM_H */
\ No newline at end of file
+#endif /* __FRONTEND_PARAM_H */