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)
+    omega_o = 598.088;              % Equilibrium Rotor Speed
+    x_controlled_o = 0;             % Equilibrium lateral controller output
+    y_controlled_o = 0;             % Equilibrium longitudinal controller output
+    yaw_controlled_o = 0;           % Equilibrium yaw controller output
+    
+    % Equilibrium height controller output
+    height_controlled_o = (((Rm*If + ...
+    + (((omega_o * 2 * Rm * Kv * Kq  ...
+    * Kd + 1)^2) - 1)/(4* Rm*Kv^2*Kq ...
+    *Kd))/Vb)*(Pmax- Pmin)+Pmin)*100;     
\ No newline at end of file

groundStation/.gitignore

+# Object files
+*.o
+*.ko
+*.obj
+*.elf
+
+# Precompiled Headers
+*.gch
+*.pch
+
+# Libraries
+*.lib
+*.a
+*.la
+*.lo
+
+# Shared objects (inc. Windows DLLs)
+*.dll
+*.so
+*.so.*
+*.dylib
+
+# Executables
+*.exe
+*.out
+*.app
+*.i*86
+*.x86_64
+*.hex
+
+# Debug files
+*.dSYM/
+*.su
+
+
+# vrpn/build files
+src/vrpn/build*
+src/vrpn/pc_linux64/*
+
+#Exacutables
+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)
+SYMLINKS=monitor setpid getpid setsetpoint
+
+# 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 $(SYMLINKS)
+
+$(SYMLINKS): $(CLIBINARY)
+	$(foreach symlink, $(SYMLINKS), ln -s $(CLIBINARY) $(symlink);)
+
+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)

groundStation/README.md

+# groundStation
+
+## Make Process
+
+First, if submodules were not recursevly added through git. Run this command if you have made any attempt to make vrpn manually. 
+run
+	'git submodule update --init --recursive'
+
+Now that you have all of the files necissary.
+
+cd into the groundstation folder.
+	cd groundStation
+	make vrpn
+	make
+
+run the program with sudo privledges
+	sudo -E ./BackEnd
+
+If you wish to change the way the backend communicates to the quad and vice versa, look at src/config.h. 
+	This provides a list of environment variables which you can set and use for your computer or time of use.
+	Because the backend must be ran with sudo privledges, you will need to preserve the env. vars. with sudo rights.
+	Hence the "-E" flag.
+
+## Modifying
+See MODIFYING for the software architecture/organization and how to add new functionality.
+
+## Using
+First, the backend daemon must be running. Run the backend with ./BackEnd. Note
+the environment variables in config.h, especially the backend socket path. The backend
+requires root for bluetooth, but can run as a normal user with TCP, as long as the
+socket path is writable by the user.
+
+Once the backend is running, various CLI tools can be used to view the state of the
+quad and send commands. Each of these tools is given as the first argument
+to the CLI program, for example, to monitor the quad, use `cli monitor`. Note that
+the backend socket environment variable must be set to the same value as it
+was for the backend. The CLI program also supports busybox-style symbolic links.
+For example, if there is a symlink named `monitor` that points to the `cli` binary,
+running the `monitor` program will effectively run `cli monitor`.
+
+The names of the binaries is subject to change.
+
+For more in depth usage explainations, use the --help flag.
+'./Cli --help'
+
+For help with the specific cli command you are running, use the --help flag once again.
+'./Cli setpid --help'
+
+### Example
+In one terminal or screen, run the backend:
+
+`UCART_SOCKET=./ucart.socket ./BackEnd`
+
+This will activate the quad and the backend, and the backend will be available for
+connections from the frontend tools. One useful tool is the monitor. In another
+terminal window, run the monitor forever:
+
+`UCART_SOCKET=./ucart.socket ./cli monitor -f`
+
+This will begin a periodic monitoring that updates 10 times per second.
+
+Finally, in a third window, export the socket path:
+
+`export UCART_SOCKET=./ucart.socket`
+
+and then run any other tools to modify the quad, for example modifying PID constants:
+
+`./cli setpid --pitch -p 1.000`
+