controls/MATLAB/zeroLoadCurrent/zeroLoadCurrent.m

+function [I_0, I_1, I_2, dutyCycle, error, residual_error, residual_error_constantIf] = zeroLoadCurrent(absoluteFilePath)
+%This function takes in an absolute file path to a .csv or Excel file with 
+%the following format:
+%   Second column should be the motor speed in revolutions per minute
+%   Third column should be the current (A)
+%absoluteFilePath = 'C:\Users\Andy\Documents\MATLAB\MicroCART\Zero Load Current\No Load Friction Current.csv';
+
+    %Get the data from the file
+    data = readtable(absoluteFilePath);
+
+    %Extract the motor speed column and convert it to radians per second
+    dutyCycle = data.(1);
+    motorSpeed_rpm = data.(2);
+    motorSpeed_radPerSec = motorSpeed_rpm * pi/30;
+
+    %Extract the current column
+    current = data.(3);
+
+    %Define the three columns of the A matrix for the least squares 
+    %approximation, using this equation:
+    %   
+    %   I_f = sgn(motorSpeed)*I_0 + motorSpeed*I_1 + (motorSpeed^2)*I_2
+    %
+    %   for this least squares equation: A*x = b (where A and b are matrices)
+    %
+    % where: sgn(motorSpeed) -- first column of the A matrix,
+    %        motorSpeed      -- second column of the A matrix
+    %        motorSpeed^2    -- third column of the A matrix
+    %        I_f             -- b matrix
+    %
+    %  and: [I_0; I_1; I_2]  -- x vector that we are solving for
+    A_col1 = sign(motorSpeed_radPerSec);
+    A_col2 = motorSpeed_radPerSec;
+    A_col3 = motorSpeed_radPerSec .^ 2;
+
+    %Create the A matrix from its three columns
+    A = [A_col1, A_col2, A_col3];
+
+    %Define the b matrix
+    b = current;
+
+    %Least squares approximation -- solving for x, which is I_vector
+    I_vector = (A' * A)^(-1) * A' * b;
+
+    %Get the error vector
+    error = A*I_vector - b;
+    
+    If_constant = 0.511;
+    error_withIfConstant = A*[If_constant; 0; 0] - b;
+    
+    %Get the residual error
+    residual_error = sum(error.^2);
+    
+    residual_error_constantIf = sum(error_withIfConstant.^2);
+
+    %Extract the components of the vector
+    I_0 = I_vector(1);
+    I_1 = I_vector(2);
+    I_2 = I_vector(3);
+
+end %function

controls/model/3D_Animation/eigenaxis_ucart.m

+%**************************************************************************
+% Project   : Autonomous Helicopter
+% Group     : 05gr835 
+% Created   : 2005-04-28
+% Edited    : 2005-05-25
+% -------------------------------------------------------------------------
+% The non-linear model contains the following files:
+% 
+% thrusten.m        : Main rotor thrust equations
+% rigid.m           : Rigid body equations
+% parameters.m      : Helicopter parameters
+% mrflap.m          : Main rotor flapping equations
+% force_torque.m    : Force and torque equations
+% eigenaxis.m       : Eigen axis rotation for use in VR toolbox
+
+%**************************************************************************
+
+% eigenaxis.m       : This file contains the eigen axis rotation
+%algorithm. This file is created by group 05gr830.
+
+function y = eigenaxis(u)
+
+
+if abs(u(1))< 0.0001
+    u(1) = 0.0001;
+end
+
+if abs(u(2))< 0.0001
+    u(2) = 0.0001;
+end
+
+if abs(u(3))< 0.0001
+    u(3) = 0.0001;
+end
+
+u = [ -u(1); -u(2); u(3) ];% [Pitch, Yaw, Roll] 
+
+C11 = cos(u(2))*cos(u(3));
+C12 = cos(u(2))*sin(u(3));
+C13 = -sin(u(2));
+C21 = sin(u(1))*sin(u(2))*cos(u(3))-cos(u(1))*sin(u(3));
+C22 = sin(u(1))*sin(u(2))*sin(u(3))+cos(u(1))*cos(u(3));
+C23 = sin(u(1))*cos(u(2));
+C31 = cos(u(1))*sin(u(2))*cos(u(3))+sin(u(1))*sin(u(3));
+C32 = cos(u(1))*sin(u(2))*sin(u(3))-sin(u(1))*cos(u(3));
+C33 = cos(u(1))*cos(u(2));
+    
+theta = acos(0.5*(C11+C22+C33-1));
+
+e = [C23-C32; C31-C13; C12-C21]/(2*sin(theta));
+    
+y = [e; theta];

controls/model/3D_Animation/license.txt

+Copyright (c) 2009, The MathWorks, Inc.
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without 
+modification, are permitted provided that the following conditions are 
+met:
+
+    * Redistributions of source code must retain the above copyright 
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright 
+      notice, this list of conditions and the following disclaimer in 
+      the documentation and/or other materials provided with the distribution
+    * Neither the name of the The MathWorks, Inc. nor the names 
+      of its contributors may be used to endorse or promote products derived 
+      from this software without specific prior written permission.
+      
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 
+ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 
+LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 
+CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 
+SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 
+INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 
+CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 
+ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 
+POSSIBILITY OF SUCH DAMAGE.

controls/model/3D_Animation/sfun_time.c

+#define S_FUNCTION_LEVEL 2
+#define S_FUNCTION_NAME  sfun_time
+
+#define TIME_SCALE_FACTOR(S) ssGetSFcnParam(S,0)
+
+/*
+ * Need to include simstruc.h for the definition of the SimStruct and
+ * its associated macro definitions.
+ */
+#include "simstruc.h"
+
+/*
+ *  Include the standard ANSI C header for handling time functions:
+ *  ---------------------------------------------------------------
+ */
+#include <time.h>
+
+static void mdlInitializeSizes(SimStruct *S)
+{
+
+   ssSetNumSFcnParams(S, 1);  /* Number of expected parameters */
+
+   if (ssGetNumSFcnParams(S) != ssGetSFcnParamsCount(S)) return;
+
+    ssSetNumContStates(S, 0);
+    ssSetNumDiscStates(S, 0);
+
+   if (!ssSetNumInputPorts(S, 0)) return;
+
+   if (!ssSetNumOutputPorts(S, 0)) return;
+   
+   ssSetNumSampleTimes(S, 1);
+   ssSetNumRWork(S, 1);
+   ssSetNumIWork(S, 0);
+   ssSetNumPWork(S, 0);
+   ssSetNumModes(S, 0);
+   ssSetNumNonsampledZCs(S, 0);
+   ssSetOptions(S, 0);
+}
+
+#define MDL_INITIALIZE_SAMPLE_TIMES
+static void mdlInitializeSampleTimes(SimStruct *S)
+{
+   ssSetSampleTime(S, 0, CONTINUOUS_SAMPLE_TIME);
+   ssSetOffsetTime(S, 0, 0.0);
+}
+
+#define MDL_START
+static void mdlStart(SimStruct *S)
+{
+    ssSetRWorkValue(S,0,ssGetTStart(S));
+}
+
+static void mdlOutputs(SimStruct *S, int_T tid)
+{
+   real_T             t_previousSimTime = ssGetRWorkValue(S,0);
+   const real_T      *scaleFactor = mxGetPr(TIME_SCALE_FACTOR(S));
+   time_T             t_SimTime = ssGetT(S);
+   real_T             t_diff = 0.0;
+   real_T             dt;
+   real_T             t_current;
+   real_T             t_0;
+   real_T             t_previous;
+   real_T             t_elapsed;
+   
+   /* Desired Delta time */
+   dt = (t_SimTime - t_previousSimTime) * (scaleFactor[0]);
+   /* Get clock time at the beginning of this step*/   
+   t_previous = (real_T)clock()/CLOCKS_PER_SEC;
+   t_0 = (real_T)clock()/CLOCKS_PER_SEC;
+
+   /* Wait to reach the desired time */
+   while (t_diff<dt){
+        t_current = (real_T) clock()/CLOCKS_PER_SEC;
+        /* Look for wrapup */
+        if (t_current<t_previous){
+            t_elapsed = t_previous - t_0;
+            t_0 = (real_T) clock()/CLOCKS_PER_SEC - t_elapsed;
+        }
+        t_diff = t_current - t_0;
+        t_previous = t_current;
+   }
+   
+   /* Store current time to be used in next time step*/
+   ssSetRWorkValue(S, 0, t_SimTime);
+}
+
+static void mdlTerminate(SimStruct *S)
+{
+    UNUSED_ARG(S); /* unused input argument */
+}
+
+/*
+ *  Required S-function trailer:
+ *  ----------------------------
+ */
+#ifdef  MATLAB_MEX_FILE    /* Is this file being compiled as a MEX-file? */
+#include "simulink.c"      /* MEX-file interface mechanism */
+#else
+#include "cg_sfun.h"       /* Code generation registration function */
+#endif

controls/model/modelParameters.m

+    % Model Parameters
+    m = 1.19;            % 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)
+    Jzz = 0.0332;        % 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.1558*10^-6;   % Rotor thrust constant
+    Kh = 0;              % Rotor in-plane drag constant
+    Kd = 1.8087e-07;     % Rotor drag constant
+    rhx = 0.016;         % X-axis distance from center of mass to a rotor hub
+    rhy = 0.016;         % Y-axis distance from center of mass to a rotor hub
+    rhz = 0.003;         % Z-axis distance from center of mass to a rotor hub
+    Rm = 0.2308;         % Motor resistance
+    Kq = 96.3422;        % Motor torque constant
+    Kv = 96.3422;        % Motor back emf constant
+    If = 0.511;          % Motor internal friction current
+    Pmin = 0.40;         % Minimum zybo output duty cycle command
+    Pmax = 0.80;         % Maximum zybo output duty cycle command
+    Tc = 0.01;           % Camera system sampling period
+    tau_c = 0;           % Camera system total latency
+    Vb = 11.1;           % Nominal battery voltage (V)
\ No newline at end of file

.gitignore → groundStation/.gitignore

@@ -38,4 +38,7 @@ src/vrpn/build*
 src/vrpn/pc_linux64/*
 
 #Exacutables
-./BlueTooth
+logs
+BackEnd
+obj
+Cli

groundStation/Makefile

+# Declaration of variables
+
+# Generic Variables
+GCC=gcc
+GXX=g++
+CFLAGS= -Wall -pedantic -Wextra -Werror -std=gnu99 -g -Wno-unused-parameter -Wno-unused-variable -Wno-unused-function -Wno-unused-but-set-variable
+CXXFLAGS= -Wall -pedantic -Wextra -Werror -Wno-reorder -Wno-unused-variable -std=c++0x -g
+INCLUDES = $(foreach dir, $(INCDIR), -I$(dir))
+INCDIR=inc src/vrpn src/vrpn/quat src/vrpn/build $(BESRCDIR) $(CLISRCDIR) $(FESRCDIR)
+LIBS= -lpthread -lbluetooth -lvrpn -lquat -Lsrc/vrpn/build -Lsrc/vrpn/build/quat 
+OBJDIR=obj
+
+# Backend Specific Variables
+BEBINARY=BackEnd
+BESRCDIR=src/backend
+BECSOURCES := $(wildcard $(BESRCDIR)/*.c )
+BECOBJECTS = $(BECSOURCES:$(BESRCDIR)/%.c=$(OBJDIR)/%.o)
+BECPPSOURCES := $(wildcard $(BESRCDIR)/*.cpp )
+BECPPOBJECTS = $(BECPPSOURCES:$(BESRCDIR)/%.cpp=$(OBJDIR)/%.o)
+
+# CLI Specific Variables
+CLIBINARY=Cli
+CLISRCDIR=src/cli
+CLISOURCES := $(wildcard $(CLISRCDIR)/*.c)
+CLIOBJECTS = $(CLISOURCES:$(CLISRCDIR)/%.c=$(OBJDIR)/%.o)
+
+# Frontend-common stuff
+FESRCDIR=src/frontend
+FECSOURCES := $(wildcard $(FESRCDIR)/*.c )
+FECOBJECTS = $(FECSOURCES:$(FESRCDIR)/%.c=$(OBJDIR)/%.o)
+
+
+OBJECTS= $(CLIOBJECTS) $(BECOBJECTS) $(BECPPOBJECTS) $(FECOBJECTS)
+
+# Default target
+all: logs objdir backend cli
+
+vrpn: vrpn/build
+
+cli:  $(CLIOBJECTS) $(FECOBJECTS)
+	$(GCC) $(CFLAGS) $^ -o $(CLIBINARY) $(INCLUDES) $(LIBS)
+
+$(CLIOBJECTS) : $(OBJDIR)/%.o : $(CLISRCDIR)/%.c
+	$(GCC)  $(CFLAGS) -c $^ -o $@ $(INCLUDES) $(LIBS)
+
+
+backend: $(BECPPOBJECTS) $(BECOBJECTS)
+	$(GXX) $(CXXFLAGS) $^ -o $(BEBINARY) $(INCLUDES) $(LIBS)
+
+$(FECOBJECTS) : $(OBJDIR)/%.o : $(FESRCDIR)/%.c
+	$(GCC)  $(CFLAGS) -c $^ -o $@ $(INCLUDES) $(LIBS)
+
+$(BECOBJECTS) : $(OBJDIR)/%.o : $(BESRCDIR)/%.c
+	$(GCC)  $(CFLAGS) -c $^ -o $@ $(INCLUDES) $(LIBS)
+
+$(BECPPOBJECTS) : $(OBJDIR)/%.o : $(BESRCDIR)/%.cpp
+	$(GXX)  $(CXXFLAGS) -c $^ -o $@ $(INCLUDES) $(LIBS)
+
+vrpn/build:
+	mkdir -p src/vrpn/build
+	cd src/vrpn/build && cmake .. && make
+
+logs:
+	mkdir -p logs
+
+objdir:
+	mkdir -p obj
+
+clean_logs:
+	rm -f logs/*
+
+clean:
+	rm -rf $(OBJDIR)/ $(BEBINARY) $(CLIBINARY)
+
+debug:
+	@echo $(OBJECTS)