Merge branch 'master' of git.ece.iastate.edu:danc/MicroCART_17-18 into quad-impl-lidar

Conflicts:
	quad/sw/modular_quad_pid/src/iic_utils.c

.gitlab-ci.yml

 build:
   stage: build
   script:
-    - echo "It works."
+    - bash ci-build.sh
 
 # run tests using the binary built before
 test:
   stage: test
   script:
-    - bash test-ci.sh
+    - bash ci-test.sh

ci-build.sh

+#!/bin/bash
+
+PROJECT_ROOT=$(pwd)
+export PROJECT_ROOT
+
+# Quad
+bash quad/ci-build.sh || exit 1

test-ci.sh → ci-test.sh

@@ -4,4 +4,4 @@ PROJECT_ROOT=$(pwd)
 export PROJECT_ROOT
 
 # Quad
-bash quad/test-ci.sh || exit 1
+bash quad/ci-test.sh || exit 1

controls/MATLAB/momentOfInertia/J_calc_bifilar.m

+function  J  = J_calc_bifilar( m , g, D, L, t, n , varargin)
+%UNTITLED Summary of this function goes here
+%   Detailed explanation goes here
+
+average_strings = {'average','mean','ave'}; 
+least_squares_strings = {'least squares','ls'}; 
+
+mode = 0; 
+if ~isempty(varargin)
+    for k = 1:length(varargin)
+        if any(strcmpi(varargin{k},average_strings))
+            mode = 0; 
+        end
+        if any(strcmpi(varargin{k},least_squares_strings))
+            mode = 1; 
+        end
+    end
+end
+
+if mode == 0 %averge
+        N = length(t); 
+        Js = zeros(1,N); 
+        for k = 1:N
+            Js(k) = m(k)*g*D(k)^2/(16*pi^2*L(k))*(t(k)/n(k))^2; 
+        end        
+        J = mean(Js); 
+end
+
+if mode == 1 %least squares
+    J = (16*pi^2*L./(m*g.*D.^2))\(t./n).^2 ; 
+end
+
+
+
+end
+

controls/MATLAB/momentOfInertia/J_calc_bifilar_script.m

+
+% inches2meters = 0.0254 ; 
+
+%%
+% %Jxx
+% m = 1.244*ones(5,1); 
+% D = (12+3/8)*inches2meters*ones(5,1); 
+% L = (81+3/4)*inches2meters*ones(5,1); 
+% t = [ 94.58 ; 93.91 ; 93.81 ; 94.16 ; 94.04 ] ; 
+% n = 50*ones(5,1); 
+
+% %Jyy
+% m = 1.244*ones(5,1); 
+% D = (12+3/8)*inches2meters*ones(5,1); 
+% L = (91+3/4)*inches2meters*ones(5,1); 
+% t = [ 103.31 ; 103.15 ; 103.31 ; 103.47 ; 103.13 ] ; 
+% n = 50*ones(5,1); 
+
+
+% %Jzz 
+% m = 1.244*ones(5,1); 
+% D = (19+3/16)*inches2meters*ones(5,1); 
+% L = (96+1/4)*inches2meters*ones(5,1); 
+% t = [ 96.64 ; 97.47 ; 97.75 ; 97.35 ; 97.78 ] ; 
+% n = 50*ones(5,1); 
+
+load .\git_repos\microCART_17-18\controls\dataCollection\bifilar_inertia_2_18_2017\MicroCART_bifilar_Jxx_2_18_2017 ; 
+Jxx_a = J_calc_bifilar( m , g, D, L, t, n , 'average')
+Jxx_ls = J_calc_bifilar( m , g, D, L, t, n , 'least squares')
+
+clear D L m n t 
+load .\git_repos\microCART_17-18\controls\dataCollection\bifilar_inertia_2_18_2017\MicroCART_bifilar_Jyy_2_18_2017 ; 
+Jyy_a = J_calc_bifilar( m , g, D, L, t, n , 'average')
+Jyy_ls = J_calc_bifilar( m , g, D, L, t, n , 'least squares')
+
+clear D L m n t 
+load .\git_repos\microCART_17-18\controls\dataCollection\bifilar_inertia_2_18_2017\MicroCART_bifilar_Jzz_2_18_2017 ; 
+Jzz_a = J_calc_bifilar( m , g, D, L, t, n , 'average')
+Jzz_ls = J_calc_bifilar( m , g, D, L, t, n , 'least squares')

controls/model/modelParameters.m

     % Model Parameters
-    m = 1.19;                       % Quadrotor + battery mass
+    m = 1.244;                      % Quadrotor + battery mass
     g = 9.81;                       % Acceleration of gravity
-    %Jxx = 0.0218;                   % Quadrotor and battery motor of inertia around bx (pitch)
-    %Jyy = 0.0277;                   % Quadrotor and battery motor of inertia around by (roll)
-    Jxx = 0.0277;
-    Jyy = 0.0218;
-    Jzz = 0.0332;                   % Quadrotor and battery motor of inertia around bz (yaw)
+%     Jxx = 0.0277;                   % Quadrotor and battery motor of inertia around bx (pitch)
+%     Jyy = 0.0218;                   % Quadrotor and battery motor of inertia around by (roll)
+%     Jzz = 0.0332;                   % Quadrotor and battery motor of inertia around bz (yaw)
+    Jxx = 0.0130;                   % Quadrotor and battery motor of inertia around bx (pitch)
+    Jyy = 0.0140;                   % Quadrotor and battery motor of inertia around by (roll)
+    Jzz = 0.0285;                   % Quadrotor and battery motor of inertia around bz (yaw)
     Jreq = 4.2012e-05;              % Rotor and motor moment of inertia around axis of rotation 
     Kt = 8.6519e-6;                 % Rotor thrust constant
     Kh = 0;                         % Rotor in-plane drag constant

quad/ci-build.sh

+#!/bin/bash
+
+QUAD_ROOT=$PROJECT_ROOT/quad
+export QUAD_ROOT
+
+# Build Quad code
+cd $QUAD_ROOT/sw/modular_quad_pid/
+bash build.sh || exit 1

quad/test-ci.sh → quad/ci-test.sh

@@ -3,9 +3,11 @@
 QUAD_ROOT=$PROJECT_ROOT/quad
 export QUAD_ROOT
 
+# Build Test Library
 cd $QUAD_ROOT/lib/test
 make || exit 1
 
+# Test UART buffer
 cd $QUAD_ROOT/sw/modular_quad_pid/test
 make || exit 1
 ./test_uart_buff || exit 1

quad/sw/modular_quad_pid/.build_app.tcl

+cd ..
+sdk set_workspace .
+sdk build_project -type app -name modular_quad_pid

quad/sw/modular_quad_pid/.build_bsp.tcl

+cd ..
+sdk set_workspace .
+sdk build_project -type bsp -name system_bsp

quad/sw/modular_quad_pid/.create_bsp.tcl

+cd ..
+sdk set_workspace .
+sdk create_bsp_project -name system_bsp -hwproject system_hw_platform -proc ps7_cortexa9_0 -os standalone

quad/sw/modular_quad_pid/build.sh

+#!/bin/bash
+
+source /remote/Xilinx/2015.4/SDK/2015.4/settings64.sh
+
+echo "Building modular_quad_pid"
+
+if [ ! -d ../.metadata ]; then
+    # Haven't configured XSDK environment yet. Do that now
+
+    ECLIPSE=/remote/Xilinx/2015.4/SDK/2015.4/eclipse/lnx64.o/eclipse
+    VM=/remote/Xilinx/2015.4/SDK/2015.4/tps/lnx64/jre/bin
+    WSPACE=$(dirname $0)/..
+    HW=$WSPACE/system_hw_platform
+    BSP=$WSPACE/system_bsp
+    APP=$WSPACE/modular_quad_pid
+
+    echo "Setting up dependencies for modular_quad_pid"
+
+    # Import the system_hw_platform into the workspace
+    $ECLIPSE -vm $VM -nosplash -application org.eclipse.cdt.managedbuilder.core.headlessbuild \
+        -import $HW \
+        -data $WSPACE \
+        -vmargs -Dorg.eclipse.cdt.core.console=org.eclipse.cdt.core.systemConsole || exit 1
+
+    # Create the BSP
+    xsct .create_bsp.tcl || exit 1
+
+    # Import the system_bsp and modular_quad_pid into workspace
+    $ECLIPSE -vm $VM -nosplash -application org.eclipse.cdt.managedbuilder.core.headlessbuild \
+        -import $BSP \
+        -import $APP \
+        -data $WSPACE \
+        -vmargs -Dorg.eclipse.cdt.core.console=org.eclipse.cdt.core.systemConsole || exit 1
+
+    # Build the BSP
+    xsct .build_bsp.tcl || exit 1
+
+fi
+
+xsct .build_app.tcl || exit 1

quad/sw/modular_quad_pid/src/control_algorithm.c

@@ -10,7 +10,8 @@
 #include "control_algorithm.h"
 #include "communication.h"
 
-#define ROLL_PITCH_MAX_ANGLE 0.3490 // 20 degrees
+#define ROLL_PITCH_MAX_ANGLE 1.5708 // 90 degrees
+#define PWM_DIFF_BOUNDS 30000
 
  int control_algorithm_init(parameter_t * parameter_struct)
  {
@@ -184,13 +185,13 @@
 
 	parameter_struct->pid_controllers[PITCH_ID].current_point = sensor_struct->pitch_angle_filtered;
 	parameter_struct->pid_controllers[PITCH_ID].setpoint =
-			/*(user_defined_struct->flight_mode == AUTO_FLIGHT_MODE)?
-			(parameter_struct->pid_controllers[LOCAL_X_ID].pid_correction) + pitch_trim :*/ user_input_struct->pitch_angle_manual_setpoint;
+		(user_defined_struct->flight_mode == AUTO_FLIGHT_MODE)?
+			(parameter_struct->pid_controllers[LOCAL_X_ID].pid_correction) + pitch_trim : user_input_struct->pitch_angle_manual_setpoint;
 
 	parameter_struct->pid_controllers[ROLL_ID].current_point = sensor_struct->roll_angle_filtered;
 	parameter_struct->pid_controllers[ROLL_ID].setpoint =
-			/*(user_defined_struct->flight_mode == AUTO_FLIGHT_MODE)?
-			(parameter_struct->pid_controllers[LOCAL_Y_ID].pid_correction) + roll_trim :*/ user_input_struct->roll_angle_manual_setpoint;
+			(user_defined_struct->flight_mode == AUTO_FLIGHT_MODE)?
+			(parameter_struct->pid_controllers[LOCAL_Y_ID].pid_correction) + roll_trim : user_input_struct->roll_angle_manual_setpoint;
 
 
 	//logging and PID computation
@@ -232,15 +233,17 @@
 	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
+	// NOTE!!!!!!!!!!!!!!!!!!!!!!
+	// re-enable the check for AUTO_FLIGHT_MODE when autonomous is fully enabled
 	if((user_input_struct->rc_commands[THROTTLE] >
-	118000) || (user_defined_struct->flight_mode == AUTO_FLIGHT_MODE))
+	118000))// || (user_defined_struct->flight_mode == AUTO_FLIGHT_MODE))
 	{
 
 		if(user_defined_struct->flight_mode == AUTO_FLIGHT_MODE)
 		{
 			//THROTTLE
-			raw_actuator_struct->controller_corrected_motor_commands[THROTTLE] =
-				((int)(parameter_struct->pid_controllers[ALT_ID].pid_correction)) + sensor_struct->trims.throttle;
+			//raw_actuator_struct->controller_corrected_motor_commands[THROTTLE] =
+			//	((int)(parameter_struct->pid_controllers[ALT_ID].pid_correction)) + sensor_struct->trims.throttle;
 
 			//ROLL
 			raw_actuator_struct->controller_corrected_motor_commands[ROLL] =
@@ -284,23 +287,23 @@
 			raw_actuator_struct->controller_corrected_motor_commands[THROTTLE] = 0;
 
 		//BOUNDS CHECKING
-		if(raw_actuator_struct->controller_corrected_motor_commands[ROLL] > 20000)
-			raw_actuator_struct->controller_corrected_motor_commands[ROLL] = 20000;
+		if(raw_actuator_struct->controller_corrected_motor_commands[ROLL] > PWM_DIFF_BOUNDS)
+			raw_actuator_struct->controller_corrected_motor_commands[ROLL] = PWM_DIFF_BOUNDS;
 
-		if(raw_actuator_struct->controller_corrected_motor_commands[ROLL] < -20000)
-			raw_actuator_struct->controller_corrected_motor_commands[ROLL] = -20000;
+		if(raw_actuator_struct->controller_corrected_motor_commands[ROLL] < -PWM_DIFF_BOUNDS)
+			raw_actuator_struct->controller_corrected_motor_commands[ROLL] = -PWM_DIFF_BOUNDS;
 
-		if(raw_actuator_struct->controller_corrected_motor_commands[PITCH] > 20000)
-			raw_actuator_struct->controller_corrected_motor_commands[PITCH] = 20000;
+		if(raw_actuator_struct->controller_corrected_motor_commands[PITCH] > PWM_DIFF_BOUNDS)
+			raw_actuator_struct->controller_corrected_motor_commands[PITCH] = PWM_DIFF_BOUNDS;
 
-		if(raw_actuator_struct->controller_corrected_motor_commands[PITCH] < -20000)
-			raw_actuator_struct->controller_corrected_motor_commands[PITCH] = -20000;
+		if(raw_actuator_struct->controller_corrected_motor_commands[PITCH] < -PWM_DIFF_BOUNDS)
+			raw_actuator_struct->controller_corrected_motor_commands[PITCH] = -PWM_DIFF_BOUNDS;
 
-		if(raw_actuator_struct->controller_corrected_motor_commands[YAW] > 20000)
-			raw_actuator_struct->controller_corrected_motor_commands[YAW] = 20000;
+		if(raw_actuator_struct->controller_corrected_motor_commands[YAW] > PWM_DIFF_BOUNDS)
+			raw_actuator_struct->controller_corrected_motor_commands[YAW] = PWM_DIFF_BOUNDS;
 
-		if(raw_actuator_struct->controller_corrected_motor_commands[YAW] < -20000)
-			raw_actuator_struct->controller_corrected_motor_commands[YAW] = -20000;
+		if(raw_actuator_struct->controller_corrected_motor_commands[YAW] < -PWM_DIFF_BOUNDS)
+			raw_actuator_struct->controller_corrected_motor_commands[YAW] = -PWM_DIFF_BOUNDS;
 
 	}
 	else