Merge branch 'master' of git.ece.iastate.edu:DANC/Microcart_17-18

Readme.md

@@ -31,7 +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)
+[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

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

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

documentation/how_to_document_things_on_gitlab.md

@@ -9,7 +9,7 @@ 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 while-project encompassing, those links should go
+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.