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/scripts/bypass_kill_switch.sh b/groundStation/scripts/bypass_kill_switch.sh
index 183df7b8e30d642e0bf92b8a0fbc7b1ef1949e7f..afd5e6b35dc8422a697e2768962aa50de934a12e 100755
--- a/groundStation/scripts/bypass_kill_switch.sh
+++ b/groundStation/scripts/bypass_kill_switch.sh
@@ -4,10 +4,18 @@ if [ -z "$1" ]; then
echo "No argument supplied"
exit 0
fi
-cd ..
+#cd ..
+
+
+./setsource 35 1 38 0
+./setsource 35 2 38 0
+./setsource 35 3 38 0
+
+./setsource 35 0 13 0
+
while true ; do
- ./setparam 37 0 $1
+ ./setparam 13 0 $(( $1 + 1 ))
sleep 0.4
- ./setparam 37 0 $(( $1 - 1 ))
+ ./setparam 13 0 $(( $1 - 1 ))
sleep 0.4
done
diff --git a/groundStation/scripts/rate_controller.sh b/groundStation/scripts/rate_controller.sh
index fd37f3b5b514c29cb2a501e00b4b4c7f122c49bc..11f4e2f2f2cd559cd25a2c66b580244a696f63ef 100755
--- a/groundStation/scripts/rate_controller.sh
+++ b/groundStation/scripts/rate_controller.sh
@@ -45,9 +45,3 @@
./setparam "Y vel PID" "kd" -0.02
./setparam "y vel" "alpha" 0.88
-
-./setparam "x val clamp" 0 -1
-./setparam "x val clamp" 1 1
-
-./setparam "y val clamp" 0 -1
-./setparam "y val clamp" 1 1
diff --git a/groundStation/scripts/take_off.sh b/groundStation/scripts/take_off.sh
new file mode 100755
index 0000000000000000000000000000000000000000..70b9642291695d555dd1b071c05c8b9e2c0a434c
--- /dev/null
+++ b/groundStation/scripts/take_off.sh
@@ -0,0 +1,15 @@
+#! /bin/bash
+
+#./setsource "T trim add" "summand 1" "zero" 0
+./setsource 14 0 38 0
+#./setparam "Throttle trim" 0 135000
+./setparam 13 0 135000
+
+sleep 2
+
+#./setsource "T trim add" "summand 1" "Altitude PID" "Correction"
+./setsource 14 0 8 0
+#./setparam "Throttle trim" 0 145000
+./setparam 13 0 150000
+#./setparam "Alt Setpoint" 0 -0.3
+./setparam 11 0 -0.5
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 */
diff --git a/quad/src/gen_diagram/network.dot b/quad/src/gen_diagram/network.dot
index 915326f30f671e5ec268f0f4237bbe14af111de1..174e1564fe62c01c56a092bd73e22a4efa945275 100644
--- a/quad/src/gen_diagram/network.dot
+++ b/quad/src/gen_diagram/network.dot
@@ -1,14 +1,14 @@
digraph G {
rankdir="LR"
"Roll PID"[shape=record
-label="<f0>Roll PID |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=15.000] |<f5> [Ki=0.000] |<f6> [Kd=0.200] |<f7> [alpha=0.000]"]
+label="<f0>Roll PID |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=35.000] |<f5> [Ki=0.000] |<f6> [Kd=1.000] |<f7> [alpha=0.880]"]
"Roll" -> "Roll PID":f1 [label="Constant"]
-"RC Roll" -> "Roll PID":f2 [label="Constant"]
+"Y Vel PID" -> "Roll PID":f2 [label="Correction"]
"Ts_IMU" -> "Roll PID":f3 [label="Constant"]
"Pitch PID"[shape=record
-label="<f0>Pitch PID |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=15.000] |<f5> [Ki=0.000] |<f6> [Kd=0.200] |<f7> [alpha=0.000]"]
+label="<f0>Pitch PID |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=35.000] |<f5> [Ki=0.000] |<f6> [Kd=1.000] |<f7> [alpha=0.880]"]
"Pitch trim add" -> "Pitch PID":f1 [label="Sum"]
-"RC Pitch" -> "Pitch PID":f2 [label="Constant"]
+"X Vel PID" -> "Pitch PID":f2 [label="Correction"]
"Ts_IMU" -> "Pitch PID":f3 [label="Constant"]
"Yaw PID"[shape=record
label="<f0>Yaw PID |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=2.600] |<f5> [Ki=0.000] |<f6> [Kd=0.000] |<f7> [alpha=0.000]"]
@@ -16,33 +16,33 @@ label="<f0>Yaw PID |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [K
"Yaw Setpoint" -> "Yaw PID":f2 [label="Constant"]
"Ts_VRPN" -> "Yaw PID":f3 [label="Constant"]
"Roll Rate PID"[shape=record
-label="<f0>Roll Rate PID |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=4600.000] |<f5> [Ki=0.000] |<f6> [Kd=550.000] |<f7> [alpha=0.000]"]
-"dPhi" -> "Roll Rate PID":f1 [label="Constant"]
+label="<f0>Roll Rate PID |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=3000.000] |<f5> [Ki=0.000] |<f6> [Kd=500.000] |<f7> [alpha=0.880]"]
+"Gyro X" -> "Roll Rate PID":f1 [label="Constant"]
"Roll PID" -> "Roll Rate PID":f2 [label="Correction"]
"Ts_IMU" -> "Roll Rate PID":f3 [label="Constant"]
"Pitch Rate PID"[shape=record
-label="<f0>Pitch Rate PID |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=4600.000] |<f5> [Ki=0.000] |<f6> [Kd=550.000] |<f7> [alpha=0.000]"]
-"dTheta" -> "Pitch Rate PID":f1 [label="Constant"]
+label="<f0>Pitch Rate PID |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=3000.000] |<f5> [Ki=0.000] |<f6> [Kd=500.000] |<f7> [alpha=0.880]"]
+"Gyro Y" -> "Pitch Rate PID":f1 [label="Constant"]
"Pitch PID" -> "Pitch Rate PID":f2 [label="Correction"]
"Ts_IMU" -> "Pitch Rate PID":f3 [label="Constant"]
"Yaw Rate PID"[shape=record
label="<f0>Yaw Rate PID |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=29700.000] |<f5> [Ki=0.000] |<f6> [Kd=0.000] |<f7> [alpha=0.000]"]
-"dPsi" -> "Yaw Rate PID":f1 [label="Constant"]
-"RC Yaw" -> "Yaw Rate PID":f2 [label="Constant"]
+"Gyro Z" -> "Yaw Rate PID":f1 [label="Constant"]
+"Yaw PID" -> "Yaw Rate PID":f2 [label="Correction"]
"Ts_IMU" -> "Yaw Rate PID":f3 [label="Constant"]
"X pos PID"[shape=record
-label="<f0>X pos PID |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=-0.015] |<f5> [Ki=-0.005] |<f6> [Kd=-0.030] |<f7> [alpha=0.000]"]
+label="<f0>X pos PID |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=-0.550] |<f5> [Ki=-0.007] |<f6> [Kd=0.000] |<f7> [alpha=0.000]"]
"VRPN X" -> "X pos PID":f1 [label="Constant"]
"X Setpoint" -> "X pos PID":f2 [label="Constant"]
"Ts_VRPN" -> "X pos PID":f3 [label="Constant"]
"Y pos PID"[shape=record
-label="<f0>Y pos PID |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=0.015] |<f5> [Ki=0.005] |<f6> [Kd=0.030] |<f7> [alpha=0.000]"]
+label="<f0>Y pos PID |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=0.550] |<f5> [Ki=0.007] |<f6> [Kd=0.000] |<f7> [alpha=0.000]"]
"VRPN Y" -> "Y pos PID":f1 [label="Constant"]
"Y Setpoint" -> "Y pos PID":f2 [label="Constant"]
"Ts_VRPN" -> "Y pos PID":f3 [label="Constant"]
"Altitude PID"[shape=record
-label="<f0>Altitude PID |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=9804.000] |<f5> [Ki=817.000] |<f6> [Kd=7353.000] |<f7> [alpha=0.000]"]
-"Lidar" -> "Altitude PID":f1 [label="Constant"]
+label="<f0>Altitude PID |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=-9804.000] |<f5> [Ki=-817.000] |<f6> [Kd=-7353.000] |<f7> [alpha=0.000]"]
+"VRPN Alt" -> "Altitude PID":f1 [label="Constant"]
"Alt Setpoint" -> "Altitude PID":f2 [label="Constant"]
"Ts_VRPN" -> "Altitude PID":f3 [label="Constant"]
"X Setpoint"[shape=record
@@ -65,20 +65,18 @@ label="<f0>Pitch |<f1> [Constant=0.000]"]
label="<f0>Roll |<f1> [Constant=0.000]"]
"Yaw"[shape=record
label="<f0>Yaw |<f1> [Constant=0.000]"]
-"Lidar"[shape=record
-label="<f0>Lidar |<f1> [Constant=0.000]"]
"Pitch trim"[shape=record
-label="<f0>Pitch trim |<f1> [Constant=0.020]"]
+label="<f0>Pitch trim |<f1> [Constant=0.045]"]
"Pitch trim add"[shape=record
label="<f0>Pitch trim add |<f1> --\>Summand 1 |<f2> --\>Summand 2"]
"Pitch trim" -> "Pitch trim add":f1 [label="Constant"]
"Pitch" -> "Pitch trim add":f2 [label="Constant"]
-"dTheta"[shape=record
-label="<f0>dTheta |<f1> [Constant=0.000]"]
-"dPhi"[shape=record
-label="<f0>dPhi |<f1> [Constant=0.000]"]
-"dPsi"[shape=record
-label="<f0>dPsi |<f1> [Constant=0.000]"]
+"Gyro Y"[shape=record
+label="<f0>Gyro Y |<f1> [Constant=0.000]"]
+"Gyro X"[shape=record
+label="<f0>Gyro X |<f1> [Constant=0.000]"]
+"Gyro Z"[shape=record
+label="<f0>Gyro Z |<f1> [Constant=0.000]"]
"P PWM Clamp"[shape=record
label="<f0>P PWM Clamp |<f1> --\>Bounds in |<f2> [Min=-20000.000] |<f3> [Max=20000.000]"]
"Pitch Rate PID" -> "P PWM Clamp":f1 [label="Correction"]
@@ -106,35 +104,23 @@ label="<f0>RC Roll |<f1> [Constant=0.000]"]
label="<f0>RC Yaw |<f1> [Constant=0.000]"]
"RC Throttle"[shape=record
label="<f0>RC Throttle |<f1> [Constant=0.000]"]
-"Signal Mixer"[shape=record
-label="<f0>Signal Mixer |<f1> --\>Throttle |<f2> --\>Pitch |<f3> --\>Roll |<f4> --\>Yaw"]
-"RC Throttle" -> "Signal Mixer":f1 [label="Constant"]
-"P PWM Clamp" -> "Signal Mixer":f2 [label="Bounded"]
-"R PWM Clamp" -> "Signal Mixer":f3 [label="Bounded"]
-"Y PWM Clamp" -> "Signal Mixer":f4 [label="Bounded"]
-"Ts_IMU"[shape=record
-label="<f0>Ts_IMU |<f1> [Constant=0.005]"]
-"Ts_VRPN"[shape=record
-label="<f0>Ts_VRPN |<f1> [Constant=0.040]"]
-"zero"[shape=record
-label="<f0>zero |<f1> [Constant=0.000]"]
"X Vel PID"[shape=record
-label="<f0>X Vel PID |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=0.000] |<f5> [Ki=0.000] |<f6> [Kd=0.000] |<f7> [alpha=0.000]"]
+label="<f0>X Vel PID |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=0.100] |<f5> [Ki=0.000] |<f6> [Kd=0.020] |<f7> [alpha=0.880]"]
"X Vel" -> "X Vel PID":f1 [label="Correction"]
"X Vel Clamp" -> "X Vel PID":f2 [label="Bounded"]
"Ts_VRPN" -> "X Vel PID":f3 [label="Constant"]
"Y Vel PID"[shape=record
-label="<f0>Y Vel PID |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=0.000] |<f5> [Ki=0.000] |<f6> [Kd=0.000] |<f7> [alpha=0.000]"]
+label="<f0>Y Vel PID |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=-0.100] |<f5> [Ki=0.000] |<f6> [Kd=-0.020] |<f7> [alpha=0.880]"]
"Y Vel" -> "Y Vel PID":f1 [label="Correction"]
"Y vel Clamp" -> "Y Vel PID":f2 [label="Bounded"]
"Ts_VRPN" -> "Y Vel PID":f3 [label="Constant"]
"X Vel"[shape=record
-label="<f0>X Vel |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=0.000] |<f5> [Ki=0.000] |<f6> [Kd=1.000] |<f7> [alpha=0.000]"]
+label="<f0>X Vel |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=0.000] |<f5> [Ki=0.000] |<f6> [Kd=-1.000] |<f7> [alpha=0.000]"]
"VRPN X" -> "X Vel":f1 [label="Constant"]
"zero" -> "X Vel":f2 [label="Constant"]
"Ts_VRPN" -> "X Vel":f3 [label="Constant"]
"Y Vel"[shape=record
-label="<f0>Y Vel |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=0.000] |<f5> [Ki=0.000] |<f6> [Kd=1.000] |<f7> [alpha=0.000]"]
+label="<f0>Y Vel |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=0.000] |<f5> [Ki=0.000] |<f6> [Kd=-1.000] |<f7> [alpha=0.000]"]
"VRPN Y" -> "Y Vel":f1 [label="Constant"]
"zero" -> "Y Vel":f2 [label="Constant"]
"Ts_VRPN" -> "Y Vel":f3 [label="Constant"]
@@ -144,8 +130,16 @@ label="<f0>X Vel Clamp |<f1> --\>Bounds in |<f2> [Min=-2.000] |<f3> [Max=2.000]
"Y vel Clamp"[shape=record
label="<f0>Y vel Clamp |<f1> --\>Bounds in |<f2> [Min=-2.000] |<f3> [Max=2.000]"]
"Y pos PID" -> "Y vel Clamp":f1 [label="Correction"]
-"X Stick Gain"[shape=record
-label="<f0>X Stick Gain |<f1> --\>Input |<f2> [Gain=5.000]"]
-"Y Stick Gain"[shape=record
-label="<f0>Y Stick Gain |<f1> --\>Input |<f2> [Gain=-5.000]"]
+"Signal Mixer"[shape=record
+label="<f0>Signal Mixer |<f1> --\>Throttle |<f2> --\>Pitch |<f3> --\>Roll |<f4> --\>Yaw"]
+"T trim add" -> "Signal Mixer":f1 [label="Sum"]
+"P PWM Clamp" -> "Signal Mixer":f2 [label="Bounded"]
+"R PWM Clamp" -> "Signal Mixer":f3 [label="Bounded"]
+"Y PWM Clamp" -> "Signal Mixer":f4 [label="Bounded"]
+"Ts_IMU"[shape=record
+label="<f0>Ts_IMU |<f1> [Constant=0.005]"]
+"Ts_VRPN"[shape=record
+label="<f0>Ts_VRPN |<f1> [Constant=0.040]"]
+"zero"[shape=record
+label="<f0>zero |<f1> [Constant=0.000]"]
}
\ No newline at end of file
diff --git a/quad/src/gen_diagram/network.png b/quad/src/gen_diagram/network.png
index 77d9834a8db57a5a81833f796d81f0e17b134d0a..c12bb2905d8e97d2c51cfb7c90f763352b264910 100644
Binary files a/quad/src/gen_diagram/network.png and b/quad/src/gen_diagram/network.png differ
diff --git a/quad/src/graph_blocks/node_pid.c b/quad/src/graph_blocks/node_pid.c
index e776b7ed28e9295c9f30be9adb104ae3e9598647..74ee28ab93f7817bdd9e59c61136b6b67d03c525 100644
--- a/quad/src/graph_blocks/node_pid.c
+++ b/quad/src/graph_blocks/node_pid.c
@@ -48,7 +48,7 @@ static void pid_computation(void *state, const double* params, const double *inp
if (fabs(params[PID_KI]) <= FLT_EPSILON) {
pid_state->acc_error = 0;
} else {
- pid_state->acc_error += error;
+ pid_state->acc_error += (error * inputs[PID_DT]);
}
if (pid_state->just_reset) {
@@ -60,7 +60,7 @@ static void pid_computation(void *state, const double* params, const double *inp
// Compute each term's contribution
P = params[PID_KP] * error;
- I = params[PID_KI] * pid_state->acc_error * inputs[PID_DT];
+ I = params[PID_KI] * pid_state->acc_error;
// Low-pass filter on derivative
double change_in_value = inputs[PID_CUR_POINT] - pid_state->prev_val;
double term1 = params[PID_ALPHA] * pid_state->last_filtered;
diff --git a/quad/src/quad_app/PID.h b/quad/src/quad_app/PID.h
index 5a83225b0b9813e4d311bf61c8f1eae89c7f24a0..faf5579f090e7bfe424725ce65a762e7fa9a8e9b 100644
--- a/quad/src/quad_app/PID.h
+++ b/quad/src/quad_app/PID.h
@@ -10,16 +10,10 @@
#include "type_def.h"
-// Yaw constants
-
-// when using units of degrees
-//#define YAW_ANGULAR_VELOCITY_KP 40.0f
-//#define YAW_ANGULAR_VELOCITY_KI 0.0f
-//#define YAW_ANGULAR_VELOCITY_KD 0.0f
-//#define YAW_ANGLE_KP 2.6f
-//#define YAW_ANGLE_KI 0.0f
-//#define YAW_ANGLE_KD 0.0f
+#define VEL_CLAMP 2.0
+#define PITCH_TRIM 0.045
+// Yaw constants
// when using units of radians
#define YAW_ANGULAR_VELOCITY_KP 29700//200.0f * 2292.0f
#define YAW_ANGULAR_VELOCITY_KI 0.0f
@@ -28,59 +22,49 @@
#define YAW_ANGLE_KI 0.0f
#define YAW_ANGLE_KD 0.0f
-// Roll constants
-//#define ROLL_ANGULAR_VELOCITY_KP 0.95f
-//#define ROLL_ANGULAR_VELOCITY_KI 0.0f
-//#define ROLL_ANGULAR_VELOCITY_KD 0.13f//0.4f//0.7f
-//#define ROLL_ANGLE_KP 17.0f //9.0f
-//#define ROLL_ANGLE_KI 0.0f
-//#define ROLL_ANGLE_KD 0.3f // 0.2f
-//#define YPOS_KP 0.0f
-//#define YPOS_KI 0.0f
-//#define YPOS_KD 0.0f
+
// when using units of radians
-#define ROLL_ANGULAR_VELOCITY_KP 100.0f*46.0f//102.0f*46.0f//9384.0f//204.0f * 46.0f
+#define ROLL_ANGULAR_VELOCITY_KP 3000
#define ROLL_ANGULAR_VELOCITY_KI 0.0f
-#define ROLL_ANGULAR_VELOCITY_KD 100.f*5.5f//102.0f*6.8f//1387.2//204.0f * 6.8f
-#define ROLL_ANGLE_KP 15.0f
+#define ROLL_ANGULAR_VELOCITY_KD 500
+#define ROLL_ANGULAR_VELOCITY_ALPHA 0.88
+#define ROLL_ANGLE_KP 35
#define ROLL_ANGLE_KI 0.0f
-#define ROLL_ANGLE_KD 0.2f
-#define YPOS_KP 0.015f
-#define YPOS_KI 0.005f
-#define YPOS_KD 0.03f
+#define ROLL_ANGLE_KD 1.0
+#define ROLL_ANGLE_ALPHA 0.88
+#define YVEL_KP 0.1
+#define YVEL_KD 0.02
+#define YVEL_ALPHA 0.88
+#define YPOS_KP 0.55
+#define YPOS_KI 0.0075
+#define YPOS_KD 0.0
#define YPOS_ALPHA 0
//Pitch constants
-
-// when using units of degrees
-//#define PITCH_ANGULAR_VELOCITY_KP 0.95f
-//#define PITCH_ANGULAR_VELOCITY_KI 0.0f
-//#define PITCH_ANGULAR_VELOCITY_KD 0.13f//0.35f//0.7f
-//#define PITCH_ANGLE_KP 17.0f // 7.2f
-//#define PITCH_ANGLE_KI 0.0f
-//#define PITCH_ANGLE_KD 0.3f //0.3f
-//#define XPOS_KP 40.0f
-//#define XPOS_KI 0.0f
-//#define XPOS_KD 10.0f//0.015f
-
// when using units of radians
-#define PITCH_ANGULAR_VELOCITY_KP 100.0f*46.0f//101.0f*46.0f//9292.0f//202.0f * 46.0f
+#define PITCH_ANGULAR_VELOCITY_KP 3000
#define PITCH_ANGULAR_VELOCITY_KI 0.0f
-#define PITCH_ANGULAR_VELOCITY_KD 100.0f*5.5f//101.0f*6.8f//1373.6//202.0f * 6.8f
-#define PITCH_ANGLE_KP 15.0f
+#define PITCH_ANGULAR_VELOCITY_KD 500
+#define PITCH_ANGULAR_VELOCITY_ALPHA 0.88
+#define PITCH_ANGLE_KP 35
#define PITCH_ANGLE_KI 0.0f
-#define PITCH_ANGLE_KD 0.2f
-#define XPOS_KP -0.015f
-#define XPOS_KI -0.005f
-#define XPOS_KD -0.03f
+#define PITCH_ANGLE_KD 1.0
+#define PITCH_ANGLE_ALPHA 0.88
+#define XVEL_KP -0.1
+#define XVEL_KD -0.02
+#define XVEL_ALPHA 0.88
+#define XPOS_KP 0.55
+#define XPOS_KI 0.0075
+#define XPOS_KD 0.0
#define XPOS_ALPHA 0
//Throttle constants
-#define ALT_ZPOS_KP 9804.0f
-#define ALT_ZPOS_KI 817.0f
-#define ALT_ZPOS_KD 7353.0f
+#define ALT_ZPOS_KP -9804.0f
+#define ALT_ZPOS_KI -817.0f
+#define ALT_ZPOS_KD -7353.0f
+#define ALT_ZPOS_ALPHA 0.88
#endif /* PID_H_ */
diff --git a/quad/src/quad_app/control_algorithm.c b/quad/src/quad_app/control_algorithm.c
index 7c1af8196811cdb7eaf7bfa4203306401fcb0a0a..345ed847f65d9b67e5404d045d37023b8abce987 100644
--- a/quad/src/quad_app/control_algorithm.c
+++ b/quad/src/quad_app/control_algorithm.c
@@ -67,13 +67,9 @@ int control_algorithm_init(parameter_t * ps)
ps->pitch_trim_add = graph_add_defined_block(graph, BLOCK_ADD, "Pitch trim add");
// Yaw angular velocity PID
- // theta_dot is the angular velocity about the y-axis
- // phi_dot is the angular velocity about the x-axis
- // psi_dot is the angular velocity about the z-axis
- // These are calculated from using the gimbal equations
- ps->theta_dot = graph_add_defined_block(graph, BLOCK_CONSTANT, "dTheta");
- ps->phi_dot = graph_add_defined_block(graph, BLOCK_CONSTANT, "dPhi");
- ps->psi_dot = graph_add_defined_block(graph, BLOCK_CONSTANT, "dPsi");
+ ps->gyro_y = graph_add_defined_block(graph, BLOCK_CONSTANT, "Gyro Y");
+ ps->gyro_x = graph_add_defined_block(graph, BLOCK_CONSTANT, "Gyro X");
+ ps->gyro_z = graph_add_defined_block(graph, BLOCK_CONSTANT, "Gyro Z");
ps->clamp_d_pwmP = graph_add_defined_block(graph, BLOCK_BOUNDS, "P PWM Clamp");
ps->clamp_d_pwmR = graph_add_defined_block(graph, BLOCK_BOUNDS, "R PWM Clamp");
ps->clamp_d_pwmY = graph_add_defined_block(graph, BLOCK_BOUNDS, "Y PWM Clamp");
@@ -91,18 +87,30 @@ int control_algorithm_init(parameter_t * ps)
ps->rc_yaw = graph_add_defined_block(graph, BLOCK_CONSTANT, "RC Yaw");
ps->rc_throttle = graph_add_defined_block(graph, BLOCK_CONSTANT, "RC Throttle");
+ // Create blocks for velocity controllers
+ ps->x_vel_pid = graph_add_defined_block(graph, BLOCK_PID, "X Vel PID");
+ ps->y_vel_pid = graph_add_defined_block(graph, BLOCK_PID, "Y Vel PID");
+ ps->x_vel = graph_add_defined_block(graph, BLOCK_PID, "X Vel");
+ ps->y_vel = graph_add_defined_block(graph, BLOCK_PID, "Y Vel");
+ ps->x_vel_clamp = graph_add_defined_block(graph, BLOCK_BOUNDS, "X Vel Clamp");
+ ps->y_vel_clamp = graph_add_defined_block(graph, BLOCK_BOUNDS, "Y vel Clamp");
+
ps->mixer = graph_add_defined_block(graph, BLOCK_MIXER, "Signal Mixer");
ps->angle_time = graph_add_defined_block(graph, BLOCK_CONSTANT, "Ts_IMU");
ps->pos_time = graph_add_defined_block(graph, BLOCK_CONSTANT, "Ts_VRPN");
+ // Useful zero block
+ int zero_block = graph_add_defined_block(graph, BLOCK_CONSTANT, "zero");
+ graph_set_param_val(graph, zero_block, CONST_SET, 0);
+
// Connect pitch PID chain
// Trims
graph_set_source(graph, ps->pitch_trim_add, ADD_SUMMAND1, ps->pitch_trim, CONST_VAL);
graph_set_source(graph, ps->pitch_trim_add, ADD_SUMMAND2, ps->cur_pitch, CONST_VAL);
// Controllers
graph_set_source(graph, ps->pitch_r_pid, PID_SETPOINT, ps->pitch_pid, PID_CORRECTION);
- graph_set_source(graph, ps->pitch_r_pid, PID_CUR_POINT, ps->theta_dot, CONST_VAL);
+ graph_set_source(graph, ps->pitch_r_pid, PID_CUR_POINT, ps->gyro_y, CONST_VAL);
graph_set_source(graph, ps->pitch_r_pid, PID_DT, ps->angle_time, CONST_VAL);
graph_set_source(graph, ps->pitch_pid, PID_CUR_POINT, ps->pitch_trim_add, ADD_SUM);
//graph_set_source(graph, ps->pitch_pid, PID_CUR_POINT, ps->vrpn_pitch, CONST_VAL);
@@ -110,7 +118,7 @@ int control_algorithm_init(parameter_t * ps)
// Connect roll PID chain
graph_set_source(graph, ps->roll_r_pid, PID_SETPOINT, ps->roll_pid, PID_CORRECTION);
- graph_set_source(graph, ps->roll_r_pid, PID_CUR_POINT, ps->phi_dot, CONST_VAL);
+ graph_set_source(graph, ps->roll_r_pid, PID_CUR_POINT, ps->gyro_x, CONST_VAL);
graph_set_source(graph, ps->roll_r_pid, PID_DT, ps->angle_time, CONST_VAL);
graph_set_source(graph, ps->roll_pid, PID_CUR_POINT, ps->cur_roll, CONST_VAL);
//graph_set_source(graph, ps->roll_pid, PID_CUR_POINT, ps->vrpn_roll, CONST_VAL);
@@ -118,25 +126,49 @@ int control_algorithm_init(parameter_t * ps)
// Connect yaw PID chain
graph_set_source(graph, ps->yaw_r_pid, PID_SETPOINT, ps->rc_yaw, PID_CORRECTION);
- graph_set_source(graph, ps->yaw_r_pid, PID_CUR_POINT, ps->psi_dot, CONST_VAL);
+ graph_set_source(graph, ps->yaw_r_pid, PID_CUR_POINT, ps->gyro_z, CONST_VAL);
graph_set_source(graph, ps->yaw_r_pid, PID_DT, ps->angle_time, CONST_VAL);
+ // X velocity PID
+ // Use a PID block to compute the derivative
+ graph_set_param_val(graph, ps->x_vel, PID_KD, -1);
+ graph_set_source(graph, ps->x_vel, PID_DT, ps->pos_time, CONST_VAL);
+ graph_set_source(graph, ps->x_vel, PID_CUR_POINT, ps->vrpn_x, CONST_VAL);
+ graph_set_source(graph, ps->x_vel, PID_SETPOINT, zero_block, CONST_VAL);
+ // Connect velocity PID itself
+ graph_set_source(graph, ps->x_vel_pid, PID_DT, ps->pos_time, CONST_VAL);
+ graph_set_source(graph, ps->x_vel_pid, PID_CUR_POINT, ps->x_vel, PID_CORRECTION);
+ graph_set_source(graph, ps->x_vel_pid, PID_SETPOINT, ps->x_vel_clamp, BOUNDS_OUT);
+ graph_set_source(graph, ps->x_vel_clamp, BOUNDS_IN, ps->x_pos_pid, PID_CORRECTION);
+
+ // Y velocity PID
+ // Use a PID block to compute the derivative
+ graph_set_param_val(graph, ps->y_vel, PID_KD, -1);
+ graph_set_source(graph, ps->y_vel, PID_DT, ps->pos_time, CONST_VAL);
+ graph_set_source(graph, ps->y_vel, PID_CUR_POINT, ps->vrpn_y, CONST_VAL);
+ graph_set_source(graph, ps->y_vel, PID_SETPOINT, zero_block, CONST_VAL);
+ // Connect velocity PID itself
+ graph_set_source(graph, ps->y_vel_pid, PID_DT, ps->pos_time, CONST_VAL);
+ graph_set_source(graph, ps->y_vel_pid, PID_CUR_POINT, ps->y_vel, PID_CORRECTION);
+ graph_set_source(graph, ps->y_vel_pid, PID_SETPOINT, ps->y_vel_clamp, BOUNDS_OUT);
+ graph_set_source(graph, ps->y_vel_clamp, BOUNDS_IN, ps->y_pos_pid, PID_CORRECTION);
- // X autonomous
+ // X position
graph_set_source(graph, ps->x_pos_pid, PID_DT, ps->pos_time, CONST_VAL);
graph_set_source(graph, ps->x_pos_pid, PID_CUR_POINT, ps->vrpn_x, CONST_VAL);
graph_set_source(graph, ps->x_pos_pid, PID_SETPOINT, ps->x_set, CONST_VAL);
- // Y autonomous
+ // Y position
graph_set_source(graph, ps->y_pos_pid, PID_DT, ps->pos_time, CONST_VAL);
graph_set_source(graph, ps->y_pos_pid, PID_CUR_POINT, ps->vrpn_y, CONST_VAL);
graph_set_source(graph, ps->y_pos_pid, PID_SETPOINT, ps->y_set, CONST_VAL);
+
// Alt autonomous
graph_set_source(graph, ps->alt_pid, PID_DT, ps->angle_time, CONST_VAL);
graph_set_source(graph, ps->alt_pid, PID_CUR_POINT, ps->lidar, CONST_VAL);
graph_set_source(graph, ps->alt_pid, PID_SETPOINT, ps->alt_set, CONST_VAL);
graph_set_source(graph, ps->throttle_trim_add, ADD_SUMMAND1, ps->alt_pid, PID_CORRECTION);
graph_set_source(graph, ps->throttle_trim_add, ADD_SUMMAND2, ps->throttle_trim, CONST_VAL);
- // Yaw autonomous
+ // Yaw angle
graph_set_source(graph, ps->yaw_pid, PID_DT, ps->pos_time, CONST_VAL);
graph_set_source(graph, ps->yaw_pid, PID_SETPOINT, ps->yaw_set, CONST_VAL);
graph_set_source(graph, ps->yaw_pid, PID_CUR_POINT, ps->cur_yaw, CONST_VAL);
@@ -156,17 +188,21 @@ int control_algorithm_init(parameter_t * ps)
graph_set_param_val(graph, ps->pitch_pid, PID_KP, PITCH_ANGLE_KP);
graph_set_param_val(graph, ps->pitch_pid, PID_KI, PITCH_ANGLE_KI);
graph_set_param_val(graph, ps->pitch_pid, PID_KD, PITCH_ANGLE_KD);
+ graph_set_param_val(graph, ps->pitch_pid, PID_ALPHA, PITCH_ANGLE_ALPHA);
graph_set_param_val(graph, ps->pitch_r_pid, PID_KP, PITCH_ANGULAR_VELOCITY_KP);
graph_set_param_val(graph, ps->pitch_r_pid, PID_KI, PITCH_ANGULAR_VELOCITY_KI);
graph_set_param_val(graph, ps->pitch_r_pid, PID_KD, PITCH_ANGULAR_VELOCITY_KD);
+ graph_set_param_val(graph, ps->pitch_r_pid, PID_ALPHA, PITCH_ANGULAR_VELOCITY_ALPHA);
// Set roll PID constants
graph_set_param_val(graph, ps->roll_pid, PID_KP, ROLL_ANGLE_KP);
graph_set_param_val(graph, ps->roll_pid, PID_KI, ROLL_ANGLE_KI);
graph_set_param_val(graph, ps->roll_pid, PID_KD, ROLL_ANGLE_KD);
+ graph_set_param_val(graph, ps->roll_pid, PID_ALPHA, ROLL_ANGLE_ALPHA);
graph_set_param_val(graph, ps->roll_r_pid, PID_KP, ROLL_ANGULAR_VELOCITY_KP);
graph_set_param_val(graph, ps->roll_r_pid, PID_KI, ROLL_ANGULAR_VELOCITY_KI);
graph_set_param_val(graph, ps->roll_r_pid, PID_KD, ROLL_ANGULAR_VELOCITY_KD);
+ graph_set_param_val(graph, ps->roll_r_pid, PID_ALPHA, ROLL_ANGULAR_VELOCITY_ALPHA);
// Set yaw PID constants
graph_set_param_val(graph, ps->yaw_pid, PID_KP, YAW_ANGLE_KP);
@@ -176,6 +212,15 @@ int control_algorithm_init(parameter_t * ps)
graph_set_param_val(graph, ps->yaw_r_pid, PID_KI, YAW_ANGULAR_VELOCITY_KI);
graph_set_param_val(graph, ps->yaw_r_pid, PID_KD, YAW_ANGULAR_VELOCITY_KD);
+ // Set velocity constants
+ graph_set_param_val(graph, ps->x_vel_pid, PID_KP, XVEL_KP);
+ graph_set_param_val(graph, ps->x_vel_pid, PID_KD, XVEL_KD);
+ graph_set_param_val(graph, ps->y_vel_pid, PID_KP, YVEL_KP);
+ graph_set_param_val(graph, ps->y_vel_pid, PID_KD, YVEL_KD);
+ // Differentiators
+ graph_set_param_val(graph, ps->x_vel, PID_ALPHA, XVEL_ALPHA);
+ graph_set_param_val(graph, ps->y_vel, PID_ALPHA, YVEL_ALPHA);
+
// Set X/Y/Alt constants
graph_set_param_val(graph, ps->x_pos_pid, PID_KP, XPOS_KP);
graph_set_param_val(graph, ps->x_pos_pid, PID_KI, XPOS_KI);
@@ -188,6 +233,7 @@ int control_algorithm_init(parameter_t * ps)
graph_set_param_val(graph, ps->alt_pid, PID_KP, ALT_ZPOS_KP);
graph_set_param_val(graph, ps->alt_pid, PID_KI, ALT_ZPOS_KI);
graph_set_param_val(graph, ps->alt_pid, PID_KD, ALT_ZPOS_KD);
+ graph_set_param_val(graph, ps->alt_pid, PID_ALPHA, ALT_ZPOS_ALPHA);
// Set PWM difference clamping limits
graph_set_param_val(graph, ps->clamp_d_pwmP, BOUNDS_MIN, -PWM_DIFF_BOUNDS);
@@ -197,60 +243,14 @@ int control_algorithm_init(parameter_t * ps)
graph_set_param_val(graph, ps->clamp_d_pwmY, BOUNDS_MIN, -PWM_DIFF_BOUNDS);
graph_set_param_val(graph, ps->clamp_d_pwmY, BOUNDS_MAX, PWM_DIFF_BOUNDS);
-
- // Velocity stuff
- int zero_block = graph_add_defined_block(graph, BLOCK_CONSTANT, "zero");
-
- ps->x_vel_pid = graph_add_defined_block(graph, BLOCK_PID, "X Vel PID");
- ps->y_vel_pid = graph_add_defined_block(graph, BLOCK_PID, "Y Vel PID");
- ps->x_vel = graph_add_defined_block(graph, BLOCK_PID, "X Vel");
- ps->y_vel = graph_add_defined_block(graph, BLOCK_PID, "Y Vel");
- ps->x_vel_clamp = graph_add_defined_block(graph, BLOCK_BOUNDS, "X Vel Clamp");
- ps->y_vel_clamp = graph_add_defined_block(graph, BLOCK_BOUNDS, "Y vel Clamp");
- ps->vel_x_gain = graph_add_defined_block(graph, BLOCK_GAIN, "X Stick Gain");
- ps->vel_y_gain = graph_add_defined_block(graph, BLOCK_GAIN, "Y Stick Gain");
-
- graph_set_source(graph, ps->x_vel_pid, PID_DT, ps->pos_time, CONST_VAL);
- graph_set_source(graph, ps->y_vel_pid, PID_DT, ps->pos_time, CONST_VAL);
- graph_set_source(graph, ps->x_vel, PID_DT, ps->pos_time, CONST_VAL);
- graph_set_source(graph, ps->y_vel, PID_DT, ps->pos_time, CONST_VAL);
-
- graph_set_source(graph, ps->x_vel_pid, PID_SETPOINT, ps->x_vel, PID_CORRECTION);
- graph_set_source(graph, ps->y_vel_pid, PID_SETPOINT, ps->y_vel, PID_CORRECTION);
- graph_set_source(graph, ps->x_vel_pid, PID_CUR_POINT, ps->x_vel, PID_CORRECTION);
- graph_set_source(graph, ps->y_vel_pid, PID_CUR_POINT, ps->y_vel, PID_CORRECTION);
-
- graph_set_source(graph, ps->x_vel, PID_CUR_POINT, ps->vrpn_x, CONST_VAL);
- graph_set_source(graph, ps->y_vel, PID_CUR_POINT, ps->vrpn_y, CONST_VAL);
- graph_set_source(graph, ps->x_vel, PID_SETPOINT, zero_block, CONST_VAL);
- graph_set_source(graph, ps->y_vel, PID_SETPOINT, zero_block, CONST_VAL);
-
- graph_set_source(graph, ps->x_vel_pid, PID_SETPOINT, ps->x_vel_clamp, GAIN_RESULT);
- graph_set_source(graph, ps->y_vel_pid, PID_SETPOINT, ps->y_vel_clamp, GAIN_RESULT);
-
- graph_set_source(graph, ps->x_vel_clamp, BOUNDS_IN, ps->x_pos_pid, PID_CORRECTION);
- graph_set_source(graph, ps->y_vel_clamp, BOUNDS_IN, ps->y_pos_pid, PID_CORRECTION);
- //graph_set_source(graph, ps->vel_x_gain, GAIN_INPUT, ps->rc_pitch, CONST_VAL);
- //graph_set_source(graph, ps->vel_y_gain, GAIN_INPUT, ps->rc_roll, CONST_VAL);
- //graph_set_source(graph, ps->x_vel_clamp, BOUNDS_IN, ps->vel_x_gain, GAIN_RESULT);
- //graph_set_source(graph, ps->y_vel_clamp, BOUNDS_IN, ps->vel_y_gain, GAIN_RESULT);
-
- float vel_clamps = 2;
- graph_set_param_val(graph, ps->x_vel_clamp, BOUNDS_MIN, -vel_clamps);
- graph_set_param_val(graph, ps->x_vel_clamp, BOUNDS_MAX, vel_clamps);
- graph_set_param_val(graph, ps->y_vel_clamp, BOUNDS_MIN, -vel_clamps);
- graph_set_param_val(graph, ps->y_vel_clamp, BOUNDS_MAX, vel_clamps);
-
-
- graph_set_param_val(graph, ps->x_vel, PID_KD, 1);
- graph_set_param_val(graph, ps->y_vel, PID_KD, 1);
-
- graph_set_param_val(graph, ps->vel_x_gain, GAIN_GAIN, 5);
- graph_set_param_val(graph, ps->vel_y_gain, GAIN_GAIN, -5);
-
+ // Set velocity clamping limits
+ graph_set_param_val(graph, ps->x_vel_clamp, BOUNDS_MIN, -VEL_CLAMP);
+ graph_set_param_val(graph, ps->x_vel_clamp, BOUNDS_MAX, VEL_CLAMP);
+ graph_set_param_val(graph, ps->y_vel_clamp, BOUNDS_MIN, -VEL_CLAMP);
+ graph_set_param_val(graph, ps->y_vel_clamp, BOUNDS_MAX, VEL_CLAMP);
// Set trims
- graph_set_param_val(graph, ps->pitch_trim, CONST_SET, 0.02);
+ graph_set_param_val(graph, ps->pitch_trim, CONST_SET, PITCH_TRIM);
// Initial value for sampling periods
graph_set_param_val(graph, ps->pos_time, CONST_SET, 0.04);
@@ -341,12 +341,9 @@ int control_algorithm_init(parameter_t * ps)
// Sensor values
graph_set_param_val(graph, ps->cur_pitch, CONST_SET, sensor_struct->pitch_angle_filtered);
graph_set_param_val(graph, ps->cur_roll, CONST_SET, sensor_struct->roll_angle_filtered);
- //graph_set_param_val(graph, ps->theta_dot, CONST_SET, sensor_struct->theta_dot);
- //graph_set_param_val(graph, ps->phi_dot, CONST_SET, sensor_struct->phi_dot);
- //graph_set_param_val(graph, ps->psi_dot, CONST_SET, sensor_struct->psi_dot);
- graph_set_param_val(graph, ps->theta_dot, CONST_SET, sensor_struct->gyr_y);
- graph_set_param_val(graph, ps->phi_dot, CONST_SET, sensor_struct->gyr_x);
- graph_set_param_val(graph, ps->psi_dot, CONST_SET, sensor_struct->gyr_z);
+ graph_set_param_val(graph, ps->gyro_y, CONST_SET, sensor_struct->gyr_y);
+ graph_set_param_val(graph, ps->gyro_x, CONST_SET, sensor_struct->gyr_x);
+ graph_set_param_val(graph, ps->gyro_z, CONST_SET, sensor_struct->gyr_z);
graph_set_param_val(graph, ps->lidar, CONST_SET, sensor_struct->lidar_altitude);
// RC values
@@ -363,9 +360,9 @@ int control_algorithm_init(parameter_t * ps)
//memcpy(raw_actuator_struct->controller_corrected_motor_commands, user_input_struct->rc_commands, sizeof(int) * 6);
// don't use the PID corrections if the throttle is less than about 10% of its range
-
- // re-enable the check for AUTO_FLIGHT_MODE when autonomous is fully enabled!!!
- if((user_input_struct->rc_commands[THROTTLE] > 118000))
+ // unless we are in autonomous
+ if((user_input_struct->rc_commands[THROTTLE] > 118000) ||
+ user_defined_struct->flight_mode == AUTO_FLIGHT_MODE)
{
//THROTTLE
diff --git a/quad/src/quad_app/hw_iface.h b/quad/src/quad_app/hw_iface.h
index bde0aa3f8170f465e8ab33be4576804850abfee8..48b3d0cf7f3cf26a0931c79e2b9c656dabc608ce 100644
--- a/quad/src/quad_app/hw_iface.h
+++ b/quad/src/quad_app/hw_iface.h
@@ -10,8 +10,8 @@
* NOTE:
* If you wound up here after following some IDE function declaration trail,
* you've hit the end of the application layer. Go to the location of the
- * hardware layer appropriate for your circumstance:p
- * ../../xsdk_worksapce/modular_quad_pid -> running quad_app on the Zybo
+ * hardware layer appropriate for your circumstance:
+ * ../../xsdk_worksapce/real_quad -> running quad_app on the Zybo
* ../virt_quad -> running quad_app in a Unix environment
*/
diff --git a/quad/src/quad_app/initialize_components.c b/quad/src/quad_app/initialize_components.c
index 967eaa6294b342641fc1dcdfb0b3327ea4490427..e476fd633eb9ecce40eb77af6570eac63462833a 100644
--- a/quad/src/quad_app/initialize_components.c
+++ b/quad/src/quad_app/initialize_components.c
@@ -21,14 +21,12 @@ int protection_loops(modular_structs_t *structs)
struct PWMInputDriver *pwm_inputs = &structs->hardware_struct.pwm_inputs;
read_rec_all(pwm_inputs, rc_commands);
-
- // wait for RC receiver to connect to transmitter
- while(rc_commands[THROTTLE] < 75000)
- read_rec_all(pwm_inputs, rc_commands);
-
- // wait until throttle is low and the gear switch is engaged (so you don't immediately break out of the main loop below)
- // also wait for the flight mode to be set to manual
- while(rc_commands[THROTTLE] > 125000 || read_kill(rc_commands[GEAR]) || !read_flap(rc_commands[FLAP])) {
+
+ while(rc_commands[THROTTLE] < 75000 || // wait for RC receiver to connect to transmitter
+ rc_commands[THROTTLE] > 125000 || // Wait until throttle is low
+ read_kill(rc_commands[GEAR]) || // Wait until gear switch is engaged (so you don't immediately break out of the main loop below)
+ !read_flap(rc_commands[FLAP])) // Wait for the flight mode to be set to manual
+ {
process_received(structs);
read_rec_all(pwm_inputs, rc_commands);
}
diff --git a/quad/src/quad_app/type_def.h b/quad/src/quad_app/type_def.h
index d77d8eb938a13f1a64e44aedf838c395ecabe667..80e676956b648a9cb9bd6e528ca96dc147d25c3a 100644
--- a/quad/src/quad_app/type_def.h
+++ b/quad/src/quad_app/type_def.h
@@ -327,9 +327,9 @@ typedef struct parameter_t {
int cur_pitch;
int cur_roll;
int cur_yaw;
- int theta_dot;
- int phi_dot;
- int psi_dot;
+ int gyro_y;
+ int gyro_x;
+ int gyro_z;
int lidar;
// VRPN blocks
int vrpn_x;
diff --git a/quad/src/virt_quad/hw_impl_unix_i2c.c b/quad/src/virt_quad/hw_impl_unix_i2c.c
index b77e635a54b66eed2e16cd1d6eeb06fef2cb859e..97228f0bada9ca366f30c145f66c54edae0cedde 100644
--- a/quad/src/virt_quad/hw_impl_unix_i2c.c
+++ b/quad/src/virt_quad/hw_impl_unix_i2c.c
@@ -5,6 +5,8 @@
#include <pthread.h>
#include "iic_utils.h"
+#define NUM_INPUTS 7
+
void * update_i2c_input_cache(void *);
union val {
@@ -12,16 +14,16 @@ union val {
unsigned short s;
};
-static char *input_names[6];
-static int fifos[6];
-static union val cache[6];
+static char *input_names[NUM_INPUTS];
+static int fifos[NUM_INPUTS];
+static union val cache[NUM_INPUTS];
static short last_dev;
static short last_reg;
static short last_val;
static int nums[] = {0, 1, 2, 3, 4, 5};
-static pthread_t workers[6];
+static pthread_t workers[NUM_INPUTS];
int unix_i2c_reset(struct I2CDriver *self) {
input_names[0] = "i2c-mpu-accel-x";
@@ -30,12 +32,13 @@ int unix_i2c_reset(struct I2CDriver *self) {
input_names[3] = "i2c-mpu-gryo-x";
input_names[4] = "i2c-mpu-gryo-y";
input_names[5] = "i2c-mpu-gyro-z";
+ input_names[6] = "i2c-lidar";
mkdir(VIRT_QUAD_FIFOS_DIR, 0777);
// Start up worker thread whose job is to update the caches
int i;
- for (i = 0; i < 6; i += 1) {
+ for (i = 0; i < NUM_INPUTS; i += 1) {
pthread_create(&workers[i], 0, update_i2c_input_cache, &nums[i]);
}
@@ -45,6 +48,7 @@ int unix_i2c_reset(struct I2CDriver *self) {
cache[3].s = 0;
cache[4].s = 0;
cache[5].s = 0;
+ cache[6].s = 0;
return 0;
}
@@ -87,6 +91,13 @@ int unix_i2c_read(struct I2CDriver *self,
buff[12] = cache[5].b[0];
buff[13] = cache[5].b[1];
}
+ else if (last_reg == LIDARLITE_DEVICE_ADDR) {
+ buff[0] = cache[6].b[0];
+ buff[1] = cache[6].b[0];
+ }
+ else {
+ return -1;
+ }
}
return 0;
}