Compare revisions

c5387ea4 · 99c54dbe · c5387ea4 · c5387ea4 · c5387ea4 · c5387ea4
--- a/groundStation/src/frontend/setpoint_common.h
+++ b/groundStation/src/frontend/setpoint_common.h
+#ifndef SETPOINT_COMMON_H
+#define SETPOINT_COMMON_H
+
+struct frontend_setpoint_data {
+	float height;
+	float lat;
+	/* names with two g's due to long type name */
+	float longg;
+};
+
+
+#endif /* SETPOINT_COMMON_H */
\ No newline at end of file
--- a/vrpn @ 99c54dbe
+++ b/vrpn @ 99c54dbe
+Subproject commit 99c54dbefe04897cc7c146101a126f62c0e65f97
--- a/quad/.gitignore
+++ b/quad/.gitignore
+zybo_fsbl_bsp/
+test.log
--- a/quad/sw/.gitignore
+++ b/quad/sw/.gitignore
+SDK.log
+test.log
+.metadata/
+system_bsp/
+
--- a/quad/sw/README.md
+++ b/quad/sw/README.md
+# Branch for hardware/software designs for Quadcopter
--- a/quad/sw/imu_logger/.cproject
+++ b/quad/sw/imu_logger/.cproject
+<?xml version="1.0" encoding="UTF-8" standalone="no"?>
+<?fileVersion 4.0.0?>
+
+<cproject storage_type_id="org.eclipse.cdt.core.XmlProjectDescriptionStorage">
+	<storageModule moduleId="org.eclipse.cdt.core.settings">
+		<cconfiguration id="xilinx.gnu.arm.exe.debug.980189137">
+			<storageModule buildSystemId="org.eclipse.cdt.managedbuilder.core.configurationDataProvider" id="xilinx.gnu.arm.exe.debug.980189137" moduleId="org.eclipse.cdt.core.settings" name="Debug">
+				<externalSettings/>
+				<extensions>
+					<extension id="com.xilinx.sdk.managedbuilder.XELF.arm" point="org.eclipse.cdt.core.BinaryParser"/>
+					<extension id="org.eclipse.cdt.core.GmakeErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
+					<extension id="org.eclipse.cdt.core.CWDLocator" point="org.eclipse.cdt.core.ErrorParser"/>
+					<extension id="org.eclipse.cdt.core.GCCErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
+					<extension id="org.eclipse.cdt.core.GASErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
+					<extension id="org.eclipse.cdt.core.GLDErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
+				</extensions>
+			</storageModule>
+			<storageModule moduleId="cdtBuildSystem" version="4.0.0">
+				<configuration artifactExtension="elf" artifactName="${ProjName}" buildArtefactType="org.eclipse.cdt.build.core.buildArtefactType.exe" buildProperties="org.eclipse.cdt.build.core.buildType=org.eclipse.cdt.build.core.buildType.debug,org.eclipse.cdt.build.core.buildArtefactType=org.eclipse.cdt.build.core.buildArtefactType.exe" cleanCommand="rm -rf" description="" id="xilinx.gnu.arm.exe.debug.980189137" name="Debug" parent="xilinx.gnu.arm.exe.debug">
+					<folderInfo id="xilinx.gnu.arm.exe.debug.980189137." name="/" resourcePath="">
+						<toolChain id="xilinx.gnu.arm.exe.debug.toolchain.1195127676" name="Xilinx ARM GNU Toolchain" superClass="xilinx.gnu.arm.exe.debug.toolchain">
+							<targetPlatform binaryParser="com.xilinx.sdk.managedbuilder.XELF.arm" id="xilinx.arm.target.gnu.base.debug.2072264921" isAbstract="false" name="Debug Platform" superClass="xilinx.arm.target.gnu.base.debug"/>
+							<builder buildPath="${workspace_loc:/modular_quad_pid}/Debug" enableAutoBuild="true" id="xilinx.gnu.arm.toolchain.builder.debug.2124876787" keepEnvironmentInBuildfile="false" managedBuildOn="true" name="GNU make" superClass="xilinx.gnu.arm.toolchain.builder.debug"/>
+							<tool id="xilinx.gnu.arm.c.toolchain.assembler.debug.192056667" name="ARM gcc assembler" superClass="xilinx.gnu.arm.c.toolchain.assembler.debug">
+								<inputType id="xilinx.gnu.assembler.input.532832448" superClass="xilinx.gnu.assembler.input"/>
+							</tool>
+							<tool id="xilinx.gnu.arm.c.toolchain.compiler.debug.177835003" name="ARM gcc compiler" superClass="xilinx.gnu.arm.c.toolchain.compiler.debug">
+								<option defaultValue="gnu.c.optimization.level.none" id="xilinx.gnu.compiler.option.optimization.level.1900496019" name="Optimization Level" superClass="xilinx.gnu.compiler.option.optimization.level" value="gnu.c.optimization.level.none" valueType="enumerated"/>
+								<option id="xilinx.gnu.compiler.option.debugging.level.1207856754" name="Debug Level" superClass="xilinx.gnu.compiler.option.debugging.level" value="gnu.c.debugging.level.max" valueType="enumerated"/>
+								<option id="xilinx.gnu.compiler.inferred.swplatform.includes.2123463819" name="Software Platform Include Path" superClass="xilinx.gnu.compiler.inferred.swplatform.includes" valueType="includePath">
+									<listOptionValue builtIn="false" value="../../system_bsp/ps7_cortexa9_0/include"/>
+								</option>
+								<option id="xilinx.gnu.compiler.symbols.defined.1696008720" name="Defined symbols (-D)" superClass="xilinx.gnu.compiler.symbols.defined"/>
+								<inputType id="xilinx.gnu.arm.c.compiler.input.909725989" name="C source files" superClass="xilinx.gnu.arm.c.compiler.input"/>
+							</tool>
+							<tool id="xilinx.gnu.arm.cxx.toolchain.compiler.debug.1470236349" name="ARM g++ compiler" superClass="xilinx.gnu.arm.cxx.toolchain.compiler.debug">
+								<option defaultValue="gnu.c.optimization.level.none" id="xilinx.gnu.compiler.option.optimization.level.9972228" name="Optimization Level" superClass="xilinx.gnu.compiler.option.optimization.level" valueType="enumerated"/>
+								<option id="xilinx.gnu.compiler.option.debugging.level.595503730" name="Debug Level" superClass="xilinx.gnu.compiler.option.debugging.level" value="gnu.c.debugging.level.max" valueType="enumerated"/>
+								<option id="xilinx.gnu.compiler.inferred.swplatform.includes.53367111" name="Software Platform Include Path" superClass="xilinx.gnu.compiler.inferred.swplatform.includes" valueType="includePath">
+									<listOptionValue builtIn="false" value="../../system_bsp/ps7_cortexa9_0/include"/>
+								</option>
+							</tool>
+							<tool id="xilinx.gnu.arm.toolchain.archiver.585254344" name="ARM archiver" superClass="xilinx.gnu.arm.toolchain.archiver"/>
+							<tool id="xilinx.gnu.arm.c.toolchain.linker.debug.360488201" name="ARM gcc linker" superClass="xilinx.gnu.arm.c.toolchain.linker.debug">
+								<option id="xilinx.gnu.linker.inferred.swplatform.lpath.47881921" name="Software Platform Library Path" superClass="xilinx.gnu.linker.inferred.swplatform.lpath" valueType="libPaths">
+									<listOptionValue builtIn="false" value="../../system_bsp/ps7_cortexa9_0/lib"/>
+								</option>
+								<option id="xilinx.gnu.linker.inferred.swplatform.flags.349145084" name="Software Platform Inferred Flags" superClass="xilinx.gnu.linker.inferred.swplatform.flags" valueType="libs">
+									<listOptionValue builtIn="false" value="-Wl,--start-group,-lxil,-lgcc,-lc,--end-group"/>
+								</option>
+								<option id="xilinx.gnu.c.linker.option.lscript.972374298" name="Linker Script" superClass="xilinx.gnu.c.linker.option.lscript" value="../src/lscript.ld" valueType="string"/>
+								<option id="xilinx.gnu.c.link.option.libs.1025826490" name="Libraries (-l)" superClass="xilinx.gnu.c.link.option.libs" valueType="libs">
+									<listOptionValue builtIn="false" value="m"/>
+								</option>
+								<inputType id="xilinx.gnu.linker.input.777404607" superClass="xilinx.gnu.linker.input">
+									<additionalInput kind="additionalinputdependency" paths="$(USER_OBJS)"/>
+									<additionalInput kind="additionalinput" paths="$(LIBS)"/>
+								</inputType>
+								<inputType id="xilinx.gnu.linker.input.lscript.634888687" name="Linker Script" superClass="xilinx.gnu.linker.input.lscript"/>
+							</tool>
+							<tool id="xilinx.gnu.arm.cxx.toolchain.linker.debug.1921379343" name="ARM g++ linker" superClass="xilinx.gnu.arm.cxx.toolchain.linker.debug">
+								<option id="xilinx.gnu.linker.inferred.swplatform.lpath.1819794269" name="Software Platform Library Path" superClass="xilinx.gnu.linker.inferred.swplatform.lpath" valueType="libPaths">
+									<listOptionValue builtIn="false" value="../../system_bsp/ps7_cortexa9_0/lib"/>
+								</option>
+								<option id="xilinx.gnu.linker.inferred.swplatform.flags.1650054208" name="Software Platform Inferred Flags" superClass="xilinx.gnu.linker.inferred.swplatform.flags" valueType="libs">
+									<listOptionValue builtIn="false" value="-Wl,--start-group,-lxil,-lgcc,-lc,--end-group"/>
+								</option>
+								<option id="xilinx.gnu.c.linker.option.lscript.1506564015" name="Linker Script" superClass="xilinx.gnu.c.linker.option.lscript" value="../src/lscript.ld" valueType="string"/>
+							</tool>
+							<tool id="xilinx.gnu.arm.size.debug.923967150" name="ARM Print Size" superClass="xilinx.gnu.arm.size.debug"/>
+						</toolChain>
+					</folderInfo>
+				</configuration>
+			</storageModule>
+			<storageModule moduleId="org.eclipse.cdt.core.externalSettings"/>
+		</cconfiguration>
+		<cconfiguration id="xilinx.gnu.arm.exe.release.255973624">
+			<storageModule buildSystemId="org.eclipse.cdt.managedbuilder.core.configurationDataProvider" id="xilinx.gnu.arm.exe.release.255973624" moduleId="org.eclipse.cdt.core.settings" name="Release">
+				<externalSettings/>
+				<extensions>
+					<extension id="com.xilinx.sdk.managedbuilder.XELF.arm" point="org.eclipse.cdt.core.BinaryParser"/>
+					<extension id="org.eclipse.cdt.core.GmakeErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
+					<extension id="org.eclipse.cdt.core.CWDLocator" point="org.eclipse.cdt.core.ErrorParser"/>
+					<extension id="org.eclipse.cdt.core.GCCErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
+					<extension id="org.eclipse.cdt.core.GASErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
+					<extension id="org.eclipse.cdt.core.GLDErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
+				</extensions>
+			</storageModule>
+			<storageModule moduleId="cdtBuildSystem" version="4.0.0">
+				<configuration artifactExtension="elf" artifactName="${ProjName}" buildArtefactType="org.eclipse.cdt.build.core.buildArtefactType.exe" buildProperties="org.eclipse.cdt.build.core.buildType=org.eclipse.cdt.build.core.buildType.release,org.eclipse.cdt.build.core.buildArtefactType=org.eclipse.cdt.build.core.buildArtefactType.exe" cleanCommand="rm -rf" description="" id="xilinx.gnu.arm.exe.release.255973624" name="Release" parent="xilinx.gnu.arm.exe.release">
+					<folderInfo id="xilinx.gnu.arm.exe.release.255973624." name="/" resourcePath="">
+						<toolChain id="xilinx.gnu.arm.exe.release.toolchain.1375808537" name="Xilinx ARM GNU Toolchain" superClass="xilinx.gnu.arm.exe.release.toolchain">
+							<targetPlatform binaryParser="com.xilinx.sdk.managedbuilder.XELF.arm" id="xilinx.arm.target.gnu.base.release.230721338" isAbstract="false" name="Release Platform" superClass="xilinx.arm.target.gnu.base.release"/>
+							<builder buildPath="${workspace_loc:/modular_quad_pid}/Release" enableAutoBuild="true" id="xilinx.gnu.arm.toolchain.builder.release.122813927" keepEnvironmentInBuildfile="false" managedBuildOn="true" name="GNU make" superClass="xilinx.gnu.arm.toolchain.builder.release"/>
+							<tool id="xilinx.gnu.arm.c.toolchain.assembler.release.1899854338" name="ARM gcc assembler" superClass="xilinx.gnu.arm.c.toolchain.assembler.release">
+								<inputType id="xilinx.gnu.assembler.input.756511915" superClass="xilinx.gnu.assembler.input"/>
+							</tool>
+							<tool id="xilinx.gnu.arm.c.toolchain.compiler.release.85270120" name="ARM gcc compiler" superClass="xilinx.gnu.arm.c.toolchain.compiler.release">
+								<option defaultValue="gnu.c.optimization.level.more" id="xilinx.gnu.compiler.option.optimization.level.515686013" name="Optimization Level" superClass="xilinx.gnu.compiler.option.optimization.level" valueType="enumerated"/>
+								<option id="xilinx.gnu.compiler.option.debugging.level.1121150517" name="Debug Level" superClass="xilinx.gnu.compiler.option.debugging.level" value="gnu.c.debugging.level.none" valueType="enumerated"/>
+								<option id="xilinx.gnu.compiler.inferred.swplatform.includes.687694973" name="Software Platform Include Path" superClass="xilinx.gnu.compiler.inferred.swplatform.includes" valueType="includePath">
+									<listOptionValue builtIn="false" value="../../system_bsp/ps7_cortexa9_0/include"/>
+								</option>
+								<inputType id="xilinx.gnu.arm.c.compiler.input.846429887" name="C source files" superClass="xilinx.gnu.arm.c.compiler.input"/>
+							</tool>
+							<tool id="xilinx.gnu.arm.cxx.toolchain.compiler.release.1846278293" name="ARM g++ compiler" superClass="xilinx.gnu.arm.cxx.toolchain.compiler.release">
+								<option defaultValue="gnu.c.optimization.level.more" id="xilinx.gnu.compiler.option.optimization.level.1613253262" name="Optimization Level" superClass="xilinx.gnu.compiler.option.optimization.level" valueType="enumerated"/>
+								<option id="xilinx.gnu.compiler.option.debugging.level.1485305325" name="Debug Level" superClass="xilinx.gnu.compiler.option.debugging.level" value="gnu.c.debugging.level.none" valueType="enumerated"/>
+								<option id="xilinx.gnu.compiler.inferred.swplatform.includes.2144106422" name="Software Platform Include Path" superClass="xilinx.gnu.compiler.inferred.swplatform.includes" valueType="includePath">
+									<listOptionValue builtIn="false" value="../../system_bsp/ps7_cortexa9_0/include"/>
+								</option>
+							</tool>
+							<tool id="xilinx.gnu.arm.toolchain.archiver.86822110" name="ARM archiver" superClass="xilinx.gnu.arm.toolchain.archiver"/>
+							<tool id="xilinx.gnu.arm.c.toolchain.linker.release.264708896" name="ARM gcc linker" superClass="xilinx.gnu.arm.c.toolchain.linker.release">
+								<option id="xilinx.gnu.linker.inferred.swplatform.lpath.444912795" name="Software Platform Library Path" superClass="xilinx.gnu.linker.inferred.swplatform.lpath" valueType="libPaths">
+									<listOptionValue builtIn="false" value="../../system_bsp/ps7_cortexa9_0/lib"/>
+								</option>
+								<option id="xilinx.gnu.linker.inferred.swplatform.flags.1169214283" name="Software Platform Inferred Flags" superClass="xilinx.gnu.linker.inferred.swplatform.flags" valueType="libs">
+									<listOptionValue builtIn="false" value="-Wl,--start-group,-lxil,-lgcc,-lc,--end-group"/>
+								</option>
+								<option id="xilinx.gnu.c.linker.option.lscript.1266246445" name="Linker Script" superClass="xilinx.gnu.c.linker.option.lscript" value="../src/lscript.ld" valueType="string"/>
+								<inputType id="xilinx.gnu.linker.input.1933167678" superClass="xilinx.gnu.linker.input">
+									<additionalInput kind="additionalinputdependency" paths="$(USER_OBJS)"/>
+									<additionalInput kind="additionalinput" paths="$(LIBS)"/>
+								</inputType>
+								<inputType id="xilinx.gnu.linker.input.lscript.574206172" name="Linker Script" superClass="xilinx.gnu.linker.input.lscript"/>
+							</tool>
+							<tool id="xilinx.gnu.arm.cxx.toolchain.linker.release.1512126497" name="ARM g++ linker" superClass="xilinx.gnu.arm.cxx.toolchain.linker.release">
+								<option id="xilinx.gnu.linker.inferred.swplatform.lpath.1491656562" name="Software Platform Library Path" superClass="xilinx.gnu.linker.inferred.swplatform.lpath" valueType="libPaths">
+									<listOptionValue builtIn="false" value="../../system_bsp/ps7_cortexa9_0/lib"/>
+								</option>
+								<option id="xilinx.gnu.linker.inferred.swplatform.flags.1353247076" name="Software Platform Inferred Flags" superClass="xilinx.gnu.linker.inferred.swplatform.flags" valueType="libs">
+									<listOptionValue builtIn="false" value="-Wl,--start-group,-lxil,-lgcc,-lc,--end-group"/>
+								</option>
+								<option id="xilinx.gnu.c.linker.option.lscript.486002186" name="Linker Script" superClass="xilinx.gnu.c.linker.option.lscript" value="../src/lscript.ld" valueType="string"/>
+							</tool>
+							<tool id="xilinx.gnu.arm.size.release.362029751" name="ARM Print Size" superClass="xilinx.gnu.arm.size.release"/>
+						</toolChain>
+					</folderInfo>
+				</configuration>
+			</storageModule>
+			<storageModule moduleId="org.eclipse.cdt.core.externalSettings"/>
+		</cconfiguration>
+	</storageModule>
+	<storageModule moduleId="org.eclipse.cdt.core.LanguageSettingsProviders"/>
+	<storageModule moduleId="cdtBuildSystem" version="4.0.0">
+		<project id="modular_quad_pid.xilinx.gnu.arm.exe.832182557" name="Xilinx ARM Executable" projectType="xilinx.gnu.arm.exe"/>
+	</storageModule>
+	<storageModule moduleId="scannerConfiguration">
+		<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId=""/>
+		<scannerConfigBuildInfo instanceId="xilinx.gnu.arm.exe.debug.980189137;xilinx.gnu.arm.exe.debug.980189137.">
+			<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId="com.xilinx.managedbuilder.ui.ARMGCCManagedMakePerProjectProfileC"/>
+		</scannerConfigBuildInfo>
+		<scannerConfigBuildInfo instanceId="xilinx.gnu.arm.exe.release.255973624;xilinx.gnu.arm.exe.release.255973624.;xilinx.gnu.arm.c.toolchain.compiler.release.85270120;xilinx.gnu.arm.c.compiler.input.846429887">
+			<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId="com.xilinx.managedbuilder.ui.ARMGCCManagedMakePerProjectProfileC"/>
+		</scannerConfigBuildInfo>
+		<scannerConfigBuildInfo instanceId="xilinx.gnu.arm.exe.release.255973624;xilinx.gnu.arm.exe.release.255973624.">
+			<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId="com.xilinx.managedbuilder.ui.ARMGCCManagedMakePerProjectProfileC"/>
+		</scannerConfigBuildInfo>
+		<scannerConfigBuildInfo instanceId="xilinx.gnu.arm.exe.debug.980189137;xilinx.gnu.arm.exe.debug.980189137.;xilinx.gnu.arm.c.toolchain.compiler.debug.177835003;xilinx.gnu.arm.c.compiler.input.909725989">
+			<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId="com.xilinx.managedbuilder.ui.ARMGCCManagedMakePerProjectProfileC"/>
+		</scannerConfigBuildInfo>
+	</storageModule>
+	<storageModule moduleId="refreshScope" versionNumber="2">
+		<configuration configurationName="Release">
+			<resource resourceType="PROJECT" workspacePath="/modular_quad_pid"/>
+		</configuration>
+		<configuration configurationName="Debug">
+			<resource resourceType="PROJECT" workspacePath="/modular_quad_pid"/>
+		</configuration>
+	</storageModule>
+	<storageModule moduleId="org.eclipse.cdt.internal.ui.text.commentOwnerProjectMappings"/>
+</cproject>
--- a/quad/sw/imu_logger/.gitignore
+++ b/quad/sw/imu_logger/.gitignore
+Debug/
+bootimage/
--- a/src/vrpn/android_widgets/vrpn_library/.project
+++ b/src/vrpn/android_widgets/vrpn_library/.project
 <?xml version="1.0" encoding="UTF-8"?>
 <projectDescription>
-	<name>vrpn_library</name>
+	<name>imu_logger</name>
 	<comment></comment>
 	<projects>
+		<project>system_bsp_new</project>
+		<project>system_bsp</project>
 	</projects>
 	<buildSpec>
 		<buildCommand>
-			<name>com.android.ide.eclipse.adt.ResourceManagerBuilder</name>
+			<name>org.eclipse.cdt.managedbuilder.core.genmakebuilder</name>
 			<arguments>
 			</arguments>
 		</buildCommand>
 		<buildCommand>
-			<name>com.android.ide.eclipse.adt.PreCompilerBuilder</name>
-			<arguments>
-			</arguments>
-		</buildCommand>
-		<buildCommand>
-			<name>org.eclipse.jdt.core.javabuilder</name>
-			<arguments>
-			</arguments>
-		</buildCommand>
-		<buildCommand>
-			<name>com.android.ide.eclipse.adt.ApkBuilder</name>
+			<name>org.eclipse.cdt.managedbuilder.core.ScannerConfigBuilder</name>
+			<triggers>full,incremental,</triggers>
 			<arguments>
 			</arguments>
 		</buildCommand>
 	</buildSpec>
 	<natures>
-		<nature>com.android.ide.eclipse.adt.AndroidNature</nature>
-		<nature>org.eclipse.jdt.core.javanature</nature>
+		<nature>org.eclipse.cdt.core.cnature</nature>
+		<nature>org.eclipse.cdt.managedbuilder.core.managedBuildNature</nature>
+		<nature>org.eclipse.cdt.managedbuilder.core.ScannerConfigNature</nature>
 	</natures>
 </projectDescription>
--- a/quad/sw/imu_logger/src/Copy of original lscript.ld
+++ b/quad/sw/imu_logger/src/Copy of original lscript.ld
+/*******************************************************************/
+/*                                                                 */
+/* This file is automatically generated by linker script generator.*/
+/*                                                                 */
+/* Version: Xilinx EDK 14.7 EDK_P.20131013                                */
+/*                                                                 */
+/* Copyright (c) 2010 Xilinx, Inc.  All rights reserved.           */
+/*                                                                 */
+/* Description : Cortex-A9 Linker Script                          */
+/*                                                                 */
+/*******************************************************************/
+
+_STACK_SIZE = DEFINED(_STACK_SIZE) ? _STACK_SIZE : 0x2000;
+_HEAP_SIZE = DEFINED(_HEAP_SIZE) ? _HEAP_SIZE : 0x2000;
+
+_ABORT_STACK_SIZE = DEFINED(_ABORT_STACK_SIZE) ? _ABORT_STACK_SIZE : 1024;
+_SUPERVISOR_STACK_SIZE = DEFINED(_SUPERVISOR_STACK_SIZE) ? _SUPERVISOR_STACK_SIZE : 2048;
+_FIQ_STACK_SIZE = DEFINED(_FIQ_STACK_SIZE) ? _FIQ_STACK_SIZE : 1024;
+_UNDEF_STACK_SIZE = DEFINED(_UNDEF_STACK_SIZE) ? _UNDEF_STACK_SIZE : 1024;
+
+/* Define Memories in the system */
+
+MEMORY
+{
+   ps7_ddr_0_S_AXI_BASEADDR : ORIGIN = 0x00100000, LENGTH = 0x1FF00000
+   ps7_ram_0_S_AXI_BASEADDR : ORIGIN = 0x00000000, LENGTH = 0x00030000
+   ps7_ram_1_S_AXI_BASEADDR : ORIGIN = 0xFFFF0000, LENGTH = 0x0000FE00
+}
+
+/* Specify the default entry point to the program */
+
+ENTRY(_vector_table)
+
+/* Define the sections, and where they are mapped in memory */
+
+SECTIONS
+{
+.text : {
+   *(.vectors)
+   *(.boot)
+   *(.text)
+   *(.text.*)
+   *(.gnu.linkonce.t.*)
+   *(.plt)
+   *(.gnu_warning)
+   *(.gcc_execpt_table)
+   *(.glue_7)
+   *(.glue_7t)
+   *(.vfp11_veneer)
+   *(.ARM.extab)
+   *(.gnu.linkonce.armextab.*)
+} > ps7_ddr_0_S_AXI_BASEADDR
+
+.init : {
+   KEEP (*(.init))
+} > ps7_ddr_0_S_AXI_BASEADDR
+
+.fini : {
+   KEEP (*(.fini))
+} > ps7_ddr_0_S_AXI_BASEADDR
+
+.rodata : {
+   __rodata_start = .;
+   *(.rodata)
+   *(.rodata.*)
+   *(.gnu.linkonce.r.*)
+   __rodata_end = .;
+} > ps7_ddr_0_S_AXI_BASEADDR
+
+.rodata1 : {
+   __rodata1_start = .;
+   *(.rodata1)
+   *(.rodata1.*)
+   __rodata1_end = .;
+} > ps7_ddr_0_S_AXI_BASEADDR
+
+.sdata2 : {
+   __sdata2_start = .;
+   *(.sdata2)
+   *(.sdata2.*)
+   *(.gnu.linkonce.s2.*)
+   __sdata2_end = .;
+} > ps7_ddr_0_S_AXI_BASEADDR
+
+.sbss2 : {
+   __sbss2_start = .;
+   *(.sbss2)
+   *(.sbss2.*)
+   *(.gnu.linkonce.sb2.*)
+   __sbss2_end = .;
+} > ps7_ddr_0_S_AXI_BASEADDR
+
+.data : {
+   __data_start = .;
+   *(.data)
+   *(.data.*)
+   *(.gnu.linkonce.d.*)
+   *(.jcr)
+   *(.got)
+   *(.got.plt)
+   __data_end = .;
+} > ps7_ddr_0_S_AXI_BASEADDR
+
+.data1 : {
+   __data1_start = .;
+   *(.data1)
+   *(.data1.*)
+   __data1_end = .;
+} > ps7_ddr_0_S_AXI_BASEADDR
+
+.got : {
+   *(.got)
+} > ps7_ddr_0_S_AXI_BASEADDR
+
+.ctors : {
+   __CTOR_LIST__ = .;
+   ___CTORS_LIST___ = .;
+   KEEP (*crtbegin.o(.ctors))
+   KEEP (*(EXCLUDE_FILE(*crtend.o) .ctors))
+   KEEP (*(SORT(.ctors.*)))
+   KEEP (*(.ctors))
+   __CTOR_END__ = .;
+   ___CTORS_END___ = .;
+} > ps7_ddr_0_S_AXI_BASEADDR
+
+.dtors : {
+   __DTOR_LIST__ = .;
+   ___DTORS_LIST___ = .;
+   KEEP (*crtbegin.o(.dtors))
+   KEEP (*(EXCLUDE_FILE(*crtend.o) .dtors))
+   KEEP (*(SORT(.dtors.*)))
+   KEEP (*(.dtors))
+   __DTOR_END__ = .;
+   ___DTORS_END___ = .;
+} > ps7_ddr_0_S_AXI_BASEADDR
+
+.fixup : {
+   __fixup_start = .;
+   *(.fixup)
+   __fixup_end = .;
+} > ps7_ddr_0_S_AXI_BASEADDR
+
+.eh_frame : {
+   *(.eh_frame)
+} > ps7_ddr_0_S_AXI_BASEADDR
+
+.eh_framehdr : {
+   __eh_framehdr_start = .;
+   *(.eh_framehdr)
+   __eh_framehdr_end = .;
+} > ps7_ddr_0_S_AXI_BASEADDR
+
+.gcc_except_table : {
+   *(.gcc_except_table)
+} > ps7_ddr_0_S_AXI_BASEADDR
+
+.mmu_tbl (ALIGN(16384)) : {
+   __mmu_tbl_start = .;
+   *(.mmu_tbl)
+   __mmu_tbl_end = .;
+} > ps7_ddr_0_S_AXI_BASEADDR
+
+.ARM.exidx : {
+   __exidx_start = .;
+   *(.ARM.exidx*)
+   *(.gnu.linkonce.armexidix.*.*)
+   __exidx_end = .;
+} > ps7_ddr_0_S_AXI_BASEADDR
+
+.preinit_array : {
+   __preinit_array_start = .;
+   KEEP (*(SORT(.preinit_array.*)))
+   KEEP (*(.preinit_array))
+   __preinit_array_end = .;
+} > ps7_ddr_0_S_AXI_BASEADDR
+
+.init_array : {
+   __init_array_start = .;
+   KEEP (*(SORT(.init_array.*)))
+   KEEP (*(.init_array))
+   __init_array_end = .;
+} > ps7_ddr_0_S_AXI_BASEADDR
+
+.fini_array : {
+   __fini_array_start = .;
+   KEEP (*(SORT(.fini_array.*)))
+   KEEP (*(.fini_array))
+   __fini_array_end = .;
+} > ps7_ddr_0_S_AXI_BASEADDR
+
+.ARM.attributes : {
+   __ARM.attributes_start = .;
+   *(.ARM.attributes)
+   __ARM.attributes_end = .;
+} > ps7_ddr_0_S_AXI_BASEADDR
+
+.sdata : {
+   __sdata_start = .;
+   *(.sdata)
+   *(.sdata.*)
+   *(.gnu.linkonce.s.*)
+   __sdata_end = .;
+} > ps7_ddr_0_S_AXI_BASEADDR
+
+.sbss (NOLOAD) : {
+   __sbss_start = .;
+   *(.sbss)
+   *(.sbss.*)
+   *(.gnu.linkonce.sb.*)
+   __sbss_end = .;
+} > ps7_ddr_0_S_AXI_BASEADDR
+
+.tdata : {
+   __tdata_start = .;
+   *(.tdata)
+   *(.tdata.*)
+   *(.gnu.linkonce.td.*)
+   __tdata_end = .;
+} > ps7_ddr_0_S_AXI_BASEADDR
+
+.tbss : {
+   __tbss_start = .;
+   *(.tbss)
+   *(.tbss.*)
+   *(.gnu.linkonce.tb.*)
+   __tbss_end = .;
+} > ps7_ddr_0_S_AXI_BASEADDR
+
+.bss (NOLOAD) : {
+   __bss_start = .;
+   *(.bss)
+   *(.bss.*)
+   *(.gnu.linkonce.b.*)
+   *(COMMON)
+   __bss_end = .;
+} > ps7_ddr_0_S_AXI_BASEADDR
+
+_SDA_BASE_ = __sdata_start + ((__sbss_end - __sdata_start) / 2 );
+
+_SDA2_BASE_ = __sdata2_start + ((__sbss2_end - __sdata2_start) / 2 );
+
+/* Generate Stack and Heap definitions */
+
+.heap (NOLOAD) : {
+   . = ALIGN(16);
+   _heap = .;
+   HeapBase = .;
+   _heap_start = .;
+   . += _HEAP_SIZE;
+   _heap_end = .;
+   HeapLimit = .;
+} > ps7_ddr_0_S_AXI_BASEADDR
+
+.stack (NOLOAD) : {
+   . = ALIGN(16);
+   _stack_end = .;
+   . += _STACK_SIZE;
+   _stack = .;
+   __stack = _stack;
+   . = ALIGN(16);
+   _irq_stack_end = .;
+   . += _STACK_SIZE;
+   __irq_stack = .;
+   _supervisor_stack_end = .;
+   . += _SUPERVISOR_STACK_SIZE;
+   . = ALIGN(16);
+   __supervisor_stack = .;
+   _abort_stack_end = .;
+   . += _ABORT_STACK_SIZE;
+   . = ALIGN(16);
+   __abort_stack = .;
+   _fiq_stack_end = .;
+   . += _FIQ_STACK_SIZE;
+   . = ALIGN(16);
+   __fiq_stack = .;
+   _undef_stack_end = .;
+   . += _UNDEF_STACK_SIZE;
+   . = ALIGN(16);
+   __undef_stack = .;
+} > ps7_ddr_0_S_AXI_BASEADDR
+
+_end = .;
+}
+
--- a/quad/sw/imu_logger/src/PID.c
+++ b/quad/sw/imu_logger/src/PID.c
+/*
+ * PID.c
+ *
+ *  Created on: Nov 10, 2014
+ *      Author: ucart
+ */
+
+#include "PID.h"
+#include <math.h>
+#include <float.h>
+
+// The generic PID diagram. This function takes in pid parameters (PID_t * pid) and calculates the output "pid_correction"
+// part based on those parameters.
+//
+//              +  ---    error    ------------------		P		+  ---			  ----------------------------
+// setpoint ---> / sum \ --------->| Kp * error	    |--------------->/ sum \ -------->| output: "pid_correction" |
+//				 \     /	|	   ------------------				 \	   /		  ----------------------------
+//				   ---		|										   ---					||
+//                -	^       |	   									+ ^  ^ +				||
+//					|		|	   -------------------------------	  |	 |			------- \/------------
+//					|		|----->| Ki * accumulated error * dt |----+	 |			|					 |
+//					|		|	   -------------------------------	I	 |			|		SYSTEM		 |
+//					|		|											 |			|					 |
+//					|		|											 |			--------||------------
+//					|		|											 |					||
+//					|		|      ----------------------------------	 |					||
+//					|		|----->| Kd * (error - last error) / dt |----+					||
+//				    |			   ----------------------------------  D					||
+//					|																		||
+//					|															 -----------\/-----------
+//					|____________________________________________________________| Sensor measurements: |
+//																				 |	 "current point"	|
+//																				 ------------------------
+//
+PID_values pid_computation(PID_t *pid) {
+
+	float P = 0.0, I = 0.0, D = 0.0;
+
+	// calculate the current error
+	float error = pid->setpoint - pid->current_point;
+
+	// Accumulate the error (if Ki is less than epsilon, rougly 0,
+	// then reset the accumulated error for safety)
+	if (fabs(pid->Ki) <= FLT_EPSILON) {
+		pid->acc_error = 0;
+	} else {
+		pid->acc_error += error;
+	}
+
+	float change_in_error = error - pid->prev_error;
+
+	// Compute each term's contribution
+	P = pid->Kp * error;
+	I = pid->Ki * pid->acc_error * pid->dt;
+	D = pid->Kd * (change_in_error / pid->dt);
+
+	PID_values ret = {P, I, D, error, change_in_error, P + I + D};
+
+	pid->prev_error = error; // Store the current error into the PID_t
+
+	pid->pid_correction = P + I + D; // Store the computed correction
+	return ret;
+}
--- a/quad/sw/imu_logger/src/PID.h
+++ b/quad/sw/imu_logger/src/PID.h
+/*
+ * PID.h
+ *
+ *  Created on: Nov 10, 2014
+ *      Author: ucart
+ */
+
+#ifndef PID_H_
+#define PID_H_
+
+#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
+
+// when using units of radians
+#define YAW_ANGULAR_VELOCITY_KP 190.0f * 2292.0f//200.0f * 2292.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
+
+// 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_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_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
+
+
+//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_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_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
+
+
+//Throttle constants
+#define ALT_ZPOS_KP 9804.0f
+#define ALT_ZPOS_KI 817.0f
+#define ALT_ZPOS_KD 7353.0f
+
+// Computes control error and correction
+PID_values pid_computation(PID_t *pid);
+
+#endif /* PID_H_ */
--- a/quad/sw/imu_logger/src/README.txt
+++ b/quad/sw/imu_logger/src/README.txt
+This application is the PID implementation of the modular control loop. This is the same implementation as the existing quad controller. The mixer in this application needs to be changed to be correct according to common implementation.
--- a/quad/sw/imu_logger/src/actuator_command_processing.c
+++ b/quad/sw/imu_logger/src/actuator_command_processing.c
+/*
+ * actuator_command_processing.c
+ *
+ *  Created on: Feb 20, 2016
+ *      Author: ucart
+ */
+ 
+#include "actuator_command_processing.h"
+#include "sensor_processing.h"
+
+int actuator_command_processing(log_t* log_struct, user_input_t * user_input_struct, raw_actuator_t* raw_actuator_struct, actuator_command_t* actuator_command_struct)
+{
+
+	Aero_to_PWMS(actuator_command_struct->pwms, raw_actuator_struct->controller_corrected_motor_commands);
+//	old_Aero_to_PWMS(actuator_command_struct->pwms, raw_actuator_struct->controller_corrected_motor_commands);
+
+    return 0;
+}
+
+/**
+ * Converts Aero 4 channel signals to PWM signals
+ * Aero channels are defined above
+ */
+void Aero_to_PWMS(int* PWMs, int* aero)
+{
+	int motor0_bias = 0, motor1_bias = 0, motor2_bias = 0, motor3_bias = 0;
+	int pwm0 = 0, pwm1 = 0, pwm2 = 0, pwm3 = 0;
+
+	pwm0 = aero[THROTTLE] - aero[PITCH] - aero[ROLL] - aero[YAW] + motor0_bias;
+	pwm1 = aero[THROTTLE] + aero[PITCH] - aero[ROLL] + aero[YAW] + motor1_bias;
+	pwm2 = aero[THROTTLE] - aero[PITCH] + aero[ROLL] + aero[YAW] + motor2_bias;
+	pwm3 = aero[THROTTLE] + aero[PITCH] + aero[ROLL] - aero[YAW] + motor3_bias;
+
+//	printf("pwm0: %d\tpwm1: %d\tpwm2: %d\tpwm3: %d\n", pwm0, pwm1, pwm2, pwm3);
+
+	/**
+	 * Boundary checks:
+	 *
+	 *  #define min 100000
+	 *	#define max 200000
+	 */
+
+	if(pwm0 < min)
+		pwm0 = min;
+	else if(pwm0 > max)
+		pwm0 = max;
+
+	if(pwm1 < min)
+		pwm1 = min;
+	else if(pwm1 > max)
+		pwm1 = max;
+
+	if(pwm2 < min)
+		pwm2 = min;
+	else if(pwm2 > max)
+		pwm2 = max;
+
+	if(pwm3 < min)
+		pwm3 = min;
+	else if(pwm3 > max)
+		pwm3 = max;
+
+	PWMs[0] = pwm0;
+	PWMs[1] = pwm1;
+	PWMs[2] = pwm2;
+	PWMs[3] = pwm3;
+
+	// the array PWMs is then written directly to the PWM hardware registers
+	// the PWMs are in units of clock cycles, not percentage duty cycle
+	// use pwm/222,222 to get the duty cycle. the freq is 450 Hz on a 100MHz clock
+}
+
+/**
+ * Converts Aero 4 channel signals to PWM signals
+ * Aero channels are defined above
+ *
+ * *deprecated
+ */
+void old_Aero_to_PWMS(int* PWMs, int* aero) {
+
+	int motor0_bias = -9900, motor1_bias = -200, motor2_bias = -10200, motor3_bias = 250;
+//	int motor0_bias = -5000, motor1_bias = 0, motor2_bias = -5000, motor3_bias = 0;
+
+	// Throttle, pitch, roll, yaw as a percentage of their max - Range 0.0 - 100.0
+	float throttle_100 = (aero[THROTTLE] - THROTTLE_MIN) / (THROTTLE_RANGE*1.0);
+	float pitch_100    = (aero[PITCH]    - PITCH_MIN)    / (PITCH_RANGE*1.0);
+	float roll_100     = (aero[ROLL]     - ROLL_MIN)     / (ROLL_RANGE*1.0);
+	float yaw_100      = (aero[YAW]      - YAW_MIN)      / (YAW_RANGE*1.0);
+
+	// This adds a +/- 300 ms range bias for the throttle
+	int throttle_base = BASE + (int) 60000 * (throttle_100 - .5);
+	// This adds a +/- 200 ms range bias for the pitch
+	int pitch_base    =        (int) 60000 * (pitch_100    - .5);
+	// This adds a +/- 200 ms range bias for the roll
+	int roll_base     =        (int) 60000 * (roll_100     - .5);
+	// This adds a +/- 75 ms range bias for the yaw
+	int yaw_base      =        (int) 15000 * (yaw_100      - .5);
+
+	int pwm0, pwm1, pwm2, pwm3;
+
+	pwm1 = throttle_base + pitch_base/2 - roll_base/2 + yaw_base + motor1_bias;
+	pwm3 = throttle_base + pitch_base/2 + roll_base/2 - yaw_base + motor3_bias;
+	pwm0 = throttle_base - pitch_base/2 - roll_base/2 - yaw_base + motor0_bias;
+	pwm2 = throttle_base - pitch_base/2 + roll_base/2 + yaw_base + motor2_bias;
+
+	/**
+	 * Boundary checks:
+	 *
+	 *  #define min 100000
+	 *	#define max 200000
+	 */
+
+	if(pwm0 < min)
+		pwm0 = min;
+	else if(pwm0 > max)
+		pwm0 = max;
+
+	if(pwm1 < min)
+		pwm1 = min;
+	else if(pwm1 > max)
+		pwm1 = max;
+
+	if(pwm2 < min)
+		pwm2 = min;
+	else if(pwm2 > max)
+		pwm2 = max;
+
+	if(pwm3 < min)
+		pwm3 = min;
+	else if(pwm3 > max)
+		pwm3 = max;
+
+	PWMs[0] = pwm0;
+	PWMs[1] = pwm1;
+	PWMs[2] = pwm2;
+	PWMs[3] = pwm3;
+
+	// the array PWMs is then written directly to the PWM hardware registers
+	// the PWMs are in units of clock cycles, not percentage duty cycle
+	// use pwm/222,222 to get the duty cycle. the freq is 450 Hz on a 100MHz clock
+
+}
--- a/quad/sw/imu_logger/src/actuator_command_processing.h
+++ b/quad/sw/imu_logger/src/actuator_command_processing.h
+/*
+ * actuator_command_processing.h
+ *
+ *  Created on: Feb 20, 2016
+ *      Author: ucart
+ */
+
+#ifndef ACTUATOR_COMMAND_PROCESSING_H_
+#define ACTUATOR_COMMAND_PROCESSING_H_
+ 
+#include <stdio.h>
+
+#include "log_data.h"
+#include "control_algorithm.h"
+
+/**
+ * @brief
+ *      Processes the commands to the actuators.
+ *
+ * @param log_struct
+ *      structure of the data to be logged
+ *
+ * @param raw_actuator_struct
+ *      structure of the commmands outputted to go to the actuators
+ *
+ * @param actuator_command_struct
+ *      structure of the commmands to go to the actuators
+ *
+ * @return 
+ *      error message
+ *
+ */
+int actuator_command_processing(log_t* log_struct, user_input_t * user_input_struct, raw_actuator_t* raw_actuator_struct, actuator_command_t* actuator_command_struct);
+
+void old_Aero_to_PWMS(int* PWMs, int* aero);
+void Aero_to_PWMS(int* PWMs, int* aero);
+
+#endif /* ACTUATOR_COMMAND_PROCESSING_H_ */
--- a/quad/sw/imu_logger/src/commands.c
+++ b/quad/sw/imu_logger/src/commands.c
+#include "communication.h"
+#include "commands.h"
+#include "type_def.h"
+#include "uart.h"
+
+struct MessageType MessageTypes[MAX_TYPE] = 
+{
+	// DEBUG
+	{
+		// Message Type ID
+		0x00,
+		
+		// Debug Subtypes
+		{
+			// NONE subtype
+			{
+				// ID
+				0x00,
+				// Command text
+				"debug",
+				// Type of the command data
+				stringType,
+				// Function pointer
+				&debug
+			},
+
+			// Echo subtype
+			{
+				// ID
+				0x01,
+				// Command text
+				"ack",
+				// Type of the command data
+				intType,
+				// Function pointer
+				NULL
+			}
+		}
+	},
+	
+	// CALIBRATION
+	{
+		// Message Type ID
+		0x01,
+		
+		// Calibration Subtypes
+		{
+			// yaw setpoint subtype
+			{
+				// ID
+				0x00,
+				// Command text
+				"setyaw",
+				// Type of the command data
+				floatType,
+				// Function pointer
+				&yawset
+			},
+			// yaw p constant subtype
+			{
+				// ID
+				0x01,
+				// Command text
+				"setyawp",
+				// Type of the command data
+				floatType,
+				// Function pointer
+				&yawp
+			},
+			// yaw d constant subtype
+			{
+				// ID
+				0x02,
+				// Command text
+				"setyawd",
+				// Type of the command data
+				floatType,
+				// Function pointer
+				&yawd
+			},
+			// roll setpoint subtype
+			{
+				// ID
+				0x03,
+				// Command text
+				"setroll",
+				// Type of the command data
+				floatType,
+				// Function pointer
+				&rollset
+			},
+			// roll p constant subtype
+			{
+				// ID
+				0x04,
+				// Command text
+				"setrollp",
+				// Type of the command data
+				floatType,
+				// Function pointer
+				&rollp
+			},
+			// roll d constant subtype
+			{
+				// ID
+				0x05,
+				// Command text
+				"setrolld",
+				// Type of the command data
+				floatType,
+				// Function pointer
+				&rolld
+			},
+			// pitch setpoint subtype
+			{
+				// ID
+				0x06,
+				// Command text
+				"setpitch",
+				// Type of the command data
+				floatType,
+				// Function pointer
+				&pitchset
+			},
+			// pitch p constant subtype
+			{
+				// ID
+				0x07,
+				// Command text
+				"setpitchp",
+				// Type of the command data
+				floatType,
+				// Function pointer
+				&pitchp
+			},
+			// pitch d constant subtype
+			{
+				// ID
+				0x08,
+				// Command text
+				"setpitchd",
+				// Type of the command data
+				floatType,
+				// Function pointer
+				&pitchd
+			},
+			// throttle setpoint subtype
+			{
+				// ID
+				0x09,
+				// Command text
+				"setthrottle",
+				// Type of the command data
+				floatType,
+				// Function pointer
+				&throttleset
+			},
+			// throttle p constant subtype
+			{
+				// ID
+				0x0A,
+				// Command text
+				"setthrottlep",
+				// Type of the command data
+				floatType,
+				// Function pointer
+				&throttlep
+			},
+			// throttle i constant subtype
+			{
+				// ID
+				0x0B,
+				// Command text
+				"setthrottlei",
+				// Type of the command data
+				floatType,
+				// Function pointer
+				&throttlei
+			},
+			// throttle d constant subtype
+			{
+				// ID
+				0x0C,
+				// Command text
+				"setthrottled",
+				// Type of the command data
+				floatType,
+				// Function pointer
+				&throttled
+			}
+		}
+	},
+	
+	// REQUEST
+	{
+		// Message Type ID
+		0x02,
+		
+		// Request Subtypes
+		{
+			// accelerometer subtype
+			{
+				// ID 
+				0x00,
+				// Command text
+				"accelreq",
+				// Type of the command data
+				floatType,
+				// Function pointer
+				&accelreq
+			},
+			// gyroscope subtype
+			{
+				// ID 
+				0x01,
+				// Command text
+				"gyroreq",
+				// Type of the command data
+				floatType,
+				// Function pointer
+				&gyroreq
+			},
+			// pitch angle subtype
+			{
+				// ID 
+				0x02,
+				// Command text
+				"reqpitchangle",
+				// Type of the command data
+				floatType,
+				// Function pointer
+				&pitchanglereq
+			},
+			// roll angle subtype
+			{
+				// ID 
+				0x03,
+				// Command text
+				"reqrollangle",
+				// Type of the command data
+				floatType,
+				// Function pointer
+				&rollanglereq
+			}
+		}
+	},
+	
+	// RESPONSE
+	{
+		// Message Type ID
+		0x03,
+		
+		// Response Subtypes
+		{
+			// accelerometer subtype
+			{
+				// ID
+				0x00,
+				// Command text
+				"respaccel",
+				// Type of the command data
+				floatType,
+				// Function pointer
+				&accelresp
+			},
+			// gyroscope subtype
+			{
+				// ID
+				0x01,
+				// Command text
+				"respgyro",
+				// Type of the command data
+				floatType,
+				// Function pointer
+				&gyroresp
+			},
+			// pitch angle subtype
+			{
+				// ID 
+				0x02,
+				// Command text
+				"resppitchangle",
+				// Type of the command data
+				floatType,
+				// Function pointer
+				&pitchangleresp
+			},
+			// roll angle subtype
+			{
+				// ID 
+				0x03,
+				// Command text
+				"resprollangle",
+				// Type of the command data
+				floatType,
+				// Function pointer
+				&rollangleresp
+			}
+		}
+	},
+	
+	// UPDATE
+	{
+		// Message Type ID
+		0x04,
+		
+		// Update Subtypes
+		{
+			// NONE subtype
+			{
+				// ID 
+				0x00,
+				// Command text
+				"update",
+				// Type of the command data
+				stringType,
+				// Function pointer
+				&update
+			},
+			// BEGIN UPDATE subtype
+			{
+				// ID
+				0x01,
+				// Command text
+				"beginupdate",
+				// Type of the command data
+				stringType,
+				// Function pointer
+				&beginupdate
+			}
+		}
+	},
+	
+	// LOG
+	{
+		// Message Type ID
+		0x05,
+		
+		// Log Subtypes
+		{
+			// NONE subtype
+			{
+				// ID
+				0x00,
+				// Command text
+				"log",
+				// Type of the command data
+				stringType,
+				// Function pointer
+				&logdata
+			},
+			// Response subtype
+			{
+				// ID
+				0x01,
+				// Command text
+				"response",
+				// Type of the command data
+				stringType,
+				// Function pointer
+				&response
+			}
+		}
+	},
+	
+};
+
+int debug(unsigned char *packet, int dataLen, modular_structs_t *structs)
+{
+	printf("function for debug\n");
+	return 0;
+}
+
+/* This is not used. Many flow changes would be need to be made to the
+	control algorithm in order to get this to work. */
+int update(unsigned char *packet, int dataLen,	modular_structs_t *structs)
+{
+	printf("function for debug\n");
+	return 0;
+}
+
+// This is called on the ground station to begin sending VRPN to the quad
+int beginupdate(unsigned char *c, int dataLen, modular_structs_t *structs) {
+	return 0;
+}
+
+int logdata(unsigned char *packet, int dataLen, modular_structs_t *structs)
+{
+	printf("Logging: %s\n", packet);
+	return 0;
+}
+
+int response(unsigned char *packet, int dataLen, modular_structs_t *structs)
+{
+	printf("This is the response: %s\n", packet);
+	
+	return 0;
+}
+
+// ------------------------------------------------------------------
+// Quad side implementation
+
+// TODO: Erase memory leaks
+
+int yawset(unsigned char *packet, int dataLen, modular_structs_t *structs)
+{
+	float value;
+	char buf[255] = {};
+	int i;
+	
+	// Copy the data into the value variable
+	memcpy(&value, ((float *)packet), dataLen);
+	
+	// Update the struct
+	structs->setpoint_struct.desiredQuadPosition.yaw = value;
+
+	// Debug print statement
+	//printf("function for yawset: %f\n", structs->setpoint_struct.desiredQuadPosition.yaw);
+	
+	// Send a reply to the ground station
+	snprintf(buf, sizeof(buf), "Successfully set desired yaw to %.2f radians\r\n", structs->setpoint_struct.desiredQuadPosition.yaw);
+	unsigned char *responsePacket;
+
+	printf("%s\n", buf);
+
+	metadata_t metadata =
+	{
+		BEGIN_CHAR,
+		MessageTypes[5].ID,
+		MessageTypes[5].subtypes[1].ID,
+		0,
+		(strlen(buf) + 1)
+	};
+	formatPacket(&metadata, buf, &responsePacket);
+
+	// Send each byte of the packet individually
+	for(i = 0; i < 8 + metadata.data_len; i++) {
+		// Debug print statement for all of the bytes being sent
+		printf("%d: 0x%x\n", i, responsePacket[i]);
+
+		uart0_sendByte(responsePacket[i]);
+	}
+
+	return 0;
+}
+
+int yawp(unsigned char *packet, int dataLen,  modular_structs_t *structs)
+{
+	float value;
+	char buf[255] = {0};
+	int i;
+	
+	memcpy(&value, ((float *)packet), dataLen);
+	structs->parameter_struct.yaw_angle_pid.Kp = value;
+	
+	printf("function for yawp: %f\n", structs->parameter_struct.yaw_angle_pid.Kp);
+	
+	// Send a reply to the ground station
+	snprintf(buf, sizeof(buf), "Successfully set yaw Kp to %.2f\r\n", structs->parameter_struct.yaw_angle_pid.Kp);
+	unsigned char *responsePacket;
+
+	printf("%s\n", buf);
+
+	metadata_t metadata =
+	{
+		BEGIN_CHAR,
+		MessageTypes[5].ID,
+		MessageTypes[5].subtypes[1].ID,
+		0,
+		(strlen(buf) + 1)
+	};
+	formatPacket(&metadata, buf, &responsePacket);
+
+	// Send each byte of the packet individually
+	for(i = 0; i < 8 + metadata.data_len; i++) {
+		// Debug print statement for all of the bytes being sent
+		//printf("%d: 0x%x\n", i, packet2[i]);
+
+		uart0_sendByte(responsePacket[i]);
+	}
+
+	return 0;
+}
+
+int yawd(unsigned char *packet, int dataLen,  modular_structs_t *structs)
+{
+	float value;
+	char buf[255] = {};
+	int i;
+	
+	memcpy(&value, ((float *)packet), dataLen);
+	structs->parameter_struct.yaw_angle_pid.Kd = value;
+	
+	printf("function for yawd: %f\n", structs->parameter_struct.yaw_angle_pid.Kd);
+	
+	// Send a reply to the ground station
+	snprintf(buf, sizeof(buf), "Successfully set yaw Kd to %.2f\r\n", structs->parameter_struct.yaw_angle_pid.Kd);
+	unsigned char *responsePacket;
+
+	printf("%s\n", buf);
+
+	metadata_t metadata =
+	{
+		BEGIN_CHAR,
+		MessageTypes[5].ID,
+		MessageTypes[5].subtypes[1].ID,
+		0,
+		(strlen(buf) + 1)
+	};
+	formatPacket(&metadata, buf, &responsePacket);
+
+	// Send each byte of the packet individually
+	for(i = 0; i < 8 + metadata.data_len; i++) {
+		// Debug print statement for all of the bytes being sent
+		//printf("%d: 0x%x\n", i, responsePacket[i]);
+
+		uart0_sendByte(responsePacket[i]);
+	}
+
+	return 0;
+}
+
+int rollset(unsigned char *packet, int dataLen,	 modular_structs_t *structs)
+{
+	float value;
+	char buf[255] = {};
+	int i;
+	
+	memcpy(&value, ((float *)packet), dataLen);
+	structs->setpoint_struct.desiredQuadPosition.roll = value;
+	
+	printf("function for rollset: %f\n", structs->setpoint_struct.desiredQuadPosition.roll);
+	
+	// Send a reply to the ground station
+	snprintf(buf, sizeof(buf), "Successfully set desired roll to %.2f radians\r\n", structs->setpoint_struct.desiredQuadPosition.roll);
+	unsigned char *responsePacket;
+
+	metadata_t metadata =
+	{
+		BEGIN_CHAR,
+		MessageTypes[5].ID,
+		MessageTypes[5].subtypes[1].ID,
+		0,
+		(strlen(buf) + 1)
+	};
+	formatPacket(&metadata, buf, &responsePacket);
+
+	// Send each byte of the packet individually
+	for(i = 0; i < 8 + metadata.data_len; i++) {
+		// Debug print statement for all of the bytes being sent
+		//printf("%d: 0x%x\n", i, responsePacket[i]);
+
+		uart0_sendByte(responsePacket[i]);
+	}
+
+	return 0;
+}
+
+int rollp(unsigned char *packet, int dataLen,  modular_structs_t *structs)
+{
+	float value;
+	char buf[255] = {};
+	int i;
+	
+	memcpy(&value, ((float *)packet), dataLen);
+	structs->parameter_struct.roll_angle_pid.Kp = value;
+	
+	printf("function for rollp: %f\n", structs->parameter_struct.roll_angle_pid.Kp);
+	
+	// Send a reply to the ground station
+	snprintf(buf, sizeof(buf), "Successfully set roll Kp to %.2f\r\n", structs->parameter_struct.roll_angle_pid.Kp);
+	unsigned char *responsePacket;
+
+	metadata_t metadata =
+	{
+		BEGIN_CHAR,
+		MessageTypes[5].ID,
+		MessageTypes[5].subtypes[1].ID,
+		0,
+		(strlen(buf) + 1)
+	};
+	formatPacket(&metadata, buf, &responsePacket);
+
+	// Send each byte of the packet individually
+	for(i = 0; i < 8 + metadata.data_len; i++) {
+		// Debug print statement for all of the bytes being sent
+		//printf("%d: 0x%x\n", i, responsePacket[i]);
+
+		uart0_sendByte(responsePacket[i]);
+	}
+
+	return 0;
+}
+
+int rolld(unsigned char *packet, int dataLen,  modular_structs_t *structs)
+{
+	float value;
+	char buf[255] = {};
+	int i;
+	
+	memcpy(&value, ((float *)packet), dataLen);
+	structs->parameter_struct.roll_angle_pid.Kd = value;
+	
+	printf("function for rolld: %f\n", structs->parameter_struct.roll_angle_pid.Kd);
+	
+	// Send a reply to the ground station
+	snprintf(buf, sizeof(buf), "Successfully set roll Kd to %.2f\r\n", structs->parameter_struct.roll_angle_pid.Kd);
+	unsigned char *responsePacket;
+
+	metadata_t metadata =
+	{
+		BEGIN_CHAR,
+		MessageTypes[5].ID,
+		MessageTypes[5].subtypes[1].ID,
+		0,
+		(strlen(buf) + 1)
+	};
+	formatPacket(&metadata, buf, &responsePacket);
+
+	// Send each byte of the packet individually
+	for(i = 0; i < 8 + metadata.data_len; i++) {
+		// Debug print statement for all of the bytes being sent
+		//printf("%d: 0x%x\n", i, responsePacket[i]);
+
+		uart0_sendByte(responsePacket[i]);
+	}
+
+	return 0;
+}
+
+int pitchset(unsigned char *packet, int dataLen,  modular_structs_t *structs)
+{
+	float value;
+	char buf[255] = {};
+	int i;
+	
+	memcpy(&value, ((float *)packet), dataLen);
+	structs->setpoint_struct.desiredQuadPosition.pitch = value;
+	
+	printf("function for pitchset: %f\n", structs->setpoint_struct.desiredQuadPosition.pitch);
+	
+	// Send a reply to the ground station
+	snprintf(buf, sizeof(buf), "Successfully set desired pitch to %.2f radians\r\n", structs->setpoint_struct.desiredQuadPosition.pitch);
+	unsigned char *responsePacket;
+
+	metadata_t metadata =
+	{
+		BEGIN_CHAR,
+		MessageTypes[5].ID,
+		MessageTypes[5].subtypes[1].ID,
+		0,
+		(strlen(buf) + 1)
+	};
+	formatPacket(&metadata, buf, &responsePacket);
+
+	// Send each byte of the packet individually
+	for(i = 0; i < 8 + metadata.data_len; i++) {
+		// Debug print statement for all of the bytes being sent
+		//printf("%d: 0x%x\n", i, responsePacket[i]);
+
+		uart0_sendByte(responsePacket[i]);
+	}
+
+	return 0;
+}
+
+int pitchp(unsigned char *packet, int dataLen,	modular_structs_t *structs)
+{
+	float value;
+	char buf[255] = {};
+	int i;
+	
+	memcpy(&value, ((float *)packet), dataLen);
+	structs->parameter_struct.pitch_angle_pid.Kp = value;
+	
+	printf("function for pitchp: %f\n", structs->parameter_struct.pitch_angle_pid.Kp);
+	
+	// Send a reply to the ground station
+	snprintf(buf, sizeof(buf), "Successfully set pitch Kp to %.2f\r\n", structs->parameter_struct.pitch_angle_pid.Kp);
+	unsigned char *responsePacket;
+
+	metadata_t metadata =
+	{
+		BEGIN_CHAR,
+		MessageTypes[5].ID,
+		MessageTypes[5].subtypes[1].ID,
+		0,
+		(strlen(buf) + 1)
+	};
+	formatPacket(&metadata, buf, &responsePacket);
+
+	// Send each byte of the packet individually
+	for(i = 0; i < 8 + metadata.data_len; i++) {
+		// Debug print statement for all of the bytes being sent
+		//printf("%d: 0x%x\n", i, responsePacket[i]);
+
+		uart0_sendByte(responsePacket[i]);
+	}
+
+	return 0;
+}
+
+int pitchd(unsigned char *packet, int dataLen,	modular_structs_t *structs)
+{
+	float value;
+	char buf[255] = {};
+	int i;
+	
+	memcpy(&value, ((float *)packet), dataLen);
+	structs->parameter_struct.pitch_angle_pid.Kd = value;
+	
+	printf("function for pitchd: %f\n", structs->parameter_struct.pitch_angle_pid.Kd);
+	
+	// Send a reply to the ground station
+	snprintf(buf, sizeof(buf), "Successfully set desired yaw to %.2f\r\n", structs->parameter_struct.pitch_angle_pid.Kd);
+	unsigned char *responsePacket;
+
+	metadata_t metadata =
+	{
+		BEGIN_CHAR,
+		MessageTypes[5].ID,
+		MessageTypes[5].subtypes[1].ID,
+		0,
+		(strlen(buf) + 1)
+	};
+	formatPacket(&metadata, buf, &responsePacket);
+
+	// Send each byte of the packet individually
+	for(i = 0; i < 8 + metadata.data_len; i++) {
+		// Debug print statement for all of the bytes being sent
+		//printf("%d: 0x%x\n", i, responsePacket[i]);
+
+		uart0_sendByte(responsePacket[i]);
+	}
+
+	return 0;
+}
+
+// ------------------------------------------------------------
+// These should be renamed to altitude!
+int throttleset(unsigned char *packet, int dataLen,	 modular_structs_t *structs)
+{
+	float value;
+	char buf[255] = {};
+	int i;
+	
+	memcpy(&value, ((float *)packet), dataLen);
+	structs->setpoint_struct.desiredQuadPosition.alt_pos = value;
+	
+	printf("function for throttleset: %f\n", structs->setpoint_struct.desiredQuadPosition.alt_pos);
+	
+	// Send a reply to the ground station
+	snprintf(buf, sizeof(buf), "Successfully set desired altitude to %.2f meters\r\n", structs->setpoint_struct.desiredQuadPosition.alt_pos);
+	unsigned char *responsePacket;
+
+	metadata_t metadata =
+	{
+		BEGIN_CHAR,
+		MessageTypes[5].ID,
+		MessageTypes[5].subtypes[1].ID,
+		0,
+		(strlen(buf) + 1)
+	};
+	formatPacket(&metadata, buf, &responsePacket);
+
+	// Send each byte of the packet individually
+	for(i = 0; i < 8 + metadata.data_len; i++) {
+		// Debug print statement for all of the bytes being sent
+		//printf("%d: 0x%x\n", i, responsePacket[i]);
+
+		uart0_sendByte(responsePacket[i]);
+	}
+
+	return 0;
+}
+
+int throttlep(unsigned char *packet, int dataLen,  modular_structs_t *structs)
+{
+	float value;
+	char buf[255] = {};
+	int i;
+	
+	memcpy(&value, ((float *)packet), dataLen);
+	structs->parameter_struct.alt_pid.Kp = value;
+	
+	printf("function for throttlep: %f\n", structs->parameter_struct.alt_pid.Kp);
+	
+	// Send a reply to the ground station
+	snprintf(buf, sizeof(buf), "Successfully set alt Kp to %.2f\r\n", structs->parameter_struct.alt_pid.Kp);
+	unsigned char *responsePacket;
+
+	metadata_t metadata =
+	{
+		BEGIN_CHAR,
+		MessageTypes[5].ID,
+		MessageTypes[5].subtypes[1].ID,
+		0,
+		(strlen(buf) + 1)
+	};
+	formatPacket(&metadata, buf, &responsePacket);
+
+	// Send each byte of the packet individually
+	for(i = 0; i < 8 + metadata.data_len; i++) {
+		// Debug print statement for all of the bytes being sent
+		//printf("%d: 0x%x\n", i, responsePacket[i]);
+
+		uart0_sendByte(responsePacket[i]);
+	}
+
+	return 0;
+}
+
+int throttlei(unsigned char *packet, int dataLen,  modular_structs_t *structs)
+{
+	float value;
+	char buf[255] = {};
+	int i;
+	
+	memcpy(&value, ((float *)packet), dataLen);
+	structs->parameter_struct.alt_pid.Ki = value;
+	
+	printf("function for throttlei: %f\n", structs->parameter_struct.alt_pid.Ki);
+	
+	// Send a reply to the ground station
+	snprintf(buf, sizeof(buf), "Successfully set alt Ki to %.2f\r\n", structs->parameter_struct.alt_pid.Ki);
+	unsigned char *responsePacket;
+
+	metadata_t metadata =
+	{
+		BEGIN_CHAR,
+		MessageTypes[5].ID,
+		MessageTypes[5].subtypes[1].ID,
+		0,
+		(strlen(buf) + 1)
+	};
+	formatPacket(&metadata, buf, &responsePacket);
+
+	// Send each byte of the packet individually
+	for(i = 0; i < 8 + metadata.data_len; i++) {
+		// Debug print statement for all of the bytes being sent
+		//printf("%d: 0x%x\n", i, responsePacket[i]);
+
+		uart0_sendByte(responsePacket[i]);
+	}
+
+	return 0;
+}
+
+int throttled(unsigned char *packet, int dataLen,  modular_structs_t *structs)
+{
+	float value;
+	char buf[255] = {};
+	int i;
+	
+	memcpy(&value, ((float *)packet), dataLen);
+	structs->parameter_struct.alt_pid.Kd = value;
+	
+	printf("function for throttled: %f\n", structs->parameter_struct.alt_pid.Kd);
+	
+	// Send a reply to the ground station
+	snprintf(buf, sizeof(buf), "Successfully set alt Kd to %.2f\r\n", structs->parameter_struct.alt_pid.Kd);
+	unsigned char *responsePacket;
+
+	metadata_t metadata =
+	{
+		BEGIN_CHAR,
+		MessageTypes[5].ID,
+		MessageTypes[5].subtypes[1].ID,
+		0,
+		(strlen(buf) + 1)
+	};
+	formatPacket(&metadata, buf, &responsePacket);
+
+	// Send each byte of the packet individually
+	for(i = 0; i < 8 + metadata.data_len; i++) {
+		// Debug print statement for all of the bytes being sent
+		//printf("%d: 0x%x\n", i, responsePacket[i]);
+
+		uart0_sendByte(responsePacket[i]);
+	}
+
+	return 0;
+}
+// These should be renamed to altitude!
+// ------------------------------------------------------------
+
+int accelreq(unsigned char *packet, int dataLen,  modular_structs_t *structs)
+{
+	printf("function for accelreq\n");
+	return 0;
+}
+
+int gyroresp(unsigned char *packet, int dataLen,  modular_structs_t *structs)
+{
+	printf("function for accelreq\n");
+	return 0;
+}
+
+int pitchangleresp(unsigned char *packet, int dataLen,	modular_structs_t *structs)
+{
+	printf("function for accelreq\n");
+	return 0;
+}
+
+int rollangleresp(unsigned char *packet, int dataLen,  modular_structs_t *structs)
+{
+	printf("function for accelreq\n");
+	return 0;
+}
+
+int gyroreq(unsigned char *packet, int dataLen,	 modular_structs_t *structs)
+{
+	printf("function for accelreq\n");
+	return 0;
+}
+
+int pitchanglereq(unsigned char *packet, int dataLen,  modular_structs_t *structs)
+{
+	printf("function for accelreq\n");
+	return 0;
+}
+
+int rollanglereq(unsigned char *packet, int dataLen,  modular_structs_t *structs)
+{
+	printf("function for accelreq\n");
+	return 0;
+}
+
+int accelresp(unsigned char *packet, int dataLen,  modular_structs_t *structs)
+{
+	printf("function for accelreq\n");
+	return 0;
+}
--- a/quad/sw/imu_logger/src/commands.h
+++ b/quad/sw/imu_logger/src/commands.h
+#ifndef _COMMANDS_H
+#define _COMMANDS_H
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include "type_def.h"
+
+// ----------------------
+// Helper stuff
+
+#define MAX_TYPE 6
+#define MAX_SUBTYPE 100
+
+enum Message{
+	BEGIN_CHAR = 0xBE,
+	END_CHAR   = 0xED
+};
+
+// This should also have double to avoid confusion with float values.
+enum DataType
+{
+	floatType,
+	intType,
+	stringType
+};
+
+// MESSAGE SUBTYPES
+struct MessageSubtype{
+	char ID;
+	char cmdText[100];
+	char cmdDataType;
+	int (*functionPtr)(unsigned char *command, int dataLen, modular_structs_t *structs);
+};
+
+// MESSAGE TYPES
+struct MessageType{
+	char ID;
+	struct MessageSubtype subtypes[MAX_SUBTYPE];
+};
+
+int debug(unsigned char *c, int dataLen, modular_structs_t *structs);
+int update(unsigned char *c, int dataLen, modular_structs_t *structs);
+int beginupdate(unsigned char *c, int dataLen, modular_structs_t *structs);
+int logdata(unsigned char *c, int dataLen, modular_structs_t *structs);
+int response(unsigned char *packet, int dataLen, modular_structs_t *structs);
+int yawset(unsigned char *c, int dataLen, modular_structs_t *structs);
+int yawp(unsigned char *c, int dataLen, modular_structs_t *structs);
+int yawd(unsigned char *c, int dataLen, modular_structs_t *structs);
+int rollset(unsigned char *c, int dataLen, modular_structs_t *structs);
+int rollp(unsigned char *c, int dataLen, modular_structs_t *structs);
+int rolld(unsigned char *c, int dataLen, modular_structs_t *structs);
+int pitchset(unsigned char *c, int dataLen, modular_structs_t *structs);
+int pitchp(unsigned char *c, int dataLen, modular_structs_t *structs);
+int pitchd(unsigned char *c, int dataLen, modular_structs_t *structs);
+int throttleset(unsigned char *c, int dataLen, modular_structs_t *structs);
+int throttlep(unsigned char *c, int dataLen, modular_structs_t *structs);
+int throttlei(unsigned char *c, int dataLen, modular_structs_t *structs);
+int throttled(unsigned char *c, int dataLen, modular_structs_t *structs);
+int accelreq(unsigned char *c, int dataLen, modular_structs_t *structs);
+int gyroresp(unsigned char *c, int dataLen, modular_structs_t *structs);
+int pitchangleresp(unsigned char *c, int dataLen, modular_structs_t *structs);
+int rollangleresp(unsigned char *c, int dataLen, modular_structs_t *structs);
+int gyroreq(unsigned char *c, int dataLen, modular_structs_t *structs);
+int pitchanglereq(unsigned char *c, int dataLen, modular_structs_t *structs);
+int rollanglereq(unsigned char *c, int dataLen, modular_structs_t *structs);
+int accelresp(unsigned char *c, int dataLen, modular_structs_t *structs);
+
+// TODO add in string to be read from the command line when sending a subtype of message
+extern struct MessageType MessageTypes[MAX_TYPE];
+
+#endif
--- a/quad/sw/imu_logger/src/communication.c
+++ b/quad/sw/imu_logger/src/communication.c
+#include "communication.h"
+
+// QUAD & Ground Station
+// Format the log data from log_message
+//int formatData(unsigned char *log_msg, unsigned char *formattedCommand)
+int formatPacket(metadata_t *metadata, void *data, unsigned char **formattedCommand)
+{
+	*formattedCommand = malloc(sizeof(char) * metadata->data_len + 8);
+	/*if (formattedCommand == NULL)
+	{
+		return -1;
+	}*/
+	
+	//----------------------------------------------------------------------------------------------
+	//	   index||	   0	|	  1	   |	  2		 |	3 & 4 |		 5 & 6		 |	7+	|	end	   |
+	//---------------------------------------------------------------------------------------------|
+	// msg param|| beg char | msg type | msg subtype | msg id | data len (bytes) | data | checksum |
+	//-------------------------------------------------------------------------------------------- |
+	//	   bytes||	   1	|	  1	   |	  1		 |	  2	  |		   2		 | var	|	 1	   |
+	//----------------------------------------------------------------------------------------------
+					
+	// Begin Char:
+	(*formattedCommand)[0] = metadata->begin_char;
+
+	// Msg type:
+	(*formattedCommand)[1] = metadata->msg_type;
+	
+	// Msg subtype
+	(*formattedCommand)[2] = metadata->msg_subtype;
+	
+	//Msg id (msgNum is 2 bytes)
+	(*formattedCommand)[3] = metadata->msg_id;
+	
+	// Data length and data - bytes 5&6 for len, 7+ for data
+	(*formattedCommand)[5] = metadata->data_len & 0x000000ff;
+	(*formattedCommand)[6] = (metadata->data_len >> 8) & 0x000000ff;
+	
+//	printf("data length %d\n", metadata->data_len);
+//	printf("data length %x\n", (*formattedCommand)[5]);
+//	printf("data length %x\n", (*formattedCommand)[6]);
+
+	memcpy(&((*formattedCommand)[7]), data, metadata->data_len);
+	
+	// Checksum
+	// receive data and calculate checksum
+	int i;
+	unsigned char packet_checksum = 0;
+	for(i = 0; i < 7 + metadata->data_len; i++)
+	{
+		packet_checksum ^= (*formattedCommand)[i];
+	}
+	
+//	printf("Packet checksum: 0x%02x\n", packet_checksum);
+
+	(*formattedCommand)[7 + metadata->data_len] = packet_checksum;
+	
+	return 0;
+}
+
+// returns the length of the data in bytes (datalen from packet) and fills data
+// and metadata with the packet information
+// use as follows:
+//
+//		packet is the entire packet message (formatted) 
+//		data is an unallocated (char *) (pass it to this function as &data) 
+//		meta_data is a pointer to an instance of metadata_t
+//
+int parse_packet(unsigned char * packet, unsigned char ** data, metadata_t * meta_data)
+{
+	//----------------------------------------------------------------------------------------------
+	//     index||     0    |     1    |      2      |  3 & 4 |      5 & 6       |  7+  |   end    |
+	//---------------------------------------------------------------------------------------------|
+	// msg param|| beg char | msg type | msg subtype | msg id | data len (bytes) | data | checksum |
+	//-------------------------------------------------------------------------------------------- |
+	//     bytes||     1    |     1    |      1      |    2   |        2         | var  |    1     |
+	//----------------------------------------------------------------------------------------------	
+	
+	// first byte must be the begin char
+	if(packet[0] != 0xBE) {
+		printf("The first packet byte is not the begin char.\n");
+		return -1;
+	}
+
+	// receive metadata
+	meta_data->begin_char = packet[0];
+	meta_data->msg_type = packet[1];
+	meta_data->msg_subtype = packet[2];
+	meta_data->msg_id = (packet[4] << 8) | (packet[3]);
+	meta_data->data_len = (packet[6] << 8) | (packet[5]);
+	unsigned char packet_checksum = packet[7+meta_data->data_len];
+
+//	printf("msg_type: %x\n", meta_data->msg_type);
+//	printf("msg_subtype: %x\n", meta_data->msg_subtype);
+//	printf("msg_type: %d\n", meta_data->data_len);
+	
+	int i;
+	
+	// receive data
+	*data = malloc(meta_data->data_len);
+	for(i = 0; i < meta_data->data_len; i++)
+	{
+		(*data)[i] = packet[7+i];
+	}
+
+	// calculate checksum
+	unsigned char calculated_checksum = 0;
+	for(i = 0; i < meta_data->data_len + 7; i++)
+	{
+		calculated_checksum ^= packet[i];
+	}
+
+	// compare checksum
+	if(packet_checksum != calculated_checksum)
+		printf("Checksums did not match (Quadlog): 0x%02x\t0x%02x\n", packet_checksum, calculated_checksum);
+
+	//////////////////////////////
+	// Send an acknowledgment packet
+
+	// Send a reply to the ground station
+
+	int buf = meta_data->msg_id;
+	unsigned char *responsePacket;
+
+	metadata_t metadata =
+	{
+		BEGIN_CHAR,
+		MessageTypes[0].ID,
+		MessageTypes[0].subtypes[1].ID,
+		0,
+		sizeof(int)
+	};
+	formatPacket(&metadata, &buf, &responsePacket);
+
+	// Send each byte of the packet individually
+	for(i = 0; i < 8 + metadata.data_len; i++) {
+		// Debug print statement for all of the bytes being sent
+		//printf("%d: 0x%x\n", i, responsePacket[i]);
+
+		uart0_sendByte(responsePacket[i]);
+	}
+	free(responsePacket);
+
+	return 0;
+}
+
+// QUAD & Ground Station
+// Process the command received
+int processCommand(unsigned char *packet, modular_structs_t *structs) {
+	int validPacket;
+	unsigned char *data;
+	metadata_t metadata;
+	
+	printf("Process Command.\n");
+
+	// Validate the message is correctly formatted
+	validPacket = parse_packet(packet, &data, &metadata);
+	if(validPacket != 0) {
+		printf("Packet is not valid.\n");
+		return -1;
+	}
+	
+	if(metadata.data_len >= 0) {
+		// Call the appropriate subtype function
+		(* (MessageTypes[metadata.msg_type].subtypes[metadata.msg_subtype].functionPtr))(data, metadata.data_len, structs);
+		
+//		printf("%s\n", MessageTypes[metadata.msg_type].subtypes[metadata.msg_subtype].cmdText);
+
+		return 0;
+	} else {
+		printf("Data length is less than 0.\n");
+	}
+	
+	// Only gets here if there is an error
+	return -1;
+}
--- a/quad/sw/imu_logger/src/communication.h
+++ b/quad/sw/imu_logger/src/communication.h
+#ifndef _COMMUNICATION_H
+#define _COMMUNICATION_H
+
+#include <stdio.h>
+#include <string.h>
+#include <stdlib.h>
+#include <limits.h>
+#include "commands.h"
+#include "type_def.h"
+#include "uart.h"
+
+int formatCommand(unsigned char *command, unsigned char **formattedCommand);
+int formatPacket(metadata_t *metadata, void *data, unsigned char **formattedCommand);
+int logData(unsigned char *log_msg, unsigned char *formattedCommand);
+int processCommand(unsigned char *command, modular_structs_t *structs);
+int parse_packet(unsigned char * packet, unsigned char ** data, metadata_t * meta_data);
+
+#endif
--- a/quad/sw/imu_logger/src/control_algorithm.c
+++ b/quad/sw/imu_logger/src/control_algorithm.c
+/*
+ * control_algorithm.c
+ *
+ *  Created on: Feb 20, 2016
+ *      Author: ucart
+ */
+
+// This implemented modular quadrotor software implements a PID control algorithm
+ 
+#include "control_algorithm.h"
+#include "communication.h"
+
+#define ROLL_PITCH_MAX_ANGLE 0.3490 // 20 degrees
+
+ int control_algorithm_init(parameter_t * parameter_struct)
+ {
+	// HUMAN Piloted (RC) PID DEFINITIONS //////
+	// RC PIDs for roll (2 loops: angle --> angular velocity)
+	parameter_struct->roll_angle_pid.dt = 0.005; parameter_struct->roll_ang_vel_pid.dt = 0.005; // 5 ms calculation period
+
+	// RC PIDs for pitch (2 loops: angle --> angular velocity)
+	parameter_struct->pitch_angle_pid.dt = 0.005; parameter_struct->pitch_ang_vel_pid.dt = 0.005; // 5 ms calculation period
+
+	// initialize Yaw PID_t and PID constants
+	// RC PID for yaw (1 loop angular velocity)
+	parameter_struct->yaw_ang_vel_pid.dt = 0.005; // 5 ms calculation period
+
+	// AUTOMATIC Pilot (Position) PID DEFINITIONS //////
+	// Local X PID using a translation from X camera system data to quad local X position (3 loops: local y position --> angle --> angular velocity)
+	parameter_struct->local_x_pid.dt = 0.100;
+
+	// Local Y PID using a translation from Y camera system data to quad local Y position(3 loops: local x position --> angle --> angular velocity)
+	parameter_struct->local_y_pid.dt = 0.100;
+
+	// CAM PIDs for yaw (2 loops angle --> angular velocity)
+	parameter_struct->yaw_angle_pid.dt = 0.100;
+
+	// CAM PID for altitude (1 loop altitude)
+	parameter_struct->alt_pid.dt = 0.100;
+
+	// PID coeffiecients (Position)
+	setPIDCoeff(&(parameter_struct->local_y_pid), YPOS_KP, YPOS_KI, YPOS_KD);
+	setPIDCoeff(&(parameter_struct->local_x_pid), XPOS_KP, XPOS_KI, XPOS_KD);
+	setPIDCoeff(&(parameter_struct->alt_pid), ALT_ZPOS_KP, ALT_ZPOS_KI, ALT_ZPOS_KD);
+
+	// PID coefficients (Angle)
+	setPIDCoeff(&(parameter_struct->pitch_angle_pid), PITCH_ANGLE_KP, PITCH_ANGLE_KI, PITCH_ANGLE_KD);
+	setPIDCoeff(&(parameter_struct->roll_angle_pid), ROLL_ANGLE_KP, ROLL_ANGLE_KI, ROLL_ANGLE_KD);
+	setPIDCoeff(&(parameter_struct->yaw_angle_pid), YAW_ANGLE_KP, YAW_ANGLE_KI, YAW_ANGLE_KD);
+
+	// PID coefficients (Angular Velocity)
+	setPIDCoeff(&(parameter_struct->pitch_ang_vel_pid), PITCH_ANGULAR_VELOCITY_KP, PITCH_ANGULAR_VELOCITY_KI, PITCH_ANGULAR_VELOCITY_KD);
+	setPIDCoeff(&(parameter_struct->roll_ang_vel_pid), ROLL_ANGULAR_VELOCITY_KP, ROLL_ANGULAR_VELOCITY_KI, ROLL_ANGULAR_VELOCITY_KD);
+	setPIDCoeff(&(parameter_struct->yaw_ang_vel_pid), YAW_ANGULAR_VELOCITY_KP, YAW_ANGULAR_VELOCITY_KI, YAW_ANGULAR_VELOCITY_KD);
+
+	return 0;
+ }
+ 
+ int control_algorithm(log_t* log_struct, user_input_t * user_input_struct, sensor_t* sensor_struct, setpoint_t* setpoint_struct, parameter_t* parameter_struct, user_defined_t* user_defined_struct, raw_actuator_t* raw_actuator_struct, modular_structs_t* structs)
+ {
+	// use the 'flap' switch as the flight mode selector
+	int cur_fm_switch = read_flap(user_input_struct->rc_commands[FLAP]);
+	static int last_fm_switch = MANUAL_FLIGHT_MODE;
+
+	// reset flight_mode to MANUAL right away if the flap switch is in manual position
+	// to engage AUTO mode the code waits for a new packet after the flap is switched to auto
+	// before actually engaging AUTO mode
+	if(cur_fm_switch == MANUAL_FLIGHT_MODE)
+		user_defined_struct->flight_mode = MANUAL_FLIGHT_MODE;
+
+	static float roll_trim = 0.0;
+	static float pitch_trim = 0.0;
+
+	// flap switch was just toggled to auto flight mode
+	if((last_fm_switch != cur_fm_switch) && (cur_fm_switch == AUTO_FLIGHT_MODE))
+	{
+		user_defined_struct->engaging_auto = 1;
+
+		// Read in trimmed values because it should read trim values right when the pilot flips the flight mode switch
+		pitch_trim = user_input_struct->pitch_angle_manual_setpoint; //rc_commands[PITCH] - PITCH_CENTER;
+		roll_trim = user_input_struct->roll_angle_manual_setpoint; //rc_commands[ROLL] - ROLL_CENTER;
+		//sensor_struct->trimmedRCValues.yaw = yaw_manual_setpoint; //rc_commands[YAW] - YAW_CENTER;
+
+//		sensor_struct->trims.roll = raw_actuator_struct->controller_corrected_motor_commands[ROLL];
+//		sensor_struct->trims.pitch = raw_actuator_struct->controller_corrected_motor_commands[PITCH];
+//		sensor_struct->trims.yaw = raw_actuator_struct->controller_corrected_motor_commands[YAW];
+		sensor_struct->trims.throttle = user_input_struct->rc_commands[THROTTLE];
+
+		log_struct->trims.roll = sensor_struct->trims.roll;
+		log_struct->trims.pitch = sensor_struct->trims.pitch;
+		log_struct->trims.yaw = sensor_struct->trims.yaw;
+		log_struct->trims.throttle = sensor_struct->trims.throttle;
+	}
+
+	if(user_input_struct->hasPacket == 0x04 && user_defined_struct->engaging_auto == 1)
+			user_defined_struct->engaging_auto = 2;
+
+	// If the quad has received a packet and it's not an update packet
+	if(user_input_struct->hasPacket != -1 && user_input_struct->hasPacket != 0x04)
+	{
+		processCommand((unsigned char *)user_input_struct->sb->buf, structs);
+	}
+
+
+	// if the flap switch was toggled to AUTO_FLIGHT_MODE and we've received a new packet
+	// then record the current position as the desired position
+	// also reset the previous error and accumulated error from the position PIDs
+	if((cur_fm_switch == AUTO_FLIGHT_MODE) && (user_defined_struct->engaging_auto == 2))
+	{
+		// zero out the accumulated error so the I terms don't cause wild things to happen
+		parameter_struct->alt_pid.acc_error = 0.0;
+		parameter_struct->local_x_pid.acc_error = 0.0;
+		parameter_struct->local_y_pid.acc_error = 0.0;
+
+		// make previous error equal to the current so the D term doesn't spike
+		parameter_struct->alt_pid.prev_error = 0.0;
+		parameter_struct->local_x_pid.prev_error = 0.0;
+		parameter_struct->local_y_pid.prev_error = 0.0;
+
+		setpoint_struct->desiredQuadPosition.alt_pos = sensor_struct->currentQuadPosition.alt_pos;
+		setpoint_struct->desiredQuadPosition.x_pos = sensor_struct->currentQuadPosition.x_pos;
+		setpoint_struct->desiredQuadPosition.y_pos = sensor_struct->currentQuadPosition.y_pos;
+		setpoint_struct->desiredQuadPosition.yaw = 0.0;//currentQuadPosition.yaw;
+
+		// reset the flag that engages auto mode
+		user_defined_struct->engaging_auto = 0;
+
+		// finally engage the AUTO_FLIGHT_MODE
+		// this ensures that we've gotten a new update packet right after the switch was set to auto mode
+		user_defined_struct->flight_mode = AUTO_FLIGHT_MODE;
+	}
+
+	//PIDS///////////////////////////////////////////////////////////////////////
+
+	/* 					Position loop
+	 * Reads current position, and outputs
+	 * a pitch or roll for the angle loop PIDs
+	 */
+
+//		static int counter_between_packets = 0;
+
+	if(user_input_struct->hasPacket == 0x04)
+	{
+		parameter_struct->local_y_pid.current_point = sensor_struct->currentQuadPosition.y_pos;
+		parameter_struct->local_y_pid.setpoint = setpoint_struct->desiredQuadPosition.y_pos;
+
+		parameter_struct->local_x_pid.current_point = sensor_struct->currentQuadPosition.x_pos;
+		parameter_struct->local_x_pid.setpoint = setpoint_struct->desiredQuadPosition.x_pos;
+
+		parameter_struct->alt_pid.current_point = sensor_struct->currentQuadPosition.alt_pos;
+		parameter_struct->alt_pid.setpoint = setpoint_struct->desiredQuadPosition.alt_pos;
+
+		//logging and PID computation
+		log_struct->local_y_PID_values = pid_computation(&(parameter_struct->local_y_pid));
+		log_struct->local_x_PID_values = pid_computation(&(parameter_struct->local_x_pid));
+		log_struct->altitude_PID_values = pid_computation(&(parameter_struct->alt_pid));
+
+		// yaw angular position PID calculation
+		parameter_struct->yaw_angle_pid.current_point = sensor_struct->currentQuadPosition.yaw;// in radians
+		parameter_struct->yaw_angle_pid.setpoint = setpoint_struct->desiredQuadPosition.yaw; // constant setpoint
+
+		//logging and PID computation
+		log_struct->angle_yaw_PID_values = pid_computation(&(parameter_struct->yaw_angle_pid));
+
+	}
+
+
+	/* 					Angle loop
+	 * Calculates current orientation, and outputs
+	 * a pitch, roll, or yaw velocity for the angular velocity loop PIDs
+	 */
+
+	//angle boundaries
+	if(parameter_struct->local_x_pid.pid_correction > ROLL_PITCH_MAX_ANGLE)
+	{
+		parameter_struct->local_x_pid.pid_correction = ROLL_PITCH_MAX_ANGLE;
+	}
+	if(parameter_struct->local_x_pid.pid_correction < -ROLL_PITCH_MAX_ANGLE)
+	{
+		parameter_struct->local_x_pid.pid_correction = -ROLL_PITCH_MAX_ANGLE;
+	}
+	if(parameter_struct->local_y_pid.pid_correction > ROLL_PITCH_MAX_ANGLE)
+	{
+		parameter_struct->local_y_pid.pid_correction = ROLL_PITCH_MAX_ANGLE;
+	}
+	if(parameter_struct->local_y_pid.pid_correction < -ROLL_PITCH_MAX_ANGLE)
+	{
+		parameter_struct->local_y_pid.pid_correction = -ROLL_PITCH_MAX_ANGLE;
+	}
+
+	parameter_struct->pitch_angle_pid.current_point = sensor_struct->pitch_angle_filtered;
+	parameter_struct->pitch_angle_pid.setpoint =
+			(user_defined_struct->flight_mode == AUTO_FLIGHT_MODE)?
+			(parameter_struct->local_x_pid.pid_correction) + pitch_trim : user_input_struct->pitch_angle_manual_setpoint;
+
+	parameter_struct->roll_angle_pid.current_point = sensor_struct->roll_angle_filtered;
+	parameter_struct->roll_angle_pid.setpoint =
+			(user_defined_struct->flight_mode == AUTO_FLIGHT_MODE)?
+			(parameter_struct->local_y_pid.pid_correction) + roll_trim : user_input_struct->roll_angle_manual_setpoint;
+
+
+	//logging and PID computation
+	log_struct->angle_pitch_PID_values = pid_computation(&(parameter_struct->pitch_angle_pid));
+	log_struct->angle_roll_PID_values = pid_computation(&(parameter_struct->roll_angle_pid));
+
+
+	/* 				Angular Velocity Loop
+	 * Takes the desired angular velocity from the angle loop,
+	 * and calculates a PID correction with the current angular velocity
+	 */
+
+	// theta_dot is the angular velocity about the y-axis
+	// it is calculated from using the gimbal equations
+	parameter_struct->pitch_ang_vel_pid.current_point = sensor_struct->theta_dot;
+	parameter_struct->pitch_ang_vel_pid.setpoint = parameter_struct->pitch_angle_pid.pid_correction;
+
+	// phi_dot is the angular velocity about the x-axis
+	// it is calculated from using the gimbal equations
+	parameter_struct->roll_ang_vel_pid.current_point = sensor_struct->phi_dot;
+	parameter_struct->roll_ang_vel_pid.setpoint = parameter_struct->roll_angle_pid.pid_correction;
+
+	// Yaw angular velocity PID
+	// psi_dot is the angular velocity about the z-axis
+	// it is calculated from using the gimbal equations
+	parameter_struct->yaw_ang_vel_pid.current_point = sensor_struct->psi_dot;
+	parameter_struct->yaw_ang_vel_pid.setpoint = (user_defined_struct->flight_mode == AUTO_FLIGHT_MODE)?
+			parameter_struct->yaw_angle_pid.pid_correction : user_input_struct->yaw_manual_setpoint; // no trim added because the controller already works well
+
+	//logging and PID computation
+	log_struct->ang_vel_pitch_PID_values = pid_computation(&(parameter_struct->pitch_ang_vel_pid));
+	log_struct->ang_vel_roll_PID_values = pid_computation(&(parameter_struct->roll_ang_vel_pid));
+	log_struct->ang_vel_yaw_PID_values = pid_computation(&(parameter_struct->yaw_ang_vel_pid));
+
+	//END PIDs///////////////////////////////////////////////////////////////////////
+
+
+	 // here for now so in case any flight command is not PID controlled, it will default to rc_command value:
+	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
+	if((user_input_struct->rc_commands[THROTTLE] >
+	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->alt_pid.pid_correction)) + sensor_struct->trims.throttle;
+
+			//ROLL
+			raw_actuator_struct->controller_corrected_motor_commands[ROLL] =
+					parameter_struct->roll_ang_vel_pid.pid_correction; // + sensor_struct->trims.roll;
+
+			//PITCH
+			raw_actuator_struct->controller_corrected_motor_commands[PITCH] =
+					parameter_struct->pitch_ang_vel_pid.pid_correction; // + sensor_struct->trims.pitch;
+
+			//YAW
+			raw_actuator_struct->controller_corrected_motor_commands[YAW] =
+					parameter_struct->yaw_ang_vel_pid.pid_correction;// + sensor_struct->trims.yaw;
+
+//			static int slow_down = 0;
+//			slow_down++;
+//			if(slow_down % 50 == 0)
+//			printf("X: %.3f\tY: %.3f\tZ: %.3f\tX_s: %.3f\tX_c: %.3f\tY_s: %.3f\tY_c: %.3f\tZ_s: %.3f\tZ_c: %.3f\t\n",
+//					parameter_struct->local_x_pid.pid_correction,
+//					parameter_struct->local_y_pid.pid_correction,
+//					parameter_struct->alt_pid.pid_correction,
+//					parameter_struct->local_x_pid.setpoint, parameter_struct->local_x_pid.current_point,
+//					parameter_struct->local_y_pid.setpoint, parameter_struct->local_y_pid.current_point,
+//					parameter_struct->alt_pid.setpoint, parameter_struct->alt_pid.current_point);
+		}
+		else{
+			//ROLL
+			raw_actuator_struct->controller_corrected_motor_commands[ROLL] =
+					parameter_struct->roll_ang_vel_pid.pid_correction;
+
+			//PITCH
+			raw_actuator_struct->controller_corrected_motor_commands[PITCH] =
+					parameter_struct->pitch_ang_vel_pid.pid_correction;
+
+			//YAW
+			raw_actuator_struct->controller_corrected_motor_commands[YAW] =
+					parameter_struct->yaw_ang_vel_pid.pid_correction;
+		}
+
+		//BOUNDS CHECKING
+		if(raw_actuator_struct->controller_corrected_motor_commands[THROTTLE] < 0)
+			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] < -20000)
+			raw_actuator_struct->controller_corrected_motor_commands[ROLL] = -20000;
+
+		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] < -20000)
+			raw_actuator_struct->controller_corrected_motor_commands[PITCH] = -20000;
+
+		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] < -20000)
+			raw_actuator_struct->controller_corrected_motor_commands[YAW] = -20000;
+
+	}
+	else
+	{
+		raw_actuator_struct->controller_corrected_motor_commands[ROLL] = 0;
+		raw_actuator_struct->controller_corrected_motor_commands[PITCH] = 0;
+		raw_actuator_struct->controller_corrected_motor_commands[YAW] = 0;
+	}
+
+	//logging
+	// here we are not actually duplicating the logging from the PID computation
+	// the PID computation logs PID_values struct where this logs the PID struct
+	// they contain different sets of data
+	log_struct->local_y_PID = parameter_struct->local_y_pid;
+	log_struct->local_x_PID = parameter_struct->local_x_pid;
+	log_struct->altitude_PID = parameter_struct->alt_pid;
+
+	log_struct->angle_roll_PID = parameter_struct->roll_angle_pid;
+	log_struct->angle_pitch_PID = parameter_struct->pitch_angle_pid;
+	log_struct->angle_yaw_PID = parameter_struct->yaw_angle_pid;
+
+	log_struct->ang_vel_roll_PID = parameter_struct->roll_ang_vel_pid;
+	log_struct->ang_vel_pitch_PID = parameter_struct->pitch_ang_vel_pid;
+	log_struct->ang_vel_yaw_PID = parameter_struct->yaw_ang_vel_pid;
+
+	last_fm_switch = cur_fm_switch;
+
+	if(user_input_struct->hasPacket != -1)
+	{
+		user_input_struct->sb->clear(user_input_struct->sb);
+		user_input_struct->hasPacket = -1;
+	}
+
+    return 0;
+ }
+ 
+ void setPIDCoeff(PID_t* p, float pValue, float iValue, float dValue) {
+
+ 	p->Kp = pValue;
+ 	p->Ki = iValue;
+ 	p->Kd = dValue;
+
+ }
+
--- a/quad/sw/imu_logger/src/control_algorithm.h
+++ b/quad/sw/imu_logger/src/control_algorithm.h
+/*
+ * control_algorithm.h
+ *
+ *  Created on: Feb 20, 2016
+ *      Author: ucart
+ */
+
+#ifndef CONTROL_ALGORITHM_H_
+#define CONTROL_ALGORITHM_H_
+ 
+#include <stdio.h>
+
+#include "log_data.h"
+#include "sensor_processing.h"
+#include "quadposition.h"
+#include "type_def.h"
+
+/**
+ * @brief 
+ *      Initializes everything used in the control algorithm.
+ *
+ * @return
+ *      error message
+ *
+ */
+int control_algorithm_init(parameter_t * parameter_struct);
+
+/**
+ * @brief 
+ *      Runs the control algorithm on the data and outputs a command for actuators.
+ *
+ * @param log_struct
+ *      structure of the data to be logged
+ *
+ * @param sensor_struct
+ *      structure of the processed data from the sensors
+ *
+ * @param setpoint_struct
+ *      structure of the setpoints used in the controller
+ *
+ * @param parameter_struct
+ *      structure of the parameters used in the controller
+ *
+ * @param user_defined_struct
+ *      structure of the user defined variables
+ *
+ * @param raw_actuator_struct
+ *      structure of the commmands outputted to go to the actuators
+ *
+ * @return 
+ *      error message
+ *
+ */
+int control_algorithm(log_t* log_struct,
+					  user_input_t * user_input_struct,
+					  sensor_t* sensor_struct,
+                      setpoint_t* setpoint_struct, 
+                      parameter_t* parameter_struct, 
+                      user_defined_t* user_defined_struct, 
+                      raw_actuator_t* raw_actuator_struct,
+                      modular_structs_t* structs);
+
+/**
+ * @brief
+ *      Internally used functions
+ *
+ */
+void setPIDCoeff(PID_t* p, float pValue, float iValue, float dValue);
+
+#endif /* CONTROL_ALGORITHM_H_ */
No results found