diff --git a/quad/xsdk_workspace/imu_logger/.cproject b/quad/xsdk_workspace/imu_logger/.cproject
deleted file mode 100644
index bd53d849cef4e32e22404350a9f4185ed467f2b2..0000000000000000000000000000000000000000
--- a/quad/xsdk_workspace/imu_logger/.cproject
+++ /dev/null
@@ -1,173 +0,0 @@
-<?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>
diff --git a/quad/xsdk_workspace/imu_logger/.gitignore b/quad/xsdk_workspace/imu_logger/.gitignore
deleted file mode 100644
index 06a1484bb54b84d2fb2054a167cd183be528bfa7..0000000000000000000000000000000000000000
--- a/quad/xsdk_workspace/imu_logger/.gitignore
+++ /dev/null
@@ -1,2 +0,0 @@
-Debug/
-bootimage/
diff --git a/quad/xsdk_workspace/imu_logger/.project b/quad/xsdk_workspace/imu_logger/.project
deleted file mode 100644
index 7fb0307b7a23ac30a9f2df395566aafeb3e10fd8..0000000000000000000000000000000000000000
--- a/quad/xsdk_workspace/imu_logger/.project
+++ /dev/null
@@ -1,27 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<projectDescription>
- <name>imu_logger</name>
- <comment></comment>
- <projects>
- <project>system_bsp_new</project>
- <project>system_bsp</project>
- </projects>
- <buildSpec>
- <buildCommand>
- <name>org.eclipse.cdt.managedbuilder.core.genmakebuilder</name>
- <arguments>
- </arguments>
- </buildCommand>
- <buildCommand>
- <name>org.eclipse.cdt.managedbuilder.core.ScannerConfigBuilder</name>
- <triggers>full,incremental,</triggers>
- <arguments>
- </arguments>
- </buildCommand>
- </buildSpec>
- <natures>
- <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>
diff --git a/quad/xsdk_workspace/imu_logger/src/Copy of original lscript.ld b/quad/xsdk_workspace/imu_logger/src/Copy of original lscript.ld
deleted file mode 100644
index 970979a58cb47d318dbc9197e5ceb885993cc867..0000000000000000000000000000000000000000
--- a/quad/xsdk_workspace/imu_logger/src/Copy of original lscript.ld
+++ /dev/null
@@ -1,284 +0,0 @@
-/*******************************************************************/
-/* */
-/* 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 = .;
-}
-
diff --git a/quad/xsdk_workspace/imu_logger/src/PID.c b/quad/xsdk_workspace/imu_logger/src/PID.c
deleted file mode 100644
index 185e7b8dc6161616321bd8fae82f5dd709f94174..0000000000000000000000000000000000000000
--- a/quad/xsdk_workspace/imu_logger/src/PID.c
+++ /dev/null
@@ -1,63 +0,0 @@
-/*
- * 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;
-}
diff --git a/quad/xsdk_workspace/imu_logger/src/PID.h b/quad/xsdk_workspace/imu_logger/src/PID.h
deleted file mode 100644
index d0e69ddcc9450d9888b8e7a424e45aabb46b2039..0000000000000000000000000000000000000000
--- a/quad/xsdk_workspace/imu_logger/src/PID.h
+++ /dev/null
@@ -1,87 +0,0 @@
-/*
- * 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_ */
diff --git a/quad/xsdk_workspace/imu_logger/src/README.txt b/quad/xsdk_workspace/imu_logger/src/README.txt
deleted file mode 100644
index 0e065fbfb65ff25e0844aa10225af4b34d0b07ab..0000000000000000000000000000000000000000
--- a/quad/xsdk_workspace/imu_logger/src/README.txt
+++ /dev/null
@@ -1 +0,0 @@
-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.
diff --git a/quad/xsdk_workspace/imu_logger/src/actuator_command_processing.c b/quad/xsdk_workspace/imu_logger/src/actuator_command_processing.c
deleted file mode 100644
index 64fce135a7eb95bb5a80962e72d64572dbccf574..0000000000000000000000000000000000000000
--- a/quad/xsdk_workspace/imu_logger/src/actuator_command_processing.c
+++ /dev/null
@@ -1,142 +0,0 @@
-/*
- * 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
-
-}
diff --git a/quad/xsdk_workspace/imu_logger/src/actuator_command_processing.h b/quad/xsdk_workspace/imu_logger/src/actuator_command_processing.h
deleted file mode 100644
index 12110d5513130b90b4b60f30ffe555bcb370a07e..0000000000000000000000000000000000000000
--- a/quad/xsdk_workspace/imu_logger/src/actuator_command_processing.h
+++ /dev/null
@@ -1,38 +0,0 @@
-/*
- * 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_ */
diff --git a/quad/xsdk_workspace/imu_logger/src/commands.c b/quad/xsdk_workspace/imu_logger/src/commands.c
deleted file mode 100644
index 97ed7720ab204d00bc88509f31204497ed5e7aed..0000000000000000000000000000000000000000
--- a/quad/xsdk_workspace/imu_logger/src/commands.c
+++ /dev/null
@@ -1,933 +0,0 @@
-#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;
-}
diff --git a/quad/xsdk_workspace/imu_logger/src/commands.h b/quad/xsdk_workspace/imu_logger/src/commands.h
deleted file mode 100644
index 7861a84bd51e0c9ac2674ab9d2cd35953eb78dc2..0000000000000000000000000000000000000000
--- a/quad/xsdk_workspace/imu_logger/src/commands.h
+++ /dev/null
@@ -1,72 +0,0 @@
-#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
diff --git a/quad/xsdk_workspace/imu_logger/src/communication.c b/quad/xsdk_workspace/imu_logger/src/communication.c
deleted file mode 100644
index b0e9e36ea049686bc37c7b8318fa640b1c1ef88f..0000000000000000000000000000000000000000
--- a/quad/xsdk_workspace/imu_logger/src/communication.c
+++ /dev/null
@@ -1,175 +0,0 @@
-#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;
-}
diff --git a/quad/xsdk_workspace/imu_logger/src/communication.h b/quad/xsdk_workspace/imu_logger/src/communication.h
deleted file mode 100644
index 8778c4edfb3f990b94ab63877dd4a1e10886cd01..0000000000000000000000000000000000000000
--- a/quad/xsdk_workspace/imu_logger/src/communication.h
+++ /dev/null
@@ -1,18 +0,0 @@
-#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
diff --git a/quad/xsdk_workspace/imu_logger/src/control_algorithm.c b/quad/xsdk_workspace/imu_logger/src/control_algorithm.c
deleted file mode 100644
index f1dd773cf11f1dc5eb89c135a56a37aaeaf71d4e..0000000000000000000000000000000000000000
--- a/quad/xsdk_workspace/imu_logger/src/control_algorithm.c
+++ /dev/null
@@ -1,353 +0,0 @@
-/*
- * 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;
-
- }
-
diff --git a/quad/xsdk_workspace/imu_logger/src/control_algorithm.h b/quad/xsdk_workspace/imu_logger/src/control_algorithm.h
deleted file mode 100644
index 463e0f512ef64f3b3d5841d3ed6396db17830579..0000000000000000000000000000000000000000
--- a/quad/xsdk_workspace/imu_logger/src/control_algorithm.h
+++ /dev/null
@@ -1,70 +0,0 @@
-/*
- * 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_ */
diff --git a/quad/xsdk_workspace/imu_logger/src/controllers.c b/quad/xsdk_workspace/imu_logger/src/controllers.c
deleted file mode 100644
index 31f70c3673c15e9e4ca56a03fb5c7ce6804a7b99..0000000000000000000000000000000000000000
--- a/quad/xsdk_workspace/imu_logger/src/controllers.c
+++ /dev/null
@@ -1,51 +0,0 @@
-/*
- * controllers.c
- *
- * Created on: Oct 11, 2014
- * Author: ucart
- */
-
-/**
- * Lots of useful information in controllers.h, look in there first
- */
-#include "controllers.h"
-#include "iic_mpu9150_utils.h"
-#include "quadposition.h"
-#include "util.h"
-#include "uart.h"
-#include "sleep.h"
-#include "stdio.h"
-#include <math.h>
-
-// 0 was -6600
-//int motor0_bias = -4500, motor1_bias = 100, motor2_bias = 5300, motor3_bias = 10300;
-int motor0_bias = -9900, motor1_bias = -200, motor2_bias = -10200, motor3_bias = 250;
-
-/**
- * Takes the raw signal inputs from the receiver and filters it so the
- * quadcopter doesn't flip or do something extreme
- */
-void filter_PWMs(int* mixer) {
-
-}
-
-/**
- * Converts PWM signals into 4 channel pitch, roll, yaw, throttle
- */
-// javey: unused
-void PWMS_to_Aero(int* PWMs, int* aero) {
- /**
- * Reference used to derive equations
- */
- // pwm0 = throttle_base - pitch_base + yaw_base;
- // pwm1 = throttle_base + roll_base - yaw_base;
- // pwm2 = throttle_base - roll_base - yaw_base;
- // pwm3 = throttle_base + pitch_base + yaw_base;
-
- aero[THROTTLE] = (PWMs[0] + PWMs[1] + PWMs[2] + PWMs[3]) / 4;
- aero[ROLL] = (PWMs[1] - PWMs[2]) / 2;
- aero[PITCH] = (PWMs[3] - PWMs[0]) / 2;
- aero[YAW] = (PWMs[3] + PWMs[0] - PWMs[1] - PWMs[2]) / 4;
-}
-
-
diff --git a/quad/xsdk_workspace/imu_logger/src/controllers.h b/quad/xsdk_workspace/imu_logger/src/controllers.h
deleted file mode 100644
index 33b6237957b49d4253c3fec7c26b917cdfe3f74e..0000000000000000000000000000000000000000
--- a/quad/xsdk_workspace/imu_logger/src/controllers.h
+++ /dev/null
@@ -1,109 +0,0 @@
-/*
- * controllers.h
- *
- * Created on: Oct 11, 2014
- * Author: ucart
- */
-#ifndef _CONTROLLERS_H
-#define _CONTROLLERS_H
-
-#include "util.h"
-#include "quadposition.h"
-
-/**
- *
- * USING PLUS CONFIGURATION
- * 0 R CW E
- * 1 + 2 W R CCW CCW N S
- * 3 W CW W
- *
- *
- * USING X CONFIGURATION
- *
- *
- * 0 2 R R CW CCW
- * x
- * 1 3 W W CCW CW
- */
-#define X_CONFIG
-
-/**
- * Pin hook ups
- *
- * PWM Recorder port mapping
- * 3.3 V || GND || PWM_REC_3 || PWM_REC_2 || PWM_REC_1 || PWM_REC_0
- *
- * Rx PINS
- * GEAR -> JD7
- * THROTTLE -> JE1
- * AILE -> JE2
- * ELEV -> JE3
- * RUDD -> JE4
- * GND -> JE5
- *
- * JE PMOD TOP PINS
- * Unused || GND || YAW || PITCH || ROLL || THROTTLE
- *
- * BOTTOM PINS
- *
- * Unused || GND || PWM3 || PWM2 || PWM1 || PWM0
- */
-
-/**
- * Gear settings
- * 1 - F mode = 171135
- * 0 - Gear = 118363
- * Kill if gear is around 118363
- */
-
-
-/*
- * Aero channel declaration
- */
-
-#define THROTTLE 0
-#define ROLL 1
-#define PITCH 2
-#define YAW 3
-#define GEAR 4
-#define FLAP 5
-
-/**
- * Signals from the Rx mins, maxes and ranges
- */
-#define THROTTLE_MAX 191900
-#define THROTTLE_MIN 110200
-#define THROTTLE_RANGE THROTTLE_MAX - THROTTLE_MIN
-
-#define ROLL_MAX 170200
-#define ROLL_MIN 129400
-#define ROLL_CENTER 149800
-#define ROLL_RANGE ROLL_MAX - ROLL_MIN
-
-#define PITCH_MAX 169900
-#define PITCH_MIN 129500
-#define PITCH_CENTER 149700
-#define PITCH_RANGE PITCH_MAX - PITCH_MIN
-
-#define YAW_MAX 169400
-#define YAW_MIN 129300
-#define YAW_CENTER 149800
-#define YAW_RANGE YAW_MAX - YAW_MIN
-
-#define GEAR_1 170800
-#define GEAR_0 118300
-
-#define FLAP_1 192000
-#define FLAP_0 107600
-
-#define GEAR_KILL GEAR_0 // The kill point for the program
-#define BASE 150000
-
-#define min 100000
-#define max 200000
-
-void filter_PWMs(int* mixer);
-void PWMS_to_Aero(int* PWMs, int* aero); // <= javey: unused
-void Aero_to_PWMS(int* PWMs, int* aero);
-
-#endif /* _CONTROLLERS_H */
diff --git a/quad/xsdk_workspace/imu_logger/src/conversion.c b/quad/xsdk_workspace/imu_logger/src/conversion.c
deleted file mode 100644
index 2f1d58a8976569c42d6c1dab136ca74293984bbf..0000000000000000000000000000000000000000
--- a/quad/xsdk_workspace/imu_logger/src/conversion.c
+++ /dev/null
@@ -1,67 +0,0 @@
-
-#include "conversion.h"
-
-// takes a floating point percentage and converts to a
-// receiver command in the range min_rx_cmd to max_rx_cmd
-// if percentage is < 0 then returns a value less than
-// center_rx_cmd but >= min_rx_cmd
-// if percentage is > 0 then returns a value greater than
-// center_rx_cmd but <= max_rx_cmd
-// if percentage is = 0 then returns center_rx_cmd
-// acceptable range of values for percentage: [-100, 100]
-int map_to_rx_cmd(float percentage, int min_rx_cmd, int center_rx_cmd,
- int max_rx_cmd)
-{
- //bounds checking
- // imagine a human flying and the stick is minimum
- if(percentage >= 100.0)
- return max_rx_cmd;
-
- //bounds checking
- // imagine a human flying and the stick is minimum
- if(percentage <= -100.0)
- return min_rx_cmd;
-
- // 0 percentage is center cmd
- // imagine a human flying and not touching the stick
- if(percentage == 0)
- return center_rx_cmd;
-
- // calculate and return a percentage of the max/min command
- if(percentage < 0)
- {
- return center_rx_cmd + ((int) (percentage/100.0 *
- ((float) max_rx_cmd - center_rx_cmd)));
- }
- else
- {
- return center_rx_cmd + ((int) (percentage/100.0 * (
- (float) center_rx_cmd - min_rx_cmd)));
- }
-
- return 0;
-}
-
-int convert_to_receiver_cmd(int var_to_convert, float max_var_to_convert, float min_var_to_convert, int center_receiver_cmd, int max_receiver_cmd, int min_receiver_cmd)
-{
-
- if(var_to_convert <= 0) {
- int ret = ((int) ((float)(min_receiver_cmd - center_receiver_cmd))/min_var_to_convert * var_to_convert) + center_receiver_cmd;
-
- if(ret < min_receiver_cmd)
- ret = min_receiver_cmd;
-
- return ret;
- }
-
- else {
- int ret = ((int) ((float)(max_receiver_cmd - center_receiver_cmd))/max_var_to_convert * var_to_convert) + center_receiver_cmd;
-
- if(ret > max_receiver_cmd)
- ret = max_receiver_cmd;
-
- return ret;
- }
-
- return 0;
-}
diff --git a/quad/xsdk_workspace/imu_logger/src/conversion.h b/quad/xsdk_workspace/imu_logger/src/conversion.h
deleted file mode 100644
index 91827016f549a762ae13971b7577a4a8121dba04..0000000000000000000000000000000000000000
--- a/quad/xsdk_workspace/imu_logger/src/conversion.h
+++ /dev/null
@@ -1,9 +0,0 @@
-#ifndef _CONVERSION_H
-#define _CONVERSION_H
-
-
-int convert_to_receiver_cmd(int var_to_convert, float max_var_to_convert, float min_var_to_convert, int center_receiver_cmd, int max_receiver_cmd, int min_receiver_cmd);
-
-int map_to_rx_cmd(float percentage, int min_rx_cmd, int center_rx_cmd, int max_rx_cmd);
-
-#endif /* _CONVERSION_H */
diff --git a/quad/xsdk_workspace/imu_logger/src/gam.h b/quad/xsdk_workspace/imu_logger/src/gam.h
deleted file mode 100644
index 6c5ed85541558c33138ef1ee7c0d358126d39af3..0000000000000000000000000000000000000000
--- a/quad/xsdk_workspace/imu_logger/src/gam.h
+++ /dev/null
@@ -1,40 +0,0 @@
-#ifndef _GAM_H
-#define _GAM_H
-
-#include "xbasic_types.h"
-
-//Gyro, accelerometer, and magnetometer data structure
-//Used for reading an instance of the sensor data
-typedef struct {
-
- // GYRO
- //Xint16 raw_gyro_x, raw_gyro_y, raw_gyro_z;
-
- float gyro_xVel_p; // In degrees per second
- float gyro_yVel_q;
- float gyro_zVel_r;
-
- // ACCELEROMETER
- //Xint16 raw_accel_x, raw_accel_y, raw_accel_z;
-
- float accel_x; //In g
- float accel_y;
- float accel_z;
-
- float accel_roll;
- float accel_pitch;
-
-
- // MAG
- //Xint16 raw_mag_x, raw_mag_y, raw_mag_z;
-
- float heading; // In degrees
-
- float mag_x; //Magnetic north: ~50 uT
- float mag_y;
- float mag_z;
-
-
-
-}gam_t;
-#endif /* _GAM_H */
diff --git a/quad/xsdk_workspace/imu_logger/src/iic_mpu9150_utils.c b/quad/xsdk_workspace/imu_logger/src/iic_mpu9150_utils.c
deleted file mode 100644
index e5915fa147a7beed91364fbfc2f3cfb3c406bed0..0000000000000000000000000000000000000000
--- a/quad/xsdk_workspace/imu_logger/src/iic_mpu9150_utils.c
+++ /dev/null
@@ -1,243 +0,0 @@
-/**
- * IIC_MPU9150_UTILS.c
- *
- * Utility functions for using I2C on a Diligent Zybo board and
- * focused on the SparkFun MPU9150
- *
- * For function descriptions please see iic_mpu9150_utils.h
- *
- * Author: ucart
- * Created: 01/20/2015
- */
-
-#include <stdio.h>
-#include <sleep.h>
-#include <math.h>
-
-#include "xparameters.h"
-#include "iic_mpu9150_utils.h"
-#include "xbasic_types.h"
-#include "xiicps.h"
-
-XIicPs_Config* i2c_config;
-XIicPs I2C0;
-double magX_correction = -1, magY_correction, magZ_correction;
-
-int initI2C0(){
-
- //Make sure CPU_1x clk is enabled for I2C controller
- Xuint16* aper_ctrl = (Xuint16*) IO_CLK_CONTROL_REG_ADDR;
-
- if(*aper_ctrl & 0x00040000){
- xil_printf("CPU_1x is set to I2C0\r\n");
- }
-
- else{
- xil_printf("CPU_1x is not set to I2C0..Setting now\r\n");
- *aper_ctrl |= 0x00040000;
- }
-
-
- // Look up
- i2c_config = XIicPs_LookupConfig(XPAR_PS7_I2C_0_DEVICE_ID);
-
- XStatus status = XIicPs_CfgInitialize(&I2C0, i2c_config, i2c_config->BaseAddress);
-
- // Check if initialization was successful
- if(status != XST_SUCCESS){
- printf("ERROR (initI2C0): Initializing I2C0\r\n");
- return -1;
- }
-
- // Reset the controller and set the clock to 400kHz
- XIicPs_Reset(&I2C0);
- XIicPs_SetSClk(&I2C0, 400000);
-
-
- return 0;
-}
-
-int startMPU9150(){
-
- // Device Reset & Wake up
- iic0Write(0x6B, 0x80);
- usleep(5000);
-
- // Set clock reference to Z Gyro
- iic0Write(0x6B, 0x03);
- // Configure Digital Low/High Pass filter
- iic0Write(0x1A,0x06); // Level 4 low pass on gyroscope
-
- // Configure Gyro to 2000dps, Accel. to +/-8G
- iic0Write(0x1B, 0x18);
- iic0Write(0x1C, 0x10);
-
- // Enable I2C bypass for AUX I2C (Magnetometer)
- iic0Write(0x37, 0x02);
-
- // Setup Mag
- iic0Write(0x37, 0x02); //INT_PIN_CFG -- INT_LEVEL=0 ; INT_OPEN=0 ; LATCH_INT_EN=0 ; INT_RD_CLEAR=0 ; FSYNC_INT_LEVEL=0 ; FSYNC_INT_EN=0 ; I2C_BYPASS_EN=0 ; CLKOUT_EN=0
-
- usleep(100000);
-
- int i;
- gam_t temp_gam;
-
- // Do about 20 reads to warm up the device
- for(i=0; i < 20; ++i){
- if(get_gam_reading(&temp_gam) == -1){
- printf("ERROR (startMPU9150): error occured while getting GAM data\r\n");
- return -1;
- }
- usleep(1000);
- }
-
- return 0;
-}
-
-void stopMPU9150(){
-
- //Put MPU to sleep
- iic0Write(0x6B, 0b01000000);
-}
-
-void iic0Write(u8 register_addr, u8 data){
-
- u16 device_addr = MPU9150_DEVICE_ADDR;
- u8 buf[] = {register_addr, data};
-
- // Check if within register range
- if(register_addr < 0 || register_addr > 0x75){
- printf("ERROR (iic0Write) : Cannot write to register address, 0x%x: out of bounds\r\n", register_addr);
- return;
- }
-
- if(register_addr <= 0x12){
- device_addr = MPU9150_COMPASS_ADDR;
- }
-
- XIicPs_MasterSendPolled(&I2C0, buf, 2, device_addr);
-
-}
-
-void iic0Read(u8* recv_buffer, u8 register_addr, int size){
-
- u16 device_addr = MPU9150_DEVICE_ADDR;
- u8 buf[] = {register_addr};
-
- // Check if within register range
- if(register_addr < 0 || register_addr > 0x75){
- printf("ERROR (iic0Read): Cannot read register address, 0x%x: out of bounds\r\n", register_addr);
- }
-
- // Set device address to the if 0x00 <= register address <= 0x12
- if(register_addr <= 0x12){
- device_addr = MPU9150_COMPASS_ADDR;
- }
-
-
- XIicPs_MasterSendPolled(&I2C0, buf, 1, device_addr);
- XIicPs_MasterRecvPolled(&I2C0, recv_buffer,size,device_addr);
-}
-
-void CalcMagSensitivity(){
-
- u8 buf[3];
- u8 ASAX, ASAY, ASAZ;
-
- // Quickly read from the factory ROM to get correction coefficents
- iic0Write(0x0A, 0x0F);
- usleep(10000);
-
- // Read raw adjustment values
- iic0Read(buf, 0x10,3);
- ASAX = buf[0];
- ASAY = buf[1];
- ASAZ = buf[2];
-
- // Set the correction coefficients
- magX_correction = (ASAX-128)*0.5/128 + 1;
- magY_correction = (ASAY-128)*0.5/128 + 1;
- magZ_correction = (ASAZ-128)*0.5/128 + 1;
-}
-
-
-void ReadMag(gam_t* gam){
-
- u8 mag_data[6];
- Xint16 raw_magX, raw_magY, raw_magZ;
-
- // Grab calibrations if not done already
- if(magX_correction == -1){
- CalcMagSensitivity();
- }
-
- // Set Mag to single read mode
- iic0Write(0x0A, 0x01);
- usleep(10000);
- mag_data[0] = 0;
-
- // Keep checking if data is ready before reading new mag data
- while(mag_data[0] == 0x00){
- iic0Read(mag_data, 0x02, 1);
- }
-
- // Get mag data
- iic0Read(mag_data, 0x03, 6);
-
- raw_magX = (mag_data[1] << 8) | mag_data[0];
- raw_magY = (mag_data[3] << 8) | mag_data[2];
- raw_magZ = (mag_data[5] << 8) | mag_data[4];
-
- // Set magnetometer data to output
- gam->mag_x = raw_magX * magX_correction;
- gam->mag_y = raw_magY * magY_correction;
- gam->mag_z = raw_magZ * magZ_correction;
-
-}
-
-/**
- * Get Gyro Accel Mag (GAM) information
- */
-int get_gam_reading(gam_t* gam) {
-
- Xint16 raw_accel_x, raw_accel_y, raw_accel_z;
- Xint16 gyro_x, gyro_y, gyro_z;
-
- Xuint8 sensor_data[ACCEL_GYRO_READ_SIZE] = {};
-
- // We should only get mag_data ~10Hz
- //Xint8 mag_data[6] = {};
-
- //readHandler = iic0_read_bytes(sensor_data, ACCEL_GYRO_BASE_ADDR, ACCEL_GYRO_READ_SIZE);
- iic0Read(sensor_data, ACCEL_GYRO_BASE_ADDR, ACCEL_GYRO_READ_SIZE);
-
- //Calculate accelerometer data
- raw_accel_x = sensor_data[ACC_X_H] << 8 | sensor_data[ACC_X_L];
- raw_accel_y = sensor_data[ACC_Y_H] << 8 | sensor_data[ACC_Y_L];
- raw_accel_z = sensor_data[ACC_Z_H] << 8 | sensor_data[ACC_Z_L];
-
- // put in G's
- gam->accel_x = (raw_accel_x / 4096.0) + ACCEL_X_BIAS; // 4,096 is the gain per LSB of the measurement reading based on a configuration range of +-8g
- gam->accel_y = (raw_accel_y / 4096.0) + ACCEL_Y_BIAS;
- gam->accel_z = (raw_accel_z / 4096.0) + ACCEL_Z_BIAS;
-
- //Get X and Y angles
- // javey: this assigns accel_(pitch/roll) in units of radians
- //gam->accel_pitch = atan(gam->accel_x / sqrt(gam->accel_y*gam->accel_y + gam->accel_z*gam->accel_z));
- //gam->accel_roll = -atan(gam->accel_y / sqrt(gam->accel_x*gam->accel_x + gam->accel_z*gam->accel_z)); // negative because sensor board is upside down
-
- //Convert gyro data to rate (we're only using the most 12 significant bits)
- gyro_x = (sensor_data[GYR_X_H] << 8) | (sensor_data[GYR_X_L]); //* G_GAIN;
- gyro_y = (sensor_data[GYR_Y_H] << 8 | sensor_data[GYR_Y_L]);// * G_GAIN;
- gyro_z = (sensor_data[GYR_Z_H] << 8 | sensor_data[GYR_Z_L]);// * G_GAIN;
-
- //Get the number of degrees
- //javey: converted to radians to following SI units
- gam->gyro_xVel_p = ((gyro_x / GYRO_SENS) * DEG_TO_RAD);// + GYRO_X_BIAS;
- gam->gyro_yVel_q = ((gyro_y / GYRO_SENS) * DEG_TO_RAD);// + GYRO_Y_BIAS;
- gam->gyro_zVel_r = ((gyro_z / GYRO_SENS) * DEG_TO_RAD);// + GYRO_Z_BIAS;
-
- return 0;
-
-}
diff --git a/quad/xsdk_workspace/imu_logger/src/iic_mpu9150_utils.h b/quad/xsdk_workspace/imu_logger/src/iic_mpu9150_utils.h
deleted file mode 100644
index 2ccb8cf394264209c302c29d48cbcb6154c959a9..0000000000000000000000000000000000000000
--- a/quad/xsdk_workspace/imu_logger/src/iic_mpu9150_utils.h
+++ /dev/null
@@ -1,167 +0,0 @@
-/* iic_mpu9150_utils.h
- *
- * A header file for the prototyping constants used for
- * the I2C Controller 0 (I2C0) on the Zybo Board
- *
- * This file is intended SOLELY for the Sparkfun MPU9150
- * and the Diligent ZyBo Board
- *
- * Author: ucart
- *
- */
-
-
-#ifndef IIC_MPU9150_UTILS_H
-#define IIC_MPU9150_UTILS_H
-
-
-#include "type_def.h"
-#include "hardware/hw_iface.h"
-
-// System configuration registers
-// (Please see Appendix B: System Level Control Registers in the Zybo TRM)
-#define IIC_SYSTEM_CONTROLLER_RESET_REG_ADDR (0xF8000224)
-#define IO_CLK_CONTROL_REG_ADDR (0xF800012C)
-
-// IIC0 Registers
-#define IIC0_CONTROL_REG_ADDR (0xE0004000)
-#define IIC0_STATUS_REG_ADDR (0xE0004004)
-#define IIC0_SLAVE_ADDR_REG (0xE0004008)
-#define IIC0_DATA_REG_ADDR (0xE000400C)
-#define IIC0_INTR_STATUS_REG_ADDR (0xE0004010)
-#define IIC0_TRANFER_SIZE_REG_ADDR (0xE0004014)
-#define IIC0_INTR_EN (0xE0004024)
-#define IIC0_TIMEOUT_REG_ADDR (0xE000401C)
-
-// MPU9150 Sensor Defines (Address is defined on the Sparkfun MPU9150 Datasheet)
-#define MPU9150_DEVICE_ADDR 0b01101000
-#define MPU9150_COMPASS_ADDR 0x0C
-
-
-#define ACCEL_GYRO_READ_SIZE 14 //Bytes
-#define ACCEL_GYRO_BASE_ADDR 0x3B //Starting register address
-
-#define MAG_READ_SIZE 6
-#define MAG_BASE_ADDR 0x03
-
-#define RAD_TO_DEG 57.29578
-#define DEG_TO_RAD 0.0174533
-
-// Array indicies when reading from ACCEL_GYRO_BASE_ADDR
-#define ACC_X_H 0
-#define ACC_X_L 1
-#define ACC_Y_H 2
-#define ACC_Y_L 3
-#define ACC_Z_H 4
-#define ACC_Z_L 5
-
-#define GYR_X_H 8
-#define GYR_X_L 9
-#define GYR_Y_H 10
-#define GYR_Y_L 11
-#define GYR_Z_H 12
-#define GYR_Z_L 13
-
-#define MAG_X_L 0
-#define MAG_X_H 1
-#define MAG_Y_L 2
-#define MAG_Y_H 3
-#define MAG_Z_L 4
-#define MAG_Z_H 5
-
-//Interrupt Status Register Masks
-#define ARB_LOST (0x200)
-#define RX_UNF (0x80)
-#define TX_OVF (0x40)
-#define RX_OVF (0x20)
-#define SLV_RDY (0x10)
-#define TIME_OUT (0x08)
-#define NACK (0x04)
-#define MORE_DAT (0x02)
-#define TRANS_COMPLETE (0x01)
-
-#define WRITE_INTR_MASK (ARB_LOST | TIME_OUT | RX_OVF | TX_OVF | NACK)
-#define READ_INTR_MASK (ARB_LOST | TIME_OUT | RX_OVF | RX_UNF | NACK)
-
-
-// Gyro is configured for +/-2000dps
-// Sensitivity gain is based off MPU9150 datasheet (pg. 11)
-#define GYRO_SENS 16.4
-
-#define GYRO_X_BIAS 0.005f
-#define GYRO_Y_BIAS -0.014f
-#define GYRO_Z_BIAS 0.045f
-
-#define ACCEL_X_BIAS 0.023f
-#define ACCEL_Y_BIAS 0.009f
-#define ACCEL_Z_BIAS 0.087f
-
-void iic_set_global(struct I2CDriver *given_i2c);
-
-// Initialize hardware; Call this FIRST before calling any other functions
-int initI2C0();
-
-void iic0Write(u8 register_addr, u8 data);
-void iic0Read(u8* recv_buffer, u8 register_addr, int size);
-
-
-// Wake up the MPU for data collection
-// Configure Gyro/Accel/Mag
-int startMPU9150();
-
-// Put MPU back to sleep
-void stopMPU9150();
-
-void CalcMagSensitivity();
-void ReadMag(gam_t* gam);
-void ReadGyroAccel(gam_t* gam);
-
-int get_gam_reading(gam_t* gam);
-
-
-/////////////
-// Deprecated functions below
-/////////////
-
-
-// Initialize hardware; Call this FIRST before calling any other functions
-void init_iic0();
-
-// Wake up the MPU for data collection
-void start_mpu9150();
-
-// Put MPU back to sleep
-void stop_mpu9150();
-
-
-// Write a byte of data at the given register address on the MPU
-void iic0_write(u16 reg_addr, u8 data);
-
-// Read a single byte at a given register address on the MPU
-u8 iic0_read(u16 reg_addr);
-
-// Read multiple bytes consecutively at a starting register address
-// places the resulting bytes in rv
-int iic0_read_bytes(u8* rv, u16 reg_addr, int bytes);
-
-// Helper function to initialize I2C0 controller on the Zybo board
-// Called by init_iic0
-void iic0_hw_init();
-
-
-// Clears the interrupt status register
-// Called by configuration functions
-void iic0_clear_intr_status();
-
-
-
-// Configure I2C0 controller on Zybo to receive data
-void iic0_config_ctrl_to_receive();
-
-// Configure I2C0 controller to transmit data
-void iic0_config_ctrl_to_transmit();
-
-
-void wake_mag();
-
-#endif /*IIC_MPU9150_UTILS_H*/
diff --git a/quad/xsdk_workspace/imu_logger/src/initialize_components.c b/quad/xsdk_workspace/imu_logger/src/initialize_components.c
deleted file mode 100644
index 84e37863fd08d33ce92ca6a2bcbd0fd6840605ec..0000000000000000000000000000000000000000
--- a/quad/xsdk_workspace/imu_logger/src/initialize_components.c
+++ /dev/null
@@ -1,130 +0,0 @@
-/*
- * initialize_components.c
- *
- * Created on: Feb 20, 2016
- * Author: ucart
- */
-
-#include "initialize_components.h"
-#include "communication.h"
-
-extern int Xil_AssertWait;
-
-int protection_loops()
-{
- int rc_commands[6]; // 6 "receiver_input" elements: Throttle, Pitch, Roll, Yaw, Gear, and Flap
-
- read_rec_all(rc_commands);
-
- // wait for RC receiver to connect to transmitter
- while(rc_commands[THROTTLE] < 75000)
- read_rec_all(rc_commands);
-
- // wait until throttle is low and the gear switch is engaged (so you don't immediately break out of the main loop below)
- // also wait for the flight mode to be set to manual
- while(rc_commands[THROTTLE] > 125000 || read_kill(rc_commands[GEAR]) || !read_flap(rc_commands[FLAP]))
- read_rec_all(rc_commands);
-
- // wait until the ground station has connected to the quad and acknowledged that its ready to start
- stringBuilder_t * ack_packet = stringBuilder_create();
- while(1)
- {
- // --------------------------------------
- // Send request to ground station to start sending VRPN
- char buf[255] = {};
- int i;
-
- // 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), "The quad is ready to receive VRPN data.\r\n");
- unsigned char *responsePacket;
-
- metadata_t metadata =
- {
- BEGIN_CHAR,
- MessageTypes[4].ID,
- MessageTypes[4].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]);
- }
-
- usleep(10000);
-
- tryReceivePacket(ack_packet, 0);
-
- unsigned char * data;
- metadata_t md;
- parse_packet((unsigned char *) ack_packet->buf, &data, &md);
-
- if(md.msg_type == 0x04 && md.msg_subtype == 0x01)
- break;
-
- // --------------------------------------
- }
-
- // let the pilot/observers know that the quad is now active
- MIO7_led_on();
-
- return 0;
-}
-
-int initializeAllComponents(user_input_t * user_input_struct, log_t * log_struct, raw_sensor_t * raw_sensor_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, actuator_command_t * actuator_command_struct)
-{
- // Turn off LED 7 to let observers know that the quad is not yet active
- MIO7_led_off();
-
- // Use the stringbuilder to keep track of data received
- if(!(user_input_struct->sb = stringBuilder_create()))
- return -1;
-
- // Initialize the controller
- //control_algorithm_init(parameter_struct);
-
- // Xilinx given initialization
- init_platform();
-
- //disable blocking asserts
- //Xil_AssertWait = FALSE;
-
- // Initialize UART0 (Bluetooth)
- if(!uart0_init(XPAR_PS7_UART_0_DEVICE_ID, 921600))
- return -1;
-
- uart0_clearFIFOs();
-
- //Enable blocking asserts
- //Xil_AssertWait = TRUE;
-
- // Initialize I2C controller and start the sensor board
- if (initI2C0() == -1) {
- return -1;
- }
-
- // Initialize PWM Recorders and Motor outputs
- pwm_init();
-
- // Initialize loop timers
- timer_init();
-
- //manual flight mode
- user_defined_struct->flight_mode = MANUAL_FLIGHT_MODE;
-
- // Get the first loop data from accelerometer for the gyroscope to use
- if(sensor_init(raw_sensor_struct, sensor_struct) == -1)
- return -1;
-
- return 0;
-}
diff --git a/quad/xsdk_workspace/imu_logger/src/initialize_components.h b/quad/xsdk_workspace/imu_logger/src/initialize_components.h
deleted file mode 100644
index 3c5b95e37f9f88cd52001392081ddd4bc0477952..0000000000000000000000000000000000000000
--- a/quad/xsdk_workspace/imu_logger/src/initialize_components.h
+++ /dev/null
@@ -1,43 +0,0 @@
-/*
- * initialize_components.h
- *
- * Created on: Nov 12, 2015
- * Author: ucart
- */
-
-#ifndef INITALIZE_COMPONENTS_H_
-#define INITALIZE_COMPONENTS_H_
-
-#include "timer.h"
-#include "control_algorithm.h"
-#include "platform.h"
-#include "uart.h"
-#include "iic_mpu9150_utils.h"
-#include "util.h"
-#include "controllers.h"
-
-/**
- * @brief
- * Runs loops to make sure the quad is responding and in the correct state before starting.
- *
- * @return
- * error message
- *
- */
-int protection_loops();
-
-/**
- * @brief
- * Initializes the sensors, communication, and anything else that needs
- * initialization.
- *
- * @return
- * error message
- *
- */
-int initializeAllComponents(user_input_t * user_input_struct, log_t * log_struct, raw_sensor_t * raw_sensor_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, actuator_command_t * actuator_command_struct);
-
-#endif /* INITALIZE_COMPONENTS_H_ */
-
diff --git a/quad/xsdk_workspace/imu_logger/src/log_data.c b/quad/xsdk_workspace/imu_logger/src/log_data.c
deleted file mode 100644
index 2acac6dc79bb30e9fd93d56c0836be0fd767f100..0000000000000000000000000000000000000000
--- a/quad/xsdk_workspace/imu_logger/src/log_data.c
+++ /dev/null
@@ -1,267 +0,0 @@
-/*
- * log_data.c
- *
- * Created on: Feb 20, 2016
- * Author: ucart
- */
-
-#include "log_data.h"
-#include "communication.h"
-
-// Current index of the log array
-int arrayIndex = 0;
-// Size of the array
-int arraySize = LOG_STARTING_SIZE;
-int resized = 0;
-
-// The number of times we resized the array
-int resizeCount = 0;
-
-// Pointer to point to the array with all the logging information
-// for now its not dynamic
-log_t logArray[LOG_STARTING_SIZE * 3];// up to 60 seconds of log
-
-int log_data(log_t* log_struct)
-{
- updateLog(*log_struct);
- return 0;
-}
-
-/**
- * Fills up an xbox hueg amount of memory with log data
- */
-void updateLog(log_t log_struct){
- // If the first iteration, allocate enough memory for "arraySize" elements of logging
-// if(logArray == NULL){
-// // size in memory is 1,720,320 bytes (1.64 megabytes) because logging struct is 420 bytes each
-// // up to 20 seconds of log before resizing
-// logArray = malloc(LOG_STARTING_SIZE * sizeof(log_t));
-// uart0_sendStr("initialized log array.\n");
-// sleep(1);
-// }
-
- // semi dynamic log
-// if((arrayIndex >= arraySize - 1) && (!resized)){
-// realloc(logArray, LOG_STARTING_SIZE * 3 * sizeof(log_t)); // up to 60 seconds of log
-// resized = 1;
-// arraySize = LOG_STARTING_SIZE * 3;
-// uart0_sendStr("resized log array.\n");
-// sleep(1);
-// }
-
- if(arrayIndex >= arraySize - 1)
- {
- return;
- }
-
- // Add log to the array
- logArray[arrayIndex++] = log_struct;
-
- // If the index is too big, reallocate memory to double the size as before
-// if(arrayIndex == arraySize){
-// arraySize *= 2;
-// logArray = (log_t *) realloc(logArray, arraySize * sizeof(log_t));
-// ++resizeCount;
-// }
-// else if(arrayIndex > arraySize){
-// // Something fishy has occured
-// xil_printf("Array index is out of bounds. This shouldn't happen but somehow you did the impossible\n\r");
-// }
-}
-
-
-/**
- * Prints all the log information.
- *
- * TODO: This should probably be transmitting in binary instead of ascii
- */
-
-void printLogging(){
- int i;//, j;
- char buf[2304] = {};
- char comments[256] = {};
- char header[1024] = {};
- char units [1024] = {};
-
- sprintf(comments, "# MicroCART On-board Quad Log\r\n# Sample size: %d\r\n", arrayIndex);
- sprintf(header, "%%Time\t" "LoopPeriod\t"
-
- //current points (measurements)
- "X_Current_Position\t" "Y_Current_Position\t" "Z_Current_Position\t"
- "Cam_Meas_Roll\tCam_Meas_Pitch\tCam_Meas_Yaw\t"
- "Quad_Meas_Roll\tQuad_Meas_Pitch\t"
- "roll_velocity\tpitch_velocity\tyaw_velocity\t"
-
- //setpoints
- "X_setpoint\t" "Y_setpoint\t" "Z_setpoint\t"
- "Roll_setpoint\tPitch_setpoint\tYaw_setpoint\t"
- "Roll_vel_setpoint\tPitch_vel_setpoint\tYaw_vel_setpoint\t"
-
- //corrections
- "PID_x\t"
- "PID_y\t"
- "PID_z\t"
- "PID_roll\t"
- "PID_pitch\t"
- "PID_yaw\t"
- "PID_roll_vel\t"
- "PID_pitch_vel\t"
- "PID_yaw_vel\t"
-
- //trims
- "Roll_trim\tPitch_trim\tYaw_trim\tThrottle_trim\t"
-
- //motor commands
- "Motor_0\tMotor_1\tMotor_2\tMotor_3\n"
-
- );
-
-
- sprintf(units, "&sec\tsec\t"
-
- //current points
- "meters\tmeters\tmeters\t"
- "radians\tradians\tradians\t"
- "radians\tradians\t"
- "radians//sec\tradians//sec\tradians//sec\t"
-
- //setpoints
- "meters\tmeters\tmeters\t"
- "radians\tradians\tradians\t"
- "radians//sec\tradians//sec\tradians//sec\t"
-
- //corrections
- "radians\tradians\tradians\t"
- "radians//sec\tradians//sec\tradians//sec\t"
- "none\tnone\tnone\t"
-
- //trims
- "none\tnone\tnone\tnone\t"
-
- //motors
- "none\tnone\tnone\tnone\n"
-
- );
-
- strcat(buf,comments);
- strcat(buf,header);
- strcat(buf,units);
-
- // Send a reply to the ground station
- unsigned char *responsePacket;
-
- metadata_t metadata =
- {
- BEGIN_CHAR,
- MessageTypes[5].ID,
- MessageTypes[5].subtypes[0].ID,
- 0,
- (strlen(buf) + 1)
- };
- formatPacket(&metadata, buf, &responsePacket);
-
-// printf("Checksum: 0x%02x", responsePacket[metadata.data_len + 7]);
-
- // Send each byte of the packet individually
- for(i = 0; i < 8 + metadata.data_len; i++) {
- uart0_sendByte(responsePacket[i]);
- if(i < 8 || i == metadata.data_len + 8)
- printf("%d: 0x%02x\n", i, responsePacket[i]);
- }
-
- //uart0_sendBytes(buf, strlen(buf));
- //usleep(100000);
-
- /*************************/
- /* print & send log data */
- for(i = 0; i < arrayIndex; i++){
- char* logLine = format(logArray[i]);
-
-
- metadata_t metadata =
- {
- BEGIN_CHAR,
- MessageTypes[5].ID,
- MessageTypes[5].subtypes[0].ID,
- 0,
- (strlen(logLine) + 1)
- };
- formatPacket(&metadata, (u8 *)logLine, &responsePacket);
-
- // Send each byte of the packet individually
-// for(j = 0; j < 8 + metadata.data_len; j++) {
-// uart0_sendByte(responsePacket[j]);
-// printf("%d: 0x%02x\n", j, responsePacket[j]);
-// }
-// uart0_sendBytes(responsePacket, 8 + metadata.data_len);
-
- uart0_sendMetaData(metadata);
- uart0_sendBytes(logLine, metadata.data_len);
- //uart0_sendByte(0);
- uart0_sendByte(responsePacket[7 + metadata.data_len]);
-
-//
-// if((i % 5) == 0)
- usleep(15000);
-
- free(logLine);
- //break;
- }
-}
-
-
-char* format(log_t log){
- char *retString = malloc(4096*2);
-
- sprintf(retString, "%.3f\t%.4f\t" //Time and TimeSlice
-
- // current points
- "%.3f\t%.3f\t%.3f\t"
- "%.3f\t%.3f\t%.3f\t"
- "%.3f\t%.3f\t"
- "%.3f\t%.3f\t%.3f\t"
-
- //setpoints
- "%.3f\t%.3f\t%.3f\t"
- "%.3f\t%.3f\t%.3f\t"
- "%.3f\t%.3f\t%.3f\t"
-
- //corrections
- "%.3f\t%.3f\t%.3f\t"
- "%.3f\t%.3f\t%.3f\t"
- "%.3f\t%.3f\t%.3f\t"
-
- //trims
- "%.2f\t%.2f\t%.2f\t%.2f\t"
-
- //motors
- "%d\t%d\t%d\t%d\n"
-
- ,log.time_stamp, log.time_slice,
-
- // current points
- log.local_x_PID.current_point,log.local_y_PID.current_point, log.altitude_PID.current_point,
- log.currentQuadPosition.roll, log.currentQuadPosition.pitch, log.currentQuadPosition.yaw,
- log.roll_angle_filtered, log.pitch_angle_filtered,
- log.phi_dot, log.theta_dot, log.psi_dot,
-
- //setpoints
- log.local_x_PID.setpoint, log.local_y_PID.setpoint, log.altitude_PID.setpoint,
- log.angle_roll_PID.setpoint, log.angle_pitch_PID.setpoint, log.angle_yaw_PID.setpoint,
- log.ang_vel_roll_PID.setpoint, log.ang_vel_pitch_PID.setpoint, log.ang_vel_pitch_PID.setpoint,
-
- //corrections
- log.local_x_PID_values.pid_correction, log.local_y_PID_values.pid_correction, log.altitude_PID_values.pid_correction,
- log.angle_roll_PID_values.pid_correction, log.angle_pitch_PID_values.pid_correction, log.angle_yaw_PID_values.pid_correction,
- log.ang_vel_roll_PID_values.pid_correction, log.ang_vel_pitch_PID_values.pid_correction, log.ang_vel_yaw_PID_values.pid_correction,
-
- //trims
- log.trims.roll, log.trims.pitch, log.trims.yaw, log.trims.throttle,
-
- //motors
- log.motors[0], log.motors[1], log.motors[2], log.motors[3]
- );
-
-
- return retString;
-}
diff --git a/quad/xsdk_workspace/imu_logger/src/log_data.h b/quad/xsdk_workspace/imu_logger/src/log_data.h
deleted file mode 100644
index 994d043ba18abd76c73e8e95ba7e93f514c941e2..0000000000000000000000000000000000000000
--- a/quad/xsdk_workspace/imu_logger/src/log_data.h
+++ /dev/null
@@ -1,47 +0,0 @@
-/*
- * log_data.h
- *
- * Created on: Feb 20, 2016
- * Author: ucart
- */
-
-#ifndef LOG_DATA_H_
-#define LOG_DATA_H_
-
-#include <stdio.h>
-#include <stdlib.h>
-#include <string.h>
-#include "PID.h"
-#include "type_def.h"
-#include "uart.h"
-#include "sleep.h"
-
-#define LOG_STARTING_SIZE 4096 //262144 // 2^18 32768 2^15
-
-
-/**
- * @brief
- * Logs the data obtained throughout the controller loop.
- *
- * @param log_struct
- * structure of the data to be logged
- *
- * @return
- * error message
- *
- */
- int log_data(log_t* log_struct);
-
- /**
- * Fills up an xbox hueg amount of memory with log data
- */
- void updateLog(log_t log_struct);
-
- /**
- * Prints all the log information.
- */
- void printLogging();
-
- char* format(log_t log);
-
-#endif /* LOG_DATA_H_ */
diff --git a/quad/xsdk_workspace/imu_logger/src/lscript.ld b/quad/xsdk_workspace/imu_logger/src/lscript.ld
deleted file mode 100644
index a9e7524bb4b15ca61a3d8c76f6a001e3b4c86e9f..0000000000000000000000000000000000000000
--- a/quad/xsdk_workspace/imu_logger/src/lscript.ld
+++ /dev/null
@@ -1,284 +0,0 @@
-/*******************************************************************/
-/* */
-/* 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 : 0x100000;
-_HEAP_SIZE = DEFINED(_HEAP_SIZE) ? _HEAP_SIZE : 0x6400000;
-
-_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 = .;
-}
-
diff --git a/quad/xsdk_workspace/imu_logger/src/main.c b/quad/xsdk_workspace/imu_logger/src/main.c
deleted file mode 100644
index 1b3af90b98580dcf8dc76bd7aea58a07df35f60e..0000000000000000000000000000000000000000
--- a/quad/xsdk_workspace/imu_logger/src/main.c
+++ /dev/null
@@ -1,90 +0,0 @@
-/*
- * main.c
- *
- * Created on: Nov 11, 2015
- * Author: ucart
- */
-#include <stdio.h>
-#include "timer.h"
-#include "log_data.h"
-#include "initialize_components.h"
-#include "user_input.h"
-#include "sensor.h"
-#include "sensor_processing.h"
-#include "control_algorithm.h"
-#include "actuator_command_processing.h"
-#include "send_actuator_commands.h"
-#include "update_gui.h"
-
-int main()
-{
- // Structures to be used throughout
- modular_structs_t structs = { };
-
-
- // Initialize all required components and structs:
- // Uart, PWM receiver/generator, I2C, Sensor Board
- // Xilinx Platform, Loop Timer, Control Algorithm
- int init_error = initializeAllComponents(&(structs.user_input_struct), &(structs.log_struct),
- &(structs.raw_sensor_struct), &(structs.sensor_struct), &(structs.setpoint_struct), &(structs.parameter_struct),
- &(structs.user_defined_struct), &(structs.raw_actuator_struct), &(structs.actuator_command_struct));
-
- if (init_error != 0) {
- printf("ERROR (main): Problem initializing...Goodbye\r\n");
- return -1;
- }
-
- // Loops to make sure the quad is responding correctly before starting the control loop
- //protection_loops();
-
- printf("The quad loop is now beginning.\n");
-
- char send_str[512];
- double timestamp = 0;
- // Main control loop
- do
- {
- // Processing of loop timer at the beginning of the control loop
- timer_start_loop();
-
- // Get data from the sensors and put it into raw_sensor_struct
- get_sensors(&(structs.log_struct), &(structs.user_input_struct), &(structs.raw_sensor_struct));
-
-
- sprintf(send_str, "%f,%f,%f,%f,%f,%f,%f\n", timestamp,
- structs.raw_sensor_struct.gam.accel_x,
- structs.raw_sensor_struct.gam.accel_y,
- structs.raw_sensor_struct.gam.accel_z,
- structs.raw_sensor_struct.gam.gyro_xVel_p,
- structs.raw_sensor_struct.gam.gyro_yVel_q,
- structs.raw_sensor_struct.gam.gyro_zVel_r);
-
- uart0_sendStr(send_str);
-
- // Process the sensor data and put it into sensor_struct
- //sensor_processing(&(structs.log_struct), &(structs.user_input_struct), &(structs.raw_sensor_struct), &(structs.sensor_struct));
-
-
- //if(structs.user_defined_struct.flight_mode == MANUAL_FLIGHT_MODE)
- //MIO7_led_on();
-
- // Processing of loop timer at the end of the control loop
- timestamp = timer_end_loop(&(structs.log_struct));
-
- } while(!kill_condition(&(structs.user_input_struct)));
-
-
- stringBuilder_free((structs.user_input_struct).sb);
-
- pwm_kill();
-
- MIO7_led_off();
-
- printLogging();
-
- flash_MIO_7_led(10, 100);
-
- return 0;
-}
-
-
diff --git a/quad/xsdk_workspace/imu_logger/src/mio7_led.c b/quad/xsdk_workspace/imu_logger/src/mio7_led.c
deleted file mode 100644
index 4742add5b48abf1f3a6998dc0e8e0aaa4a54cc87..0000000000000000000000000000000000000000
--- a/quad/xsdk_workspace/imu_logger/src/mio7_led.c
+++ /dev/null
@@ -1,53 +0,0 @@
-/*
- * mio7_led.c
- *
- * Created on: Feb 20, 2016
- * Author: Amy Seibert
- */
-
- #include "mio7_led.h"
-
-void flash_MIO_7_led(int how_many_times, int ms_between_flashes)
-{
- if(how_many_times <= 0)
- return;
-
- while(how_many_times)
- {
- MIO7_led_on();
-
- usleep(ms_between_flashes * 500);
-
- MIO7_led_off();
-
- usleep(ms_between_flashes * 500);
-
- how_many_times--;
- }
-}
-
-void MIO7_led_off()
-{
- XGpioPs Gpio;
-
- XGpioPs_Config * ConfigPtr = XGpioPs_LookupConfig(XPAR_PS7_GPIO_0_DEVICE_ID);
- XGpioPs_CfgInitialize(&Gpio, ConfigPtr, ConfigPtr->BaseAddr);
-
- XGpioPs_SetDirectionPin(&Gpio, 7, 1);
-
- // Disable LED
- XGpioPs_WritePin(&Gpio, 7, 0x00);
-}
-
-void MIO7_led_on()
-{
- XGpioPs Gpio;
-
- XGpioPs_Config * ConfigPtr = XGpioPs_LookupConfig(XPAR_PS7_GPIO_0_DEVICE_ID);
- XGpioPs_CfgInitialize(&Gpio, ConfigPtr, ConfigPtr->BaseAddr);
-
- XGpioPs_SetDirectionPin(&Gpio, 7, 1);
-
- // Enable LED
- XGpioPs_WritePin(&Gpio, 7, 0x01);
-}
diff --git a/quad/xsdk_workspace/imu_logger/src/mio7_led.h b/quad/xsdk_workspace/imu_logger/src/mio7_led.h
deleted file mode 100644
index 2517f6241e7442f361bb41ba1f456979edb6e8a0..0000000000000000000000000000000000000000
--- a/quad/xsdk_workspace/imu_logger/src/mio7_led.h
+++ /dev/null
@@ -1,42 +0,0 @@
-/*
- * mio7_led.h
- *
- * Created on: Feb 20, 2016
- * Author: Amy Seibert
- */
-
-#ifndef MIO7_LED_H_
-#define MIO7_LED_H_
-
-#include <stdio.h>
-#include <xgpiops.h>
-#include "sleep.h"
-
-/**
- * @brief
- * Flashes the MIO7 LED how_many_times times and with ms_between_flashes between the flashes.
- *
- * @param how_many_times
- * times the LED should be flashed
- *
- * @param ms_between_flashes
- * time between flashes in milliseconds
- *
- */
-void flash_MIO_7_led(int how_many_times, int ms_between_flashes);
-
-/**
- * @brief
- * Turns off MIO7 LED.
- *
- */
-void MIO7_led_off();
-
-/**
- * @brief
- * Turns on MIO7 LED.
- *
- */
-void MIO7_led_on();
-
-#endif /* MIO7_LED_H_ */
diff --git a/quad/xsdk_workspace/imu_logger/src/new_PID.h b/quad/xsdk_workspace/imu_logger/src/new_PID.h
deleted file mode 100644
index fae2778ec8b422875e1d88d6ae98a70dee00a8e3..0000000000000000000000000000000000000000
--- a/quad/xsdk_workspace/imu_logger/src/new_PID.h
+++ /dev/null
@@ -1,57 +0,0 @@
-/*
- * 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 radians
-#define YAW_ANGULAR_VELOCITY_KP 200.0 * 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
-
-// when using units of radians
-#define ROLL_ANGULAR_VELOCITY_KP 100.0 * 46.0f
-#define ROLL_ANGULAR_VELOCITY_KI 0.0f
-#define ROLL_ANGULAR_VELOCITY_KD 100.0 * 5.5f
-#define ROLL_ANGLE_KP 15.0f
-#define ROLL_ANGLE_KI 0.0f
-#define ROLL_ANGLE_KD 0.2f
-#define YPOS_KP 0.08f
-#define YPOS_KI 0.01f
-#define YPOS_KD 0.1f
-
-
-//Pitch constants
-
-// when using units of radians
-#define PITCH_ANGULAR_VELOCITY_KP 100.0 * 46.0f
-#define PITCH_ANGULAR_VELOCITY_KI 0.0f
-#define PITCH_ANGULAR_VELOCITY_KD 100.0 * 5.5f
-#define PITCH_ANGLE_KP 15.0f
-#define PITCH_ANGLE_KI 0.0f
-#define PITCH_ANGLE_KD 0.2f
-#define XPOS_KP 0.08f
-#define XPOS_KI 0.01f
-#define XPOS_KD 0.1f
-
-
-//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_ */
diff --git a/quad/xsdk_workspace/imu_logger/src/new_log_data.c b/quad/xsdk_workspace/imu_logger/src/new_log_data.c
deleted file mode 100644
index 7b2b758f2722673336296c4a3d3f7525b8d4de91..0000000000000000000000000000000000000000
--- a/quad/xsdk_workspace/imu_logger/src/new_log_data.c
+++ /dev/null
@@ -1,225 +0,0 @@
-/*
- * log_data.c
- *
- * Created on: Feb 20, 2016
- * Author: ucart
- */
-
-/*
- #include "log_data.h"
-
-// Current index of the log array
-int arrayIndex = 0;
-// Size of the array
-int arraySize = LOG_STARTING_SIZE;
-int resized = 0;
-
-// The number of times we resized the array
-int resizeCount = 0;
-
-// Pointer to point to the array with all the logging information
-// for now its not dynamic
-log_t logArray[LOG_STARTING_SIZE * 3];// up to 60 seconds of log
-
-int log_data(log_t* log_struct)
-{
- updateLog(*log_struct);
- return 0;
-}
-
-*
- * Fills up an xbox hueg amount of memory with log data
-
-void updateLog(log_t log_struct){
- // If the first iteration, allocate enough memory for "arraySize" elements of logging
-// if(logArray == NULL){
-// // size in memory is 1,720,320 bytes (1.64 megabytes) because logging struct is 420 bytes each
-// // up to 20 seconds of log before resizing
-// logArray = malloc(LOG_STARTING_SIZE * sizeof(log_t));
-// uart0_sendStr("initialized log array.\n");
-// sleep(1);
-// }
-
- // semi dynamic log
-// if((arrayIndex >= arraySize - 1) && (!resized)){
-// realloc(logArray, LOG_STARTING_SIZE * 3 * sizeof(log_t)); // up to 60 seconds of log
-// resized = 1;
-// arraySize = LOG_STARTING_SIZE * 3;
-// uart0_sendStr("resized log array.\n");
-// sleep(1);
-// }
-
- if(arrayIndex >= arraySize - 1)
- {
- return;
- }
-
- // Add log to the array
- logArray[arrayIndex++] = log_struct;
-
- // If the index is too big, reallocate memory to double the size as before
-// if(arrayIndex == arraySize){
-// arraySize *= 2;
-// logArray = (log_t *) realloc(logArray, arraySize * sizeof(log_t));
-// ++resizeCount;
-// }
-// else if(arrayIndex > arraySize){
-// // Something fishy has occured
-// xil_printf("Array index is out of bounds. This shouldn't happen but somehow you did the impossible\n\r");
-// }
-}
-
-
-*
- * Prints all the log information.
- *
- * TODO: This should probably be transmitting in binary instead of ascii
-
-
-void printLogging(){
- int i, numBytes;
- char buf[2304] = {};
- char comments[256] = {};
- char header[1024] = {};
- char units [1024] = {};
-
- char tempLog[4096*2] = {};
-
- sprintf(comments, "# MicroCART On-board Quad Log\r\n# Sample size: %d\r\n", arrayIndex);
- sprintf(header, "%%Time\t" "LoopPeriod\t"
-
- //current points (measurements)
- "X_Current_Position\t" "Y_Current_Position\t" "Z_Current_Position\t"
- "Cam_Meas_Roll\tCam_Meas_Pitch\tCam_Meas_Yaw\t"
- "Quad_Meas_Roll\tQuad_Meas_Pitch\t"
- "roll_velocity\tpitch_velocity\tyaw_velocity\t"
-
- //setpoints
- "X_setpoint\t" "Y_setpoint\t" "Z_setpoint\t"
- "Roll_setpoint\tPitch_setpoint\tYaw_setpoint\t"
- "Roll_vel_setpoint\tPitch_vel_setpoint\tYaw_vel_setpoint\t"
-
- //corrections
- "PID_x\t"
- "PID_y\t"
- "PID_z\t"
- "PID_roll\t"
- "PID_pitch\t"
- "PID_yaw\t"
- "PID_roll_vel\t"
- "PID_pitch_vel\t"
- "PID_yaw_vel\t"
-
- //trims
- "Roll_trim\tPitch_trim\tYaw_trim\tThrottle_trim\t"
-
- //motor commands
- "Motor_0\tMotor_1\tMotor_2\tMotor_3\n"
-
- );
-
-
- sprintf(units, "&sec\tsec\t"
-
- //current points
- "meters\tmeters\tmeters\t"
- "radians\tradians\tradians\t"
- "radians\tradians\t"
- "radians//sec\tradians//sec\tradians//sec\t"
-
- //setpoints
- "meters\tmeters\tmeters\t"
- "radians\tradians\tradians\t"
- "radians//sec\tradians//sec\tradians//sec\t"
-
- //corrections
- "radians\tradians\tradians\t"
- "radians//sec\tradians//sec\tradians//sec\t"
- "none\tnone\tnone\t"
-
- //trims
- "none\tnone\tnone\tnone\t"
-
- //motors
- "none\tnone\tnone\tnone\n"
-
- );
-
- strcat(buf,comments);
- strcat(buf,header);
- strcat(buf,units);
-
-
- numBytes = logData(buf, tempLog);
- uart0_sendBytes(tempLog, strlen(tempLog));
- usleep(100000);
-
- ***********************
- print & send log data
- for(i = 0; i < arrayIndex; i++){
- char* logLine = format(logArray[i]);
- numBytes = logData(logLine, tempLog);
- uart0_sendBytes(tempLog, numBytes);
- usleep(10000);
- //free(logLine);
- //break;
- }
-}
-
-
-char* format(log_t log){
- char *retString = malloc(4096*2);
-
- sprintf(retString, "%.3f\t%.4f\t" //Time and TimeSlice
-
- // current points
- "%.3f\t%.3f\t%.3f\t"
- "%.3f\t%.3f\t%.3f\t"
- "%.3f\t%.3f\t"
- "%.3f\t%.3f\t%.3f\t"
-
- //setpoints
- "%.3f\t%.3f\t%.3f\t"
- "%.3f\t%.3f\t%.3f\t"
- "%.3f\t%.3f\t%.3f\t"
-
- //corrections
- "%.3f\t%.3f\t%.3f\t"
- "%.3f\t%.3f\t%.3f\t"
- "%.3f\t%.3f\t%.3f\t"
-
- //trims
- "%d\t%d\t%d\t%d\t"
-
- //motors
- "%d\t%d\t%d\t%d\n"
-
- ,log.time_stamp, log.time_slice,
-
- // current points
- log.local_x_PID.current_point,log.local_y_PID.current_point, log.altitude_PID.current_point,
- log.currentQuadPosition.roll, log.currentQuadPosition.pitch, log.currentQuadPosition.yaw,
- log.roll_angle_filtered, log.pitch_angle_filtered,
- log.phi_dot, log.theta_dot, log.psi_dot,
-
- //setpoints
- log.local_x_PID.setpoint, log.local_y_PID.setpoint, log.altitude_PID.setpoint,
- log.angle_roll_PID.setpoint, log.angle_pitch_PID.setpoint, log.angle_yaw_PID.setpoint,
- log.ang_vel_roll_PID.setpoint, log.ang_vel_pitch_PID.setpoint, log.ang_vel_pitch_PID.setpoint,
-
- //corrections
- log.local_x_PID_values.pid_correction, log.local_y_PID_values.pid_correction, log.altitude_PID_values.pid_correction,
- log.angle_roll_PID_values.pid_correction, log.angle_pitch_PID_values.pid_correction, log.angle_yaw_PID_values.pid_correction,
- log.ang_vel_roll_PID_values.pid_correction, log.ang_vel_pitch_PID_values.pid_correction, log.ang_vel_yaw_PID_values.pid_correction,
-
- //trims
- log.trims.roll, log.trims.pitch, log.trims.yaw, log.trims.throttle,
-
- //motors
- log.motors[0], log.motors[1], log.motors[2], log.motors[3]
- );
-
-
- return retString;
-}
-*/
diff --git a/quad/xsdk_workspace/imu_logger/src/new_log_data.h b/quad/xsdk_workspace/imu_logger/src/new_log_data.h
deleted file mode 100644
index 932eb1a9207f5372fac04658e85cfbe9b7359ddd..0000000000000000000000000000000000000000
--- a/quad/xsdk_workspace/imu_logger/src/new_log_data.h
+++ /dev/null
@@ -1,48 +0,0 @@
-/*
- * log_data.h
- *
- * Created on: Feb 20, 2016
- * Author: ucart
- */
-
-#ifndef LOG_DATA_H_
-#define LOG_DATA_H_
-
-#include <stdio.h>
-#include <stdlib.h>
-#include <string.h>
-#include "PID.h"
-#include "type_def.h"
-#include "uart.h"
-#include "sleep.h"
-#include "communication.h"
-
-#define LOG_STARTING_SIZE 4096 //262144 // 2^18 32768 2^15
-
-
-/**
- * @brief
- * Logs the data obtained throughout the controller loop.
- *
- * @param log_struct
- * structure of the data to be logged
- *
- * @return
- * error message
- *
- */
- int log_data(log_t* log_struct);
-
- /**
- * Fills up an xbox hueg amount of memory with log data
- */
- void updateLog(log_t log_struct);
-
- /**
- * Prints all the log information.
- */
- void printLogging();
-
- char* format(log_t log);
-
-#endif /* LOG_DATA_H_ */
diff --git a/quad/xsdk_workspace/imu_logger/src/old_log_data.h b/quad/xsdk_workspace/imu_logger/src/old_log_data.h
deleted file mode 100644
index 932eb1a9207f5372fac04658e85cfbe9b7359ddd..0000000000000000000000000000000000000000
--- a/quad/xsdk_workspace/imu_logger/src/old_log_data.h
+++ /dev/null
@@ -1,48 +0,0 @@
-/*
- * log_data.h
- *
- * Created on: Feb 20, 2016
- * Author: ucart
- */
-
-#ifndef LOG_DATA_H_
-#define LOG_DATA_H_
-
-#include <stdio.h>
-#include <stdlib.h>
-#include <string.h>
-#include "PID.h"
-#include "type_def.h"
-#include "uart.h"
-#include "sleep.h"
-#include "communication.h"
-
-#define LOG_STARTING_SIZE 4096 //262144 // 2^18 32768 2^15
-
-
-/**
- * @brief
- * Logs the data obtained throughout the controller loop.
- *
- * @param log_struct
- * structure of the data to be logged
- *
- * @return
- * error message
- *
- */
- int log_data(log_t* log_struct);
-
- /**
- * Fills up an xbox hueg amount of memory with log data
- */
- void updateLog(log_t log_struct);
-
- /**
- * Prints all the log information.
- */
- void printLogging();
-
- char* format(log_t log);
-
-#endif /* LOG_DATA_H_ */
diff --git a/quad/xsdk_workspace/imu_logger/src/packet_processing.c b/quad/xsdk_workspace/imu_logger/src/packet_processing.c
deleted file mode 100644
index 65f44dc53a6a41d7b2e44a62bdafd6ed70a02ab7..0000000000000000000000000000000000000000
--- a/quad/xsdk_workspace/imu_logger/src/packet_processing.c
+++ /dev/null
@@ -1,85 +0,0 @@
-/*
- * process_packet.c
- *
- * Created on: Mar 2, 2016
- * Author: ucart
- */
-#include "packet_processing.h"
-#include "uart.h"
-#include "type_def.h"
-#include "sleep.h"
-#include "util.h"
-#include "communication.h"
-
-#define DEBUG 0
-
-tokenList_t tokenize(char* cmd) {
- int maxTokens = 16;
- tokenList_t ret;
- ret.numTokens = 0;
- ret.tokens = malloc(sizeof(char *)* 20 * maxTokens);
-
- int i = 0;
- ret.tokens[0] = NULL;
- char* token = strtok(cmd, " ");
- while (token != NULL && i < maxTokens - 1) {
- ret.tokens[i] = malloc(strlen(token) + 10);
- strcpy(ret.tokens[i], token);
- ret.tokens[++i] = NULL;
- ret.numTokens++;
- token = strtok(NULL, " ");
- }
-
- return ret;
-}
-
-int processUpdate(unsigned char* update, quadPosition_t* currentQuadPosition) {
- //static char buf[16384];
- //sprintf(buf, "update..(%d)..[%s]\r\n", strlen(update), update);
- //uart0_sendStr(buf);
-
- unsigned char * data;
- metadata_t md;
- parse_packet(update, &data, &md);
-
-
- // Packet must come as [NEARPY], 4 bytes each
- int packetId = getInt(data, 0);
-// printf("Packet ID: %d\n", packetId);
- float y_pos = getFloat(data, 4);
-// printf("y_pos: %f\n", y_pos);
- float x_pos = getFloat(data, 8);
-// printf("x_pos: %f\n", x_pos);
- float alt_pos = getFloat(data, 12);
-// printf("alt_pos: %f\n", alt_pos);
- float roll = getFloat(data, 16);
-// printf("roll: %f\n", roll);
- float pitch = getFloat(data, 20);
-// printf("pitch: %f\n", pitch);
- float yaw = getFloat(data, 24);
-// printf("yaw: %f\n", yaw);
-
- currentQuadPosition->packetId = packetId;
- currentQuadPosition->y_pos = y_pos;
- currentQuadPosition->x_pos = x_pos;
- currentQuadPosition->alt_pos = alt_pos;
- currentQuadPosition->roll = roll;
- currentQuadPosition->pitch = pitch;
- currentQuadPosition->yaw = yaw;
-
- return 0;
-}
-
-float getFloat(unsigned char* str, int pos) {
- union {
- float f;
- int i;
- } x;
- x.i = ((str[pos+3] << 24) | (str[pos+2] << 16) | (str[pos+1] << 8) | (str[pos]));
- return x.f;
-}
-
-int getInt(unsigned char* str, int pos) {
- int i = ((str[pos+3] << 24) | (str[pos+2] << 16) | (str[pos+1] << 8) | (str[pos]));
- return i;
-}
diff --git a/quad/xsdk_workspace/imu_logger/src/packet_processing.h b/quad/xsdk_workspace/imu_logger/src/packet_processing.h
deleted file mode 100644
index 55a5494b3e1535ac46db7abdaa0ee18b2591f212..0000000000000000000000000000000000000000
--- a/quad/xsdk_workspace/imu_logger/src/packet_processing.h
+++ /dev/null
@@ -1,23 +0,0 @@
-/*
- * process_packet.h
- *
- * Created on: Mar 2, 2016
- * Author: ucart
- */
-
-#ifndef PROCESS_PACKET_H_
-#define PROCESS_PACKET_H_
-
-#include <stdlib.h>
-#include <string.h>
-#include <stdio.h>
-#include "type_def.h"
-
-tokenList_t tokenize(char* cmd);
-int processUpdate(unsigned char* update, quadPosition_t* currentQuadPosition);
-//int processCommand(stringBuilder_t * sb, setpoint_t * setpoint_struct, parameter_t * parameter_struct);
-int doProcessing(char* cmd, tokenList_t * tokens, setpoint_t * setpoint_struct, parameter_t * parameter_struct);
-float getFloat(unsigned char* str, int pos);
-int getInt(unsigned char* str, int pos);
-
-#endif /* PROCESS_PACKET_H_ */
diff --git a/quad/xsdk_workspace/imu_logger/src/platform.c b/quad/xsdk_workspace/imu_logger/src/platform.c
deleted file mode 100644
index ea7849798004dd889c3c4bfa76a67c0867e023d3..0000000000000000000000000000000000000000
--- a/quad/xsdk_workspace/imu_logger/src/platform.c
+++ /dev/null
@@ -1,112 +0,0 @@
-/******************************************************************************
-*
-* (c) Copyright 2010-2012 Xilinx, Inc. All rights reserved.
-*
-* This file contains confidential and proprietary information of Xilinx, Inc.
-* and is protected under U.S. and international copyright and other
-* intellectual property laws.
-*
-* DISCLAIMER
-* This disclaimer is not a license and does not grant any rights to the
-* materials distributed herewith. Except as otherwise provided in a valid
-* license issued to you by Xilinx, and to the maximum extent permitted by
-* applicable law: (1) THESE MATERIALS ARE MADE AVAILABLE "AS IS" AND WITH ALL
-* FAULTS, AND XILINX HEREBY DISCLAIMS ALL WARRANTIES AND CONDITIONS, EXPRESS,
-* IMPLIED, OR STATUTORY, INCLUDING BUT NOT LIMITED TO WARRANTIES OF
-* MERCHANTABILITY, NON-INFRINGEMENT, OR FITNESS FOR ANY PARTICULAR PURPOSE;
-* and (2) Xilinx shall not be liable (whether in contract or tort, including
-* negligence, or under any other theory of liability) for any loss or damage
-* of any kind or nature related to, arising under or in connection with these
-* materials, including for any direct, or any indirect, special, incidental,
-* or consequential loss or damage (including loss of data, profits, goodwill,
-* or any type of loss or damage suffered as a result of any action brought by
-* a third party) even if such damage or loss was reasonably foreseeable or
-* Xilinx had been advised of the possibility of the same.
-*
-* CRITICAL APPLICATIONS
-* Xilinx products are not designed or intended to be fail-safe, or for use in
-* any application requiring fail-safe performance, such as life-support or
-* safety devices or systems, Class III medical devices, nuclear facilities,
-* applications related to the deployment of airbags, or any other applications
-* that could lead to death, personal injury, or severe property or
-* environmental damage (individually and collectively, "Critical
-* Applications"). Customer assumes the sole risk and liability of any use of
-* Xilinx products in Critical Applications, subject only to applicable laws
-* and regulations governing limitations on product liability.
-*
-* THIS COPYRIGHT NOTICE AND DISCLAIMER MUST BE RETAINED AS PART OF THIS FILE
-* AT ALL TIMES.
-*
-******************************************************************************/
-
-#include "xparameters.h"
-#include "xil_cache.h"
-
-#include "platform_config.h"
-
-/*
- * Uncomment the following line if ps7 init source files are added in the
- * source directory for compiling example outside of SDK.
- */
-/*#include "ps7_init.h"*/
-
-#ifdef STDOUT_IS_16550
- #include "xuartns550_l.h"
-
- #define UART_BAUD 9600
-#endif
-
-void
-enable_caches()
-{
-#ifdef __PPC__
- Xil_ICacheEnableRegion(CACHEABLE_REGION_MASK);
- Xil_DCacheEnableRegion(CACHEABLE_REGION_MASK);
-#elif __MICROBLAZE__
-#ifdef XPAR_MICROBLAZE_USE_ICACHE
- Xil_ICacheEnable();
-#endif
-#ifdef XPAR_MICROBLAZE_USE_DCACHE
- Xil_DCacheEnable();
-#endif
-#endif
-}
-
-void
-disable_caches()
-{
- Xil_DCacheDisable();
- Xil_ICacheDisable();
-}
-
-void
-init_uart()
-{
-#ifdef STDOUT_IS_16550
- XUartNs550_SetBaud(STDOUT_BASEADDR, XPAR_XUARTNS550_CLOCK_HZ, UART_BAUD);
- XUartNs550_SetLineControlReg(STDOUT_BASEADDR, XUN_LCR_8_DATA_BITS);
-#endif
-#ifdef STDOUT_IS_PS7_UART
- /* Bootrom/BSP configures PS7 UART to 115200 bps */
-#endif
-}
-
-void
-init_platform()
-{
- /*
- * If you want to run this example outside of SDK,
- * uncomment the following line and also #include "ps7_init.h" at the top.
- * Make sure that the ps7_init.c and ps7_init.h files are included
- * along with this example source files for compilation.
- */
- /* ps7_init();*/
- enable_caches();
- init_uart();
-}
-
-void
-cleanup_platform()
-{
- disable_caches();
-}
diff --git a/quad/xsdk_workspace/imu_logger/src/platform.h b/quad/xsdk_workspace/imu_logger/src/platform.h
deleted file mode 100644
index efc90882b0b336c2038d1f8a0a02ffed7d6e7ae3..0000000000000000000000000000000000000000
--- a/quad/xsdk_workspace/imu_logger/src/platform.h
+++ /dev/null
@@ -1,27 +0,0 @@
-/*
- * Copyright (c) 2008 Xilinx, Inc. All rights reserved.
- *
- * Xilinx, Inc.
- * XILINX IS PROVIDING THIS DESIGN, CODE, OR INFORMATION "AS IS" AS A
- * COURTESY TO YOU. BY PROVIDING THIS DESIGN, CODE, OR INFORMATION AS
- * ONE POSSIBLE IMPLEMENTATION OF THIS FEATURE, APPLICATION OR
- * STANDARD, XILINX IS MAKING NO REPRESENTATION THAT THIS IMPLEMENTATION
- * IS FREE FROM ANY CLAIMS OF INFRINGEMENT, AND YOU ARE RESPONSIBLE
- * FOR OBTAINING ANY RIGHTS YOU MAY REQUIRE FOR YOUR IMPLEMENTATION.
- * XILINX EXPRESSLY DISCLAIMS ANY WARRANTY WHATSOEVER WITH RESPECT TO
- * THE ADEQUACY OF THE IMPLEMENTATION, INCLUDING BUT NOT LIMITED TO
- * ANY WARRANTIES OR REPRESENTATIONS THAT THIS IMPLEMENTATION IS FREE
- * FROM CLAIMS OF INFRINGEMENT, IMPLIED WARRANTIES OF MERCHANTABILITY
- * AND FITNESS FOR A PARTICULAR PURPOSE.
- *
- */
-
-#ifndef __PLATFORM_H_
-#define __PLATFORM_H_
-
-#include "platform_config.h"
-
-void init_platform();
-void cleanup_platform();
-
-#endif
diff --git a/quad/xsdk_workspace/imu_logger/src/platform_config.h b/quad/xsdk_workspace/imu_logger/src/platform_config.h
deleted file mode 100644
index c07ab1cba9631113470838bd69dce3964dd386fa..0000000000000000000000000000000000000000
--- a/quad/xsdk_workspace/imu_logger/src/platform_config.h
+++ /dev/null
@@ -1,10 +0,0 @@
-#ifndef __PLATFORM_CONFIG_H_
-#define __PLATFORM_CONFIG_H_
-
-#define STDOUT_IS_PS7_UART
-#define UART_DEVICE_ID 0
-#ifdef __PPC__
-#define CACHEABLE_REGION_MASK 0xf0000001
-#endif
-
-#endif
diff --git a/quad/xsdk_workspace/imu_logger/src/quadposition.h b/quad/xsdk_workspace/imu_logger/src/quadposition.h
deleted file mode 100644
index 05011e04e6289f951700e838b666634b04b4cbc9..0000000000000000000000000000000000000000
--- a/quad/xsdk_workspace/imu_logger/src/quadposition.h
+++ /dev/null
@@ -1,6 +0,0 @@
-#ifndef _QUADPOSITION_H
-#define _QUADPOSITION_H
-
-
-
-#endif /* _QUADPOSITION_H */
diff --git a/quad/xsdk_workspace/imu_logger/src/send_actuator_commands.c b/quad/xsdk_workspace/imu_logger/src/send_actuator_commands.c
deleted file mode 100644
index 7174348923252cf7109fd1d2df8d08bcef673313..0000000000000000000000000000000000000000
--- a/quad/xsdk_workspace/imu_logger/src/send_actuator_commands.c
+++ /dev/null
@@ -1,24 +0,0 @@
-/*
- * send_actuator_commands.c
- *
- * Created on: Feb 20, 2016
- * Author: ucart
- */
-
-#include "send_actuator_commands.h"
-
-int send_actuator_commands(log_t* log_struct, actuator_command_t* actuator_command_struct)
-{
- int i;
- // write the PWMs to the motors
- for (i = 0; i < 4; i++)
- pwm_write_channel(actuator_command_struct->pwms[i], i);
-
- log_struct->motors[0] = actuator_command_struct->pwms[0];
- log_struct->motors[1] = actuator_command_struct->pwms[1];
- log_struct->motors[2] = actuator_command_struct->pwms[2];
- log_struct->motors[3] = actuator_command_struct->pwms[3];
-
- return 0;
-}
-
diff --git a/quad/xsdk_workspace/imu_logger/src/send_actuator_commands.h b/quad/xsdk_workspace/imu_logger/src/send_actuator_commands.h
deleted file mode 100644
index ca337ad7b78546682c57776f3ac40f03d76c9d42..0000000000000000000000000000000000000000
--- a/quad/xsdk_workspace/imu_logger/src/send_actuator_commands.h
+++ /dev/null
@@ -1,32 +0,0 @@
-/*
- * send_actuator_commands.h
- *
- * Created on: Feb 20, 2016
- * Author: ucart
- */
-
-#ifndef SEND_ACTUATOR_COMMANDS_H_
-#define SEND_ACTUATOR_COMMANDS_H_
-
-#include <stdio.h>
-
-#include "log_data.h"
-#include "actuator_command_processing.h"
-
-/**
- * @brief
- * Sends commands to the actuators.
- *
- * @param log_struct
- * structure of the data to be logged
- *
- * @param actuator_command_struct
- * structure of the commmands to go to the actuators
- *
- * @return
- * error message
- *
- */
-int send_actuator_commands(log_t* log_struct, actuator_command_t* actuator_command_struct);
-
-#endif /* SEND_ACTUATOR_COMMANDS_H_ */
diff --git a/quad/xsdk_workspace/imu_logger/src/sensor.c b/quad/xsdk_workspace/imu_logger/src/sensor.c
deleted file mode 100644
index 26f1eba14bb64d21f2ae69b63379ce877ae067e8..0000000000000000000000000000000000000000
--- a/quad/xsdk_workspace/imu_logger/src/sensor.c
+++ /dev/null
@@ -1,74 +0,0 @@
-/*
- * sensor.c
- *
- * Created on: Feb 20, 2016
- * Author: ucart
- */
-
-#include "sensor.h"
-#include "communication.h"
-#include "commands.h"
-
-int sensor_init(raw_sensor_t * raw_sensor_struct, sensor_t * sensor_struct)
-{
- if(startMPU9150() == -1)
- return -1;
-
- // read sensor board and fill in gryo/accelerometer/magnetometer struct
- get_gam_reading(&(raw_sensor_struct->gam));
-
- // Sets the first iteration to be at the accelerometer value since gyro initializes to {0,0,0} regardless of orientation
- sensor_struct->pitch_angle_filtered = raw_sensor_struct->gam.accel_roll;
- sensor_struct->roll_angle_filtered = raw_sensor_struct->gam.accel_pitch;
-
- return 0;
-}
-
-int get_sensors(log_t* log_struct, user_input_t* user_input_struct, raw_sensor_t* raw_sensor_struct)
-{
-
-// ///////// for testing update period of vrpn data from the ground station to the quad
-// static int update_counter = 0;
-// if(user_input_struct->hasPacket == 0x04)
-// {
-// char buf[200] = {};
-// // Send a reply to the ground station
-// snprintf(buf, sizeof(buf), "Received an update packet %dms\r\n", 5 * update_counter);
-// 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
-// int i;
-// 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]);
-// }
-// update_counter = 0;
-// }
-// else
-// {
-// update_counter++;
-// }
-//
-// /////////// end testing
-
-
- // the the sensor board and fill in the readings into the GAM struct
- get_gam_reading(&(raw_sensor_struct->gam));
-
- //log_struct->gam = raw_sensor_struct->gam;
-
- return 0;
-}
-
diff --git a/quad/xsdk_workspace/imu_logger/src/sensor.h b/quad/xsdk_workspace/imu_logger/src/sensor.h
deleted file mode 100644
index 6561402a1e766dfdd7c11e0f4dcab21f25513fa7..0000000000000000000000000000000000000000
--- a/quad/xsdk_workspace/imu_logger/src/sensor.h
+++ /dev/null
@@ -1,48 +0,0 @@
-/*
- * sensor.h
- *
- * Created on: Feb 20, 2016
- * Author: ucart
- */
-
-#ifndef SENSOR_H_
-#define SENSOR_H_
-
-#include <stdio.h>
-
-#include "log_data.h"
-#include "user_input.h"
-#include "iic_mpu9150_utils.h"
-#include "packet_processing.h"
-#include "uart.h"
-
-/**
- * @brief
- * Initializes the sensors.
- *
- * @return
- * error message
- *
- */
-int sensor_init(raw_sensor_t * raw_sensor_struct, sensor_t * sensor_struct);
-
-/**
- * @brief
- * Recieves data from the sensors.
- *
- * @param log_struct
- * structure of the data to be logged
- *
- * @param user_input_struct
- * structure of the input from the user
- *
- * @param raw_sensor_struct
- * structure of the raw values from the sensors
- *
- * @return
- * error message
- *
- */
-int get_sensors(log_t* log_struct, user_input_t* user_input_struct, raw_sensor_t* raw_sensor_struct);
-
-#endif /* SENSOR_TEMPLATE_H_ */
diff --git a/quad/xsdk_workspace/imu_logger/src/sensor_processing.c b/quad/xsdk_workspace/imu_logger/src/sensor_processing.c
deleted file mode 100644
index 5fd682c3fae6a0bf0039932e24a20dc6eff27120..0000000000000000000000000000000000000000
--- a/quad/xsdk_workspace/imu_logger/src/sensor_processing.c
+++ /dev/null
@@ -1,124 +0,0 @@
-/*
- * sensor_processing.c
- *
- * Created on: Feb 20, 2016
- * Author: ucart
- */
-
-#include "sensor_processing.h"
-#include "timer.h"
-#include <math.h>
-
-int sensor_processing(log_t* log_struct, user_input_t *user_input_struct, raw_sensor_t* raw_sensor_struct, sensor_t* sensor_struct)
-{
- // copy currentQuadPosition and trimmedRCValues from raw_sensor_struct to sensor_struct
- deep_copy_Qpos(&(sensor_struct->currentQuadPosition), &(raw_sensor_struct->currentQuadPosition));
-
- // Psuedo-Nonlinear Extension for determining local x/y position based on yaw angle
- // provided by Matt Rich
- //
- // local x/y/z is the moving frame of reference on the quad that we are transforming so we can assume yaw angle is 0 (well enough)
- // for the autonomous position controllers
- //
- // camera given x/y/z is the inertia frame of reference (the global coordinates)
- //
- // |local x| |cos(yaw angle) -sin(yaw angle) 0| |camera given x|
- // |local y| = |sin(yaw angle) cos(yaw angle) 0| |camera given y|
- // |local z| | 0 0 1| |camera given z|
-
- sensor_struct->currentQuadPosition.x_pos =
- sensor_struct->currentQuadPosition.x_pos * cos(sensor_struct->currentQuadPosition.yaw) +
- sensor_struct->currentQuadPosition.y_pos * -sin(sensor_struct->currentQuadPosition.yaw);
-
- sensor_struct->currentQuadPosition.y_pos =
- sensor_struct->currentQuadPosition.x_pos * sin(sensor_struct->currentQuadPosition.yaw) +
- sensor_struct->currentQuadPosition.y_pos * cos(sensor_struct->currentQuadPosition.yaw);
-
-
- // Calculate Euler angles and velocities using Gimbal Equations below
- /////////////////////////////////////////////////////////////////////////
- // | Phi_d | | 1 sin(Phi)tan(theta) cos(Phi)tan(theta) | | p |
- // | theta_d | = | 0 cos(Phi) -sin(Phi) | | q |
- // | Psi_d | | 0 sin(Phi)sec(theta) cos(Phi)sec(theat) | | r |
- //
- // Phi_dot = p + q sin(Phi) tan(theta) + r cos(Phi) tan(theta)
- // theta_dot = q cos(Phi) - r sin(Phi)
- // Psi_dot = q sin(Phi) sec(theta) + r cos(Phi) sec(theta)
- ///////////////////////////////////////////////////////////////////////////
-
- // javey:
- //
- // The gimbal equations are defined in the book "Flight Simulation" by Rolfe and Staples.
- // Find on page 46, equation 3.6
-
- // these are calculated to be used in the gimbal equations below
- // the variable roll(pitch)_angle_filtered is phi(theta)
- double sin_phi = sin(sensor_struct->roll_angle_filtered);
- double cos_phi = cos(sensor_struct->roll_angle_filtered);
- double tan_theta = tan(sensor_struct->pitch_angle_filtered);
- double sec_theta = 1/cos(sensor_struct->pitch_angle_filtered);
-
-// Gryo "p" is the angular velocity rotation about the x-axis (defined as var gyro_xVel_p in gam struct)
-// Gyro "q" is the angular velocity rotation about the y-axis (defined as var gyro_xVel_q in gam struct)
-// Gyro "r" is the angular velocity rotation about the z-axis (defined as var gyro_xVel_r in gam struct)
-
- // phi is the conventional symbol used for roll angle, so phi_dot is the roll velocity
- sensor_struct->phi_dot = raw_sensor_struct->gam.gyro_xVel_p + (raw_sensor_struct->gam.gyro_yVel_q*sin_phi*tan_theta)
- + (raw_sensor_struct->gam.gyro_zVel_r*cos_phi*tan_theta);
-
- // theta is the conventional symbol used for pitch angle, so theta_dot is the pitch velocity
- sensor_struct->theta_dot = (raw_sensor_struct->gam.gyro_yVel_q*cos_phi)
- - (raw_sensor_struct->gam.gyro_zVel_r*sin_phi);
-
- // psi is the conventional symbol used for yaw angle, so psi_dot is the yaw velocity
- sensor_struct->psi_dot = (raw_sensor_struct->gam.gyro_yVel_q*sin_phi*sec_theta)
- + (raw_sensor_struct->gam.gyro_zVel_r*cos_phi*sec_theta);
-
- // Complementary Filter Calculations
- sensor_struct->pitch_angle_filtered = 0.98 * (sensor_struct->pitch_angle_filtered + sensor_struct->theta_dot * LOOP_TIME)
- + 0.02 * raw_sensor_struct->gam.accel_pitch;
-
- sensor_struct->roll_angle_filtered = 0.98 * (sensor_struct->roll_angle_filtered + sensor_struct->phi_dot* LOOP_TIME)
- + 0.02 * raw_sensor_struct->gam.accel_roll;
-
-// static int loop_counter = 0;
-// loop_counter++;
-//
-// if(loop_counter == 50)
-// {
-// char dMsg[100] = {};
-// sprintf(dMsg, "Loop time: %.4f\n", LOOP_TIME);
-// uart0_sendStr(dMsg);
-// loop_counter = 0;
-// }
-
- //logging
- log_struct->currentQuadPosition = sensor_struct->currentQuadPosition;
- log_struct->roll_angle_filtered = sensor_struct->roll_angle_filtered;
- log_struct->pitch_angle_filtered = sensor_struct->pitch_angle_filtered;
- log_struct->phi_dot = sensor_struct->phi_dot;
- log_struct->theta_dot = sensor_struct->theta_dot;
- log_struct->psi_dot = sensor_struct->psi_dot;
- return 0;
-}
-
-void set_pitch_angle_filtered(sensor_t * sensor_struct, float accel_roll)
-{
- sensor_struct->pitch_angle_filtered = accel_roll;
-}
-void set_roll_angle_filtered(sensor_t * sensor_struct, float accel_pitch)
-{
- sensor_struct->roll_angle_filtered = accel_pitch;
-}
-
-void deep_copy_Qpos(quadPosition_t * dest, quadPosition_t * src)
-{
- dest->packetId = src->packetId;
- dest->y_pos = src->y_pos;
- dest->x_pos = src->x_pos;
- dest->alt_pos = src->alt_pos;
- dest->roll = src->roll;
- dest->pitch = src->pitch;
- dest->yaw = src->yaw;
-
-}
diff --git a/quad/xsdk_workspace/imu_logger/src/sensor_processing.h b/quad/xsdk_workspace/imu_logger/src/sensor_processing.h
deleted file mode 100644
index cf3fd122b33168782cccbb7dd1edc012ca3c2107..0000000000000000000000000000000000000000
--- a/quad/xsdk_workspace/imu_logger/src/sensor_processing.h
+++ /dev/null
@@ -1,89 +0,0 @@
-/*
- * sensor_processing.h
- *
- * Created on: Feb 20, 2016
- * Author: ucart
- */
-
-#ifndef SENSOR_PROCESSING_H_
-#define SENSOR_PROCESSING_H_
-
-#include <stdio.h>
-#include "log_data.h"
-#include "sensor.h"
-#include "conversion.h"
-#include "quadposition.h"
-/*
- * Aero channel declaration
- */
-
-#define THROTTLE 0
-#define ROLL 1
-#define PITCH 2
-#define YAW 3
-#define GEAR 4
-#define FLAP 5
-
-/**
- * Signals from the Rx mins, maxes and ranges
- */
-//#define THROTTLE_MAX 191900
-//#define THROTTLE_MIN 110200
-//#define THROTTLE_RANGE THROTTLE_MAX - THROTTLE_MIN
-//
-//#define ROLL_MAX 170200
-////#define ROLL_MAX 167664
-////#define ROLL_CENTER 148532
-//#define ROLL_MIN 129400
-//#define ROLL_CENTER 149800
-//#define ROLL_RANGE ROLL_MAX - ROLL_MIN
-//
-////#define PITCH_MAX 190800
-//#define PITCH_MAX 169900
-////#define PITCH_MIN 135760
-//#define PITCH_MIN 129500
-////#define PITCH_CENTER 152830
-//#define PITCH_CENTER 149700
-//#define PITCH_RANGE PITCH_MAX - PITCH_MIN
-//
-//#define YAW_MAX 169400
-//#define YAW_MIN 129300
-//#define YAW_CENTER 149800
-//#define YAW_RANGE YAW_MAX - YAW_MIN
-//
-//#define GEAR_1 170800
-//#define GEAR_0 118300
-//
-//#define FLAP_1 192000
-//#define FLAP_0 107600
-//
-//#define GEAR_KILL GEAR_0 // The kill point for the program
-//#define BASE 150000
-//
-//#define min 100000
-//#define max 200000
-
-/**
- * @brief
- * Processes the data from the sensors.
- *
- * @param log_struct
- * structure of the data to be logged
- *
- * @param raw_sensor_struct
- * structure of the raw data from the sensors
- *
- * @param sensor_struct
- * structure of the processed data from the sensors
- *
- * @return
- * error message
- *
- */
-
-int sensor_processing(log_t* log_struct, user_input_t *user_input_struct, raw_sensor_t * raw_sensor_struct, sensor_t* sensor_struct);
-void deep_copy_Qpos(quadPosition_t * dest, quadPosition_t * src);
-void set_pitch_angle_filtered(sensor_t * sensor_struct, float accel_roll);
-void set_roll_angle_filtered(sensor_t * sensor_struct, float accel_roll);
-
-#endif /* SENSOR_PROCESSING_H_ */
diff --git a/quad/xsdk_workspace/imu_logger/src/stringBuilder.c b/quad/xsdk_workspace/imu_logger/src/stringBuilder.c
deleted file mode 100644
index 1f24039cd4eec6a95497128504e034910df5d687..0000000000000000000000000000000000000000
--- a/quad/xsdk_workspace/imu_logger/src/stringBuilder.c
+++ /dev/null
@@ -1,198 +0,0 @@
-/*
- * stringBuilder.c
- *
- * Created on: Sep 24, 2014
- * Author: ucart
- */
-
-#include <stdlib.h>
-#include <string.h>
-#include "stringBuilder.h"
-
-int stringBuilder_maybeExpand(stringBuilder_t* sb) {
- if (!sb || !sb->buf) {
- return STRINGBUILDER_ILLEGALARGUMENT;
- }
-
- // See if we need to expand
- if (sb->length + 1 >= sb->capacity) {
- if (sb->capacity >= sb->maxCapacity) {
- // Would exceed maxCapacity, can't do anything
- return STRINGBUILDER_AT_MAX_CAPACITY;
- }
-
- // Compute new size (try doubling)
- int newCapacity = sb->capacity * 2;
- if (newCapacity <= 2) {
- newCapacity = 2;
- }
- if (newCapacity >= sb->maxCapacity) {
- newCapacity = sb->maxCapacity;
- }
-
- // Get a pointer to the old buf mem
- char* oldBuf = sb->buf;
- sb->buf = malloc(newCapacity);
- if (!sb->buf) {
- // Agh, no mem for buf. Restore the old one and return error
- sb->buf = oldBuf;
- return STRINGBUILDER_NO_MEM_FOR_EXPANSION;
- } else {
- // Got mem for new buf, copy from old buf
- strncpy(sb->buf, oldBuf, sb->length + 1);
- sb->capacity = newCapacity;
- free(oldBuf);
- }
- }
-
- return STRINGBUILDER_SUCCESS;
-}
-
-stringBuilder_t* stringBuilder_createWithMaxCapacity(int initialCapacity,
- int maxCapacity) {
- // Invalid arguments
- if (initialCapacity > maxCapacity || initialCapacity < 1
- || maxCapacity < 1) {
- return NULL ;
- }
-
- stringBuilder_t* sb = (stringBuilder_t*) malloc(sizeof(stringBuilder_t));
- if (!sb) {
- // No mem for buffer
- return NULL ;
- }
-
- // Try to allocate mem for buf
- sb->buf = malloc(initialCapacity);
- if (!sb->buf) {
- // No mem for buf
- free(sb);
- return NULL ;
- }
-
- // Set up function pointers
- sb->addStr = stringBuilder_addStr;
- sb->addStrAt = stringBuilder_addStrAt;
- sb->addChar = stringBuilder_addChar;
- sb->addCharAt = stringBuilder_addCharAt;
- sb->removeCharAt = stringBuilder_removeCharAt;
- sb->clear = stringBuilder_clear;
-
-
- // Good to go
- sb->length = 0;
- sb->capacity = initialCapacity;
- sb->maxCapacity = maxCapacity;
- sb->buf[0] = '\0';
- return sb;
-}
-
-stringBuilder_t* stringBuilder_createWithInitialCapacity(int initialCapacity) {
- return stringBuilder_createWithMaxCapacity(initialCapacity,
- STRINGBUILDER_DEFAULT_MAX_CAPACITY);
-}
-
-stringBuilder_t* stringBuilder_create() {
- return stringBuilder_createWithMaxCapacity(
- STRINGBUILDER_DEFAULT_INITIAL_CAPACITY,
- STRINGBUILDER_DEFAULT_MAX_CAPACITY);
-}
-
-void stringBuilder_free(stringBuilder_t* sb) {
- if (sb && sb->buf) {
- free(sb->buf);
- }
- if (sb) {
- free(sb);
- }
-}
-
-int stringBuilder_addStr(stringBuilder_t* sb, char* str) {
- if (!sb || !sb->buf) {
- return STRINGBUILDER_ILLEGALARGUMENT;
- }
-
- while (*str) {
- int status = sb->addChar(sb, *str);
- if (status != STRINGBUILDER_SUCCESS) {
- return status;
- }
- str++;
- }
-
- return STRINGBUILDER_SUCCESS;
-}
-
-int stringBuilder_addStrAt(stringBuilder_t* sb, char* str, int index) {
- if (!sb || !sb->buf) {
- return STRINGBUILDER_ILLEGALARGUMENT;
- }
-
- while (*str) {
- int status = sb->addCharAt(sb, *str, index);
- if (status != STRINGBUILDER_SUCCESS) {
- return status;
- }
-
- str++;
- index++;
- }
-
- return STRINGBUILDER_SUCCESS;
-}
-
-/** Add a character to end of the StringBuilder */
-int stringBuilder_addChar(stringBuilder_t* sb, char c) {
- if (!sb || !sb->buf) {
- return STRINGBUILDER_ILLEGALARGUMENT;
- }
-
- return stringBuilder_addCharAt(sb, c, sb->length);
-}
-
-/** Add a character to the StringBuilder at the specified index, if possible */
-int stringBuilder_addCharAt(stringBuilder_t* sb, char c, int index) {
- if (!sb || !sb->buf || index < 0 || index > sb->length) {
- return STRINGBUILDER_ILLEGALARGUMENT;
- }
-
- // Try expanding if necessary
- int status = stringBuilder_maybeExpand(sb);
- if (status != STRINGBUILDER_SUCCESS) {
- return status;
- }
-
- // Move everything right of index over by 1
- int i;
- for (i = sb->length; i >= index; i--) {
- sb->buf[i + 1] = sb->buf[i];
- }
-
- // Insert the character and add the null terminator
- sb->buf[index] = c;
- sb->length += 1;
-
- return STRINGBUILDER_SUCCESS;
-}
-
-/** Remove a character from the StringBuilder at the specified index */
-int stringBuilder_removeCharAt(stringBuilder_t* sb, int index) {
- if (!sb || !sb->buf || index < 0 || index >= sb->length) {
- return STRINGBUILDER_ILLEGALARGUMENT;
- }
-
- // Move everything right of index over left
- int i;
- for (i = index; i < sb->length; i++) {
- sb->buf[i] = sb->buf[i + 1];
- }
- sb->length -= 1;
-
- return STRINGBUILDER_SUCCESS;
-}
-
-void stringBuilder_clear(stringBuilder_t* sb) {
- sb->length = 0;
- sb->buf[0] = '\0';
-}
-
diff --git a/quad/xsdk_workspace/imu_logger/src/stringBuilder.h b/quad/xsdk_workspace/imu_logger/src/stringBuilder.h
deleted file mode 100644
index efcfe6276727b1322a9479175437bd2e45eccfd0..0000000000000000000000000000000000000000
--- a/quad/xsdk_workspace/imu_logger/src/stringBuilder.h
+++ /dev/null
@@ -1,59 +0,0 @@
-/*
- * stringBuffer.h
- *
- * Created on: Sep 24, 2014
- * Author: ucart
- */
-
-#ifndef STRINGBUILDER_H_
-#define STRINGBUILDER_H_
-
-#include "type_def.h"
-
-#define STRINGBUILDER_DEFAULT_INITIAL_CAPACITY 16
-#define STRINGBUILDER_DEFAULT_MAX_CAPACITY 1024
-
-enum {
- STRINGBUILDER_SUCCESS = 0,
- STRINGBUILDER_AT_MAX_CAPACITY = 1,
- STRINGBUILDER_NO_MEM_FOR_EXPANSION = 2,
- STRINGBUILDER_ILLEGALARGUMENT = 3
-};
-
-/***** Constructors *****/
-
-/** Create a StringBuilder with a max/initial capacity specified */
-stringBuilder_t* stringBuilder_createWithMaxCapacity(int, int);
-
-/** Create a StringBuilder with an initial capacity specified */
-stringBuilder_t* stringBuilder_createWithInitialCapacity(int);
-
-/** Create a StringBuilder with default initial capacity/max capacity */
-stringBuilder_t* stringBuilder_create();
-
-
-/** Free a StringBuilder */
-void stringBuilder_free(stringBuilder_t*);
-
-
-/***** Methods *****/
-
-/** Add a string to the end of the StringBuilder */
-int stringBuilder_addStr(stringBuilder_t*, char*);
-
-/** Add a string to the StringBuilder at the specified index */
-int stringBuilder_addStrAt(stringBuilder_t*, char*, int);
-
-/** Add a character to end of the StringBuilder */
-int stringBuilder_addChar(stringBuilder_t*, char);
-
-/** Add a character to the StringBuilder at the specified index */
-int stringBuilder_addCharAt(stringBuilder_t*, char, int);
-
-/** Remove a character from the StringBuilder at the specified index */
-int stringBuilder_removeCharAt(stringBuilder_t*, int);
-
-/** Clear a stringBuilder */
-void stringBuilder_clear(stringBuilder_t*);
-
-#endif /* STRINGBUILDER_H_ */
diff --git a/quad/xsdk_workspace/imu_logger/src/timer.c b/quad/xsdk_workspace/imu_logger/src/timer.c
deleted file mode 100644
index 6e18b825ff1e6dc0519b3381ae16644c43872683..0000000000000000000000000000000000000000
--- a/quad/xsdk_workspace/imu_logger/src/timer.c
+++ /dev/null
@@ -1,62 +0,0 @@
-/*
- * timer.c
- *
- * Created on: Feb 24, 2016
- * Author: ucart
- */
-
-#include "timer.h"
-
-XTime before = 0, after = 0;
-XTmrCtr axi_timer;
-double LOOP_TIME;
-double time_stamp = 0;
-
-int timer_init() {
-
- // using a axi_timer core because we've had problems with the Global Timer
- XTmrCtr_Initialize(&axi_timer, XPAR_AXI_TIMER_0_DEVICE_ID);
-
- return 0;
-}
-
-int timer_start_loop() {
- //timing code
- LOOP_TIME = ((double) (after - before)) / ((double) COUNTS_PER_SECOND);
- XTime_GetTime(&before);
- XTmrCtr_Reset(&axi_timer, 0);
- XTmrCtr_Start(&axi_timer, 0);
-
- return 0;
-}
-
-double timer_end_loop(log_t *log_struct) {
- // get number of useconds its taken to run the loop thus far
- int usec_loop = XTmrCtr_GetValue(&axi_timer, 0) / (PL_CLK_CNTS_PER_USEC);
-
- // attempt to make each loop run for the same amount of time
- while(usec_loop < DESIRED_USEC_PER_LOOP)
- {
- usec_loop = XTmrCtr_GetValue(&axi_timer, 0) / (PL_CLK_CNTS_PER_USEC);
- }
-
-// static int counter = 0;
-// counter++;
-// if(counter % 50 == 0)
-// printf("usec_loop: %d\n", usec_loop);
-
- //timing code
- XTime_GetTime(&after);
- time_stamp += LOOP_TIME;
- XTmrCtr_Stop(&axi_timer, 0);
-
- // for timing debugging, its a separate hardware PL timer not associated with the PS
- // used this to compare to the PS clock to make sure the timing matched
-// float axi_timer_val = ((float)XTmrCtr_GetValue(&axi_timer, 0)) / ((float)100000000);
-
- // Log the timing information
- //log_struct->time_stamp = time_stamp;
- //log_struct->time_slice = LOOP_TIME;
-
- return time_stamp;
-}
diff --git a/quad/xsdk_workspace/imu_logger/src/timer.h b/quad/xsdk_workspace/imu_logger/src/timer.h
deleted file mode 100644
index 96b40fff6fc3e72abcb93a2bb7d6255929055479..0000000000000000000000000000000000000000
--- a/quad/xsdk_workspace/imu_logger/src/timer.h
+++ /dev/null
@@ -1,56 +0,0 @@
-/*
- * timer.h
- *
- * Created on: Feb 24, 2016
- * Author: ucart
- */
-
-#ifndef TIMER_H_
-#define TIMER_H_
-
-#include "log_data.h"
-#include "xtime_l.h"
-#include <xtmrctr.h>
-
-extern XTime before;
-extern XTime after;
-extern XTmrCtr axi_timer;
-extern double LOOP_TIME;
-extern double time_stamp;
-
-// desired loop time is not guaranteed (its possible that the loop may take longer than desired)
-#define DESIRED_USEC_PER_LOOP 5000 // gives 5ms loops
-
-#define PL_CLK_CNTS_PER_USEC 100
-
-/**
- * @brief
- * Initializes the items necessary for loop timing.
- *
- * @return
- * error message
- *
- */
-int timer_init();
-
-/**
- * @brief
- * Does processing of the loop timer at the beginning of the control loop.
- *
- * @return
- * error message
- *
- */
-int timer_start_loop();
-
-/**
- * @brief
- * Does processing of the loop timer at the end of the control loop.
- *
- * @return
- * error message
- *
- */
-double timer_end_loop(log_t *log_struct);
-
-#endif /* TIMER_H_ */
diff --git a/quad/xsdk_workspace/imu_logger/src/type_def.h b/quad/xsdk_workspace/imu_logger/src/type_def.h
deleted file mode 100644
index 55dca9e8fb27c26397b00961b7dbec8bbe0d4136..0000000000000000000000000000000000000000
--- a/quad/xsdk_workspace/imu_logger/src/type_def.h
+++ /dev/null
@@ -1,361 +0,0 @@
-/*
- * struct_def.h
- *
- * Created on: Mar 2, 2016
- * Author: ucart
- */
-
-#ifndef TYPE_DEF_H_
-#define TYPE_DEF_H_
-
-/**
- * @brief
- * The modes for autonomous and manual flight.
- *
- */
-enum flight_mode{
- AUTO_FLIGHT_MODE,
- MANUAL_FLIGHT_MODE
-};
-
-//----------------------------------------------------------------------------------------------
-// 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 |
-//----------------------------------------------------------------------------------------------
-typedef struct {
- char begin_char;
- char msg_type;
- char msg_subtype;
- int msg_id;
- int data_len;
-} metadata_t;
-
-
-// String builder data type
-typedef struct stringBuilder_s {
- char* buf;
- int length;
- int capacity;
- int maxCapacity;
-
- // Methods
- int (*addStr)(struct stringBuilder_s*, char*);
- int (*addStrAt)(struct stringBuilder_s*, char*, int);
- int (*addChar)(struct stringBuilder_s*, char);
- int (*addCharAt)(struct stringBuilder_s*, char, int);
- int (*removeCharAt)(struct stringBuilder_s*, int);
- void (*clear)(struct stringBuilder_s*);
-} stringBuilder_t;
-
-typedef struct {
- char** tokens;
- int numTokens;
-} tokenList_t;
-
-typedef struct commands{
- int pitch, roll, yaw, throttle;
-}commands;
-
-typedef struct raw{
- int x,y,z;
-}raw;
-typedef struct PID_Consts{
- float P, I, D;
-}PID_Consts;
-
-//Camera system info
-typedef struct {
- int packetId;
-
- double y_pos;
- double x_pos;
- double alt_pos;
-
- double yaw;
- double roll;
- double pitch;
-} quadPosition_t;
-
-typedef struct {
- float yaw;
- float roll;
- float pitch;
- float throttle;
-} quadTrims_t;
-
-//Gyro, accelerometer, and magnetometer data structure
-//Used for reading an instance of the sensor data
-typedef struct {
-
- // GYRO
- //Xint16 raw_gyro_x, raw_gyro_y, raw_gyro_z;
-
- float gyro_xVel_p; // In degrees per second
- float gyro_yVel_q;
- float gyro_zVel_r;
-
- // ACCELEROMETER
- //Xint16 raw_accel_x, raw_accel_y, raw_accel_z;
-
- float accel_x; //In g
- float accel_y;
- float accel_z;
-
- float accel_roll;
- float accel_pitch;
-
-
- // MAG
- //Xint16 raw_mag_x, raw_mag_y, raw_mag_z;
-
- float heading; // In degrees
-
- float mag_x; //Magnetic north: ~50 uT
- float mag_y;
- float mag_z;
-
-
-
-}gam_t;
-
-typedef struct PID_t {
- double current_point; // Current value of the system
- double setpoint; // Desired value of the system
- float Kp; // Proportional constant
- float Ki; // Integral constant
- float Kd; // Derivative constant
- double prev_error; // Previous error
- double acc_error; // Accumulated error
- double pid_correction; // Correction factor computed by the PID
- float dt; // sample period
-} PID_t;
-
-typedef struct PID_values{
- float P; // The P component contribution to the correction output
- float I; // The I component contribution to the correction output
- float D; // The D component contribution to the correction output
- float error; // the error of this PID calculation
- float change_in_error; // error change from the previous calc. to this one
- float pid_correction; // the correction output (P + I + D)
-} PID_values;
-
-///////// MAIN MODULAR STRUCTS
-/**
- * @brief
- * Holds the data inputed by the user
- *
- */
-typedef struct user_input_t {
- int rc_commands[6]; // Commands from the RC transmitter
-
-
-// float cam_x_pos; // Current x position from the camera system
-// float cam_y_pos; // Current y position from the camera system
-// float cam_z_pos; // Current z position from the camera system
-// float cam_roll; // Current roll angle from the camera system
-// float cam_pitch; // Current pitch angle from the camera system
-// float cam_yaw; // Current yaw angle from the camera system
-
- float yaw_manual_setpoint;
- float roll_angle_manual_setpoint;
- float pitch_angle_manual_setpoint;
-
- int hasPacket;
- stringBuilder_t * sb;
-} user_input_t;
-
-/**
- * @brief
- * Holds the log data to be sent to the ground station. It may hold the
- * timestamp of when a sensor's data was obtained.
- *
- */
-typedef struct log_t {
- // Time
- float time_stamp;
- float time_slice;
-
- // Id
- int packetId;
-
- gam_t gam; // Raw and calculated gyro, accel, and mag values are all in gam_t
- float phi_dot, theta_dot, psi_dot; // gimbal equation values
-
- quadPosition_t currentQuadPosition;
-
- float roll_angle_filtered, pitch_angle_filtered;
- float lidar_altitude;
-
- float pid_P_component, pid_I_component, pid_D_component; // use these generically for any PID that you are testing
-
- // PID coefficients and errors
- PID_t local_x_PID, local_y_PID, altitude_PID;
- PID_t angle_yaw_PID, angle_roll_PID, angle_pitch_PID;
- PID_t ang_vel_yaw_PID, ang_vel_roll_PID, ang_vel_pitch_PID;
-
- PID_values local_x_PID_values, local_y_PID_values, altitude_PID_values;
- PID_values angle_yaw_PID_values, angle_roll_PID_values, angle_pitch_PID_values;
- PID_values ang_vel_yaw_PID_values, ang_vel_roll_PID_values, ang_vel_pitch_PID_values;
-
- // RC commands
- commands commands;
-
- //trimmed values
- quadTrims_t trims;
-
- int motors[4];
-
-} log_t;
-
-/**
- * @brief
- * Holds the raw data from the sensors and the timestamp if available
- *
- */
-typedef struct raw_sensor {
- int acc_x; // accelerometer x data
- int acc_x_t; // time of accelerometer x data
-
- int acc_y; // accelerometer y data
- int acc_y_t; // time of accelerometer y data
-
- int acc_z; // accelerometer z data
- int acc_z_t; // time of accelerometer z data
-
-
- int gyr_x; // gyroscope x data
- int gyr_x_t; // time of gyroscope x data
-
- int gyr_y; // gyroscope y data
- int gyr_y_t; // time of gyroscope y data
-
- int gyr_z; // gyroscope z data
- int gyr_z_t; // time of gyroscope z data
-
- int ldr_z; //lidar z data (altitude)
- int ldr_z_t; //time of lidar z data
-
- gam_t gam;
-
- // Structures to hold the current quad position & orientation
- quadPosition_t currentQuadPosition;
-
-} raw_sensor_t;
-
-/**
- * @brief
- * Holds the processed data from the sensors and the timestamp if available
- *
- */
-typedef struct sensor {
- int acc_x; // accelerometer x data
- int acc_x_t; // time of accelerometer x data
-
- int acc_y; // accelerometer y data
- int acc_y_t; // time of accelerometer y data
-
- int acc_z; // accelerometer z data
- int acc_z_t; // time of accelerometer z data
-
-
- int gyr_x; // gyroscope x data
- int gyr_x_t; // time of gyroscope x data
-
- int gyr_y; // gyroscope y data
- int gyr_y_t; // time of gyroscope y data
-
- int gyr_z; // gyroscope z data
- int gyr_z_t; // time of gyroscope z data
-
- int ldr_z; //lidar z data (altitude)
- int ldr_z_t; //time of lidar z data
-
- float pitch_angle_filtered;
- float roll_angle_filtered;
- float lidar_altitude;
-
- float phi_dot, theta_dot, psi_dot;
-
- // Structures to hold the current quad position & orientation
- quadPosition_t currentQuadPosition;
- quadTrims_t trims;
-
-} sensor_t;
-
-/**
- * @brief
- * Holds the setpoints to be used in the controller
- *
- */
-typedef struct setpoint_t {
- quadPosition_t desiredQuadPosition;
-} setpoint_t;
-
-/**
- * @brief
- * Holds the parameters that are specific to whatever type of
- * control algorithm is being used
- *
- */
-typedef struct parameter_t {
- PID_t roll_angle_pid, roll_ang_vel_pid;
- PID_t pitch_angle_pid, pitch_ang_vel_pid;
- PID_t yaw_ang_vel_pid;
- PID_t local_x_pid;
- PID_t local_y_pid;
- PID_t yaw_angle_pid;
- PID_t alt_pid;
-} parameter_t;
-
-/**
- * @brief
- * Holds user defined data for the controller
- *
- */
-typedef struct user_defined_t {
- int flight_mode;
- int engaging_auto;
-} user_defined_t;
-
-/**
- * @brief
- * Holds the raw actuator values before processing
- *
- */
-typedef struct raw_actuator_t {
-
- int controller_corrected_motor_commands[6];
-
-} raw_actuator_t;
-
-/**
- * @brief
- * Holds processed commands to go to the actuators
- *
- */
-typedef struct actuator_command_t {
- int pwms[4];
-} actuator_command_t;
-
-/**
- * @brief
- * Structures to be used throughout
- */
-typedef struct {
- user_input_t user_input_struct;
- log_t log_struct;
- raw_sensor_t raw_sensor_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;
- actuator_command_t actuator_command_struct;
-}modular_structs_t;
-
-//////// END MAIN MODULAR STRUCTS
-
-#endif /* TYPE_DEF_H_ */
diff --git a/quad/xsdk_workspace/imu_logger/src/uart.c b/quad/xsdk_workspace/imu_logger/src/uart.c
deleted file mode 100644
index c45aa1c97abb89c2f8570f287d9a4aeb57bf9cd3..0000000000000000000000000000000000000000
--- a/quad/xsdk_workspace/imu_logger/src/uart.c
+++ /dev/null
@@ -1,413 +0,0 @@
-/*
- * uart.c
- *
- * Created on: Nov 10, 2014
- * Author: ucart
- */
-
-
-#include <stdlib.h>
-#include <stdio.h>
-#include <string.h>
-#include "uart.h"
-#include <xuartps.h>
-#include <xstatus.h>
-
-// Global PS's
-XUartPs* _Uart0PS;
-XUartPs* _Uart1PS;
-
-//This is copied from xuart driver
-/***************************************************/
-
-#define XUARTPS_MAX_BAUD_ERROR_RATE 3 /* max % error allowed */
-int XUartPs_SetBaudRate_ours(XUartPs *InstancePtr, u32 BaudRate)
-{
- u8 IterBAUDDIV; /* Iterator for available baud divisor values */
- u32 BRGR_Value; /* Calculated value for baud rate generator */
- u32 CalcBaudRate; /* Calculated baud rate */
- u32 BaudError; /* Diff between calculated and requested baud rate */
- u32 Best_BRGR = 0; /* Best value for baud rate generator */
- u8 Best_BAUDDIV = 0; /* Best value for baud divisor */
- u32 Best_Error = 0xFFFFFFFF;
- u32 PercentError;
- u32 ModeReg;
- u32 InputClk;
-
- /*
- * Asserts validate the input arguments
- */
- Xil_AssertNonvoid(InstancePtr != NULL);
- Xil_AssertNonvoid(InstancePtr->IsReady == XIL_COMPONENT_IS_READY);
- //Xil_AssertNonvoid(BaudRate <= XUARTPS_MAX_RATE);
- Xil_AssertNonvoid(BaudRate >= XUARTPS_MIN_RATE);
-
- /*
- * Make sure the baud rate is not impossilby large.
- * Fastest possible baud rate is Input Clock / 2.
- */
- if ((BaudRate * 2) > InstancePtr->Config.InputClockHz) {
- return XST_UART_BAUD_ERROR;
- }
- /*
- * Check whether the input clock is divided by 8
- */
- ModeReg = XUartPs_ReadReg( InstancePtr->Config.BaseAddress,
- XUARTPS_MR_OFFSET);
-
- InputClk = InstancePtr->Config.InputClockHz;
- if(ModeReg & XUARTPS_MR_CLKSEL) {
- InputClk = InstancePtr->Config.InputClockHz / 8;
- }
-
- /*
- * Determine the Baud divider. It can be 4to 254.
- * Loop through all possible combinations
- */
- for (IterBAUDDIV = 4; IterBAUDDIV < 255; IterBAUDDIV++) {
-
- /*
- * Calculate the value for BRGR register
- */
- BRGR_Value = InputClk / (BaudRate * (IterBAUDDIV + 1));
-
- /*
- * Calculate the baud rate from the BRGR value
- */
- CalcBaudRate = InputClk/ (BRGR_Value * (IterBAUDDIV + 1));
-
- /*
- * Avoid unsigned integer underflow
- */
- if (BaudRate > CalcBaudRate) {
- BaudError = BaudRate - CalcBaudRate;
- }
- else {
- BaudError = CalcBaudRate - BaudRate;
- }
-
- /*
- * Find the calculated baud rate closest to requested baud rate.
- */
- if (Best_Error > BaudError) {
-
- Best_BRGR = BRGR_Value;
- Best_BAUDDIV = IterBAUDDIV;
- Best_Error = BaudError;
- }
- }
-
- /*
- * Make sure the best error is not too large.
- */
- PercentError = (Best_Error * 100) / BaudRate;
- if (XUARTPS_MAX_BAUD_ERROR_RATE < PercentError) {
- return XST_UART_BAUD_ERROR;
- }
-
- /*
- * Disable TX and RX to avoid glitches when setting the baud rate.
- */
- XUartPs_DisableUart(InstancePtr);
-
- XUartPs_WriteReg(InstancePtr->Config.BaseAddress,
- XUARTPS_BAUDGEN_OFFSET, Best_BRGR);
- XUartPs_WriteReg(InstancePtr->Config.BaseAddress,
- XUARTPS_BAUDDIV_OFFSET, Best_BAUDDIV);
-
- /*
- * Enable device
- */
- XUartPs_EnableUart(InstancePtr);
-
- InstancePtr->BaudRate = BaudRate;
-
- return XST_SUCCESS;
-
-}
-
-/***************************************************/
-
-/************************************************/
-/************** Main UART Interface *************/
-
-XUartPs* uart_init(XUartPs* uartps_ptr, u16 deviceID, int baudRate) {
- XUartPs_Config* config = XUartPs_LookupConfig(deviceID);
-
- //Configure XUartPs instance
- int Status = XUartPs_CfgInitialize(uartps_ptr, config, config->BaseAddress);
- if (Status != XST_SUCCESS){
- return NULL;
- }
-
- //Set Baudrate for BT
- //XUartPs_SetBaudRate(uartps_ptr, baudRate);
- XUartPs_SetBaudRate_ours(uartps_ptr, baudRate);
-
- return uartps_ptr;
-}
-
-void uart_clearFIFOs(XUartPs* uartps_ptr) {
- //Get UART0 Control Register and clear the TX and RX Fifos
- int* uart_ctrl_reg = (int*) uartps_ptr->Config.BaseAddress;
- *uart_ctrl_reg |= 0x00000003; // clear TX & RX
-}
-
-void uart_sendByte(XUartPs* uartps_ptr, char data) {
- XUartPs_SendByte(uartps_ptr->Config.BaseAddress, data);
-}
-
-void uart_sendStr(XUartPs* uartps_ptr, char* str) {
- uart_sendBytes(uartps_ptr, str, strlen(str));
-}
-
-void uart_sendBytes(XUartPs* uartps_ptr, char* data, int numBytes) {
- while (uart_isSending(uartps_ptr))
- ;
-
- int done = 0;
- while (done < numBytes) {
- done += XUartPs_Send(uartps_ptr, (unsigned char*) (&data[done]), numBytes - done);
- }
-}
-
-int uart_isSending(XUartPs* uartps_ptr) {
- return XUartPs_IsSending(uartps_ptr);
-}
-
-int uart_hasData(XUartPs* uartps_ptr) {
- return XUartPs_IsReceiveData(uartps_ptr->Config.BaseAddress);
-}
-
-void uart_recvBytes(XUartPs* uartps_ptr, char* buffer, int numBytes) {
- int received = 0;
- while (received < numBytes) {
- received += XUartPs_Recv(uartps_ptr, (unsigned char*) &buffer[received], (numBytes - received));
- }
-}
-
-char uart_recvByte(XUartPs* uartps_ptr) {
- return XUartPs_RecvByte(uartps_ptr->Config.BaseAddress);
- //char buffer[1];
- //XUartPs_Recv(uartps_ptr, (unsigned char*) &buffer[0], 1);
-
-// return buffer[0];
-}
-
-/************************************************/
-/************************************************/
-
-
-
-
-
-/************************************************/
-/********** UART 0 convenience methods **********/
-
-XUartPs* uart0_init(u16 deviceID, int baudRate){
- if (_Uart0PS) {
- free(_Uart0PS);
- }
- _Uart0PS = malloc(sizeof(XUartPs));
- return uart_init(_Uart0PS, deviceID, baudRate);
-}
-
-void uart0_clearFIFOs(){
- uart_clearFIFOs(_Uart0PS);
-}
-
-void uart0_sendByte(u8 data){
- uart_sendByte(_Uart0PS, data);
-}
-
-void uart0_sendStr(char* str) {
- uart_sendStr(_Uart0PS, str);
-}
-
-void uart0_sendMetaData(metadata_t md)
-{
- uart0_sendByte(md.begin_char);
- uart0_sendByte(md.msg_type);
- uart0_sendByte(md.msg_subtype);
- uart0_sendByte(md.msg_id & 0x00ff);
- uart0_sendByte((md.msg_id >> 8) & 0x00ff);
- uart0_sendByte(md.data_len & 0x00ff);
- uart0_sendByte((md.data_len >> 8) & 0x00ff);
-}
-
-void uart0_sendBytes(char* data, int numBytes){
- uart_sendBytes(_Uart0PS, data, numBytes);
-}
-
-int uart0_isSending(){
- return uart_isSending(_Uart0PS);
-}
-
-int uart0_hasData(){
- return uart_hasData(_Uart0PS);
-}
-
-void uart0_recvBytes(char* buffer, int numBytes) {
- uart_recvBytes(_Uart0PS, buffer, numBytes);
-}
-
-char uart0_recvByte() {
- return uart_recvByte(_Uart0PS);
-}
-
-/************************************************/
-/************************************************/
-
-
-
-
-
-
-/************************************************/
-/********** UART 1 convenience methods **********/
-
-XUartPs* uart1_init(u16 deviceID, int baudRate){
- if (_Uart1PS) {
- free(_Uart1PS);
- }
- _Uart1PS = malloc(sizeof(XUartPs));
- return uart_init(_Uart1PS, deviceID, baudRate);
-}
-
-void uart1_clearFIFOs(){
- uart_clearFIFOs(_Uart1PS);
-}
-
-void uart1_sendByte(char data){
- uart_sendByte(_Uart1PS, data);
-}
-
-void uart1_sendStr(char* str) {
- uart_sendStr(_Uart1PS, str);
-}
-
-void uart1_sendBytes(char* data, int numBytes){
- uart_sendBytes(_Uart1PS, data, numBytes);
-}
-
-int uart1_isSending(){
- return uart_isSending(_Uart1PS);
-}
-
-int uart1_hasData(){
- return uart_hasData(_Uart1PS);
-}
-
-void uart1_recvBytes(char* buffer, int numBytes) {
- uart_recvBytes(_Uart1PS, buffer, numBytes);
-}
-
-char uart1_recvByte() {
- return uart_recvByte(_Uart1PS);
-}
-
-/************************************************/
-/************************************************/
-
-int tryReceivePacket(stringBuilder_t* sb, int echo) {
- while(uart0_hasData()) {
- char c = uart0_recvByte(); // begin char
- if(c == 0xBE) {
-// printf("Beginning to read packet from UART.\n");
-
- sb->addChar(sb, 0xBE);
- char type = uart0_recvByte();
- sb->addChar(sb, type); // type
- sb->addChar(sb, uart0_recvByte()); // subtype
- sb->addChar(sb, uart0_recvByte()); // id
- sb->addChar(sb, uart0_recvByte()); // id
-
- unsigned char byte5 = uart0_recvByte(); // data length
- unsigned char byte6 = uart0_recvByte(); // data length
-
- int datalen = (byte6 << 8) | byte5;
-// printf("Received packet with data length: %d", datalen);
-
- sb->addChar(sb, byte5);
- sb->addChar(sb, byte6);
-
- // Read all the data and the checksum byte
- int i;
- for(i=0; i < datalen + 1; i++)
- {
- sb->addChar(sb, uart0_recvByte());
- }
-
-// printf("Done reading packet from UART.\n");
- return type;
- } else {
-// printf("The first byte was not the begin char: %x\n", c);
- return -1;
- }
- }
- return -1;
-/*
-
- while (uart0_hasData()) {
- char c = uart0_recvByte();
- if (c == PACKET_START_CHAR) {
-#if DEBUG
- uart0_sendStr("Start ");
-#endif
- c = uart0_recvByte();
- char type = c;
-// sb->addChar(sb, type);
- int count = 0;
-
- //uart0_sendByte(c);
-
- // if it's a 'C' packet (a command for the quad), then
- // wait for the packet end char. Else, if it's a 'U' packet
- // which is an update from the camera system, then we read
- // a fixed number of bytes (UPDATE_SIZE # of bytes)
- if(type == 'C')
- {
- while (c != PACKET_END_CHAR)
- {
- c = uart0_recvByte();
- count++;
- if(c == PACKET_END_CHAR)
- break;
- if ((c != 0 && c != '\r' && c != '\n'))
- {
- sb->addChar(sb, c);
- if (echo) {
- uart0_sendByte(c);
- }
- }
- }
- return type;
- }
-
- else if(type == 'U')
- {
- while(count < UPDATE_SIZE)
- {
- c = uart0_recvByte();
- count++;
-
- sb->addChar(sb, c);
- if (echo) {
- uart0_sendByte(c);
- }
- }
- return type;
- }
-#if DEBUG
- uart0_sendStr("End:[");
- uart0_sendStr(sb->buf);
- uart0_sendStr("] ");
-#endif
- return 0;
- }
- }
-
- return 0;
- */
-}
-
diff --git a/quad/xsdk_workspace/imu_logger/src/uart.h b/quad/xsdk_workspace/imu_logger/src/uart.h
deleted file mode 100644
index 50d46896228aa8f3173d23579a1d8b08bd520891..0000000000000000000000000000000000000000
--- a/quad/xsdk_workspace/imu_logger/src/uart.h
+++ /dev/null
@@ -1,80 +0,0 @@
-/*
- * uart.h
- *
- * Created on: Nov 10, 2014
- * Author: ucart
- */
-
-#ifndef UART_H_
-#define UART_H_
-
-#include "xparameters.h"
-#include "xuartps.h"
-#include "stringBuilder.h"
-
-#define PACKET_START_CHAR 2
-#define PACKET_END_CHAR 3
-#define UPDATE_SIZE 28
-
-extern XUartPs* _Uart0PS;
-extern XUartPs* _Uart1PS;
-
-
-/************************************************/
-/************** Main UART Interface *************/
-XUartPs* uart_init(XUartPs* uartps_ptr, u16 deviceID, int baudRate);
-void uart_clearFIFOs(XUartPs* uartps_ptr);
-void uart_sendByte(XUartPs* uartps_ptr, char data);
-void uart_sendStr(XUartPs* uartps_ptr, char* str);
-void uart_sendBytes(XUartPs* uartps_ptr, char* data, int numBytes);
-int uart_isSending(XUartPs* uartps_ptr);
-int uart_hasData(XUartPs* uartps_ptr);
-
-//char uart_recvByte(); // block until char received
-
-//int uart_recvBytes(char* buffer, int numBytes, int timeoutMicros); // block until all received
-
-//void uart_recvCallback(void (*func)(char data));
-
-void uart_recvBytes(XUartPs* uartps_ptr, char* buffer, int numBytes);
-
-char uart_recvByte(XUartPs* uartps_ptr);
-/************************************************/
-/************************************************/
-
-
-
-/************************************************/
-/********** UART 0 convenience methods **********/
-XUartPs* uart0_init(u16 deviceID, int baudRate);
-void uart0_clearFIFOs();
-void uart0_sendByte(u8 data);
-void uart0_sendStr(char* data);
-void uart0_sendBytes(char* data, int numBytes);
-int uart0_isSending();
-int uart0_hasData();
-void uart0_recvBytes(char* buffer, int numBytes);
-char uart0_recvByte();
-void uart0_sendMetaData(metadata_t md);
-/************************************************/
-/************************************************/
-
-
-
-/************************************************/
-/********** UART 1 convenience methods **********/
-XUartPs* uart1_init(u16 deviceID, int baudRate);
-void uart1_clearFIFOs();
-void uart1_sendByte(char data);
-void uart1_sendStr(char* data);
-void uart1_sendBytes(char* data, int numBytes);
-int uart1_isSending();
-int uart1_hasData();
-void uart1_recvBytes(char* buffer, int numBytes);
-char uart1_recvByte();
-/************************************************/
-/************************************************/
-
-int tryReceivePacket(stringBuilder_t* sb, int echo);
-
-#endif /* UART_H_ */
diff --git a/quad/xsdk_workspace/imu_logger/src/update_gui.c b/quad/xsdk_workspace/imu_logger/src/update_gui.c
deleted file mode 100644
index 320936e5aa73f0309209ab945f56807f72d69d1d..0000000000000000000000000000000000000000
--- a/quad/xsdk_workspace/imu_logger/src/update_gui.c
+++ /dev/null
@@ -1,14 +0,0 @@
-/*
- * update_gui.c
- *
- * Created on: Feb 20, 2016
- * Author: ucart
- */
-
-#include "update_gui.h"
-
-int update_GUI(log_t* log_struct)
-{
- return 0;
-}
-
diff --git a/quad/xsdk_workspace/imu_logger/src/update_gui.h b/quad/xsdk_workspace/imu_logger/src/update_gui.h
deleted file mode 100644
index a348ade955798e225c96f1b59b27d79a75861d59..0000000000000000000000000000000000000000
--- a/quad/xsdk_workspace/imu_logger/src/update_gui.h
+++ /dev/null
@@ -1,28 +0,0 @@
-/*
- * update_gui.h
- *
- * Created on: Feb 20, 2016
- * Author: ucart
- */
-
-#ifndef UPDATE_GUI_H_
-#define UPDATE_GUI_H_
-
-#include <stdio.h>
-
-#include "log_data.h"
-
-/**
- * @brief
- * Updates the user interface.
- *
- * @param log_struct
- * structure of the data to be logged
- *
- * @return
- * error message
- *
- */
-int update_GUI(log_t* log_struct);
-
-#endif /* UPDATE_GUI_H_ */
diff --git a/quad/xsdk_workspace/imu_logger/src/user_input.c b/quad/xsdk_workspace/imu_logger/src/user_input.c
deleted file mode 100644
index 403aed279256eb1f321ae8d53d1e7ca981d1fc3f..0000000000000000000000000000000000000000
--- a/quad/xsdk_workspace/imu_logger/src/user_input.c
+++ /dev/null
@@ -1,77 +0,0 @@
-/*
- * user_input.c
- *
- * Created on: Feb 20, 2016
- * Author: ucart
- */
-
-#include "user_input.h"
-#include "uart.h"
-
-int get_user_input(log_t* log_struct, user_input_t* user_input_struct)
-{
- // Read in values from RC Receiver
- read_rec_all(user_input_struct->rc_commands);
-
- log_struct->commands.pitch = user_input_struct->rc_commands[PITCH];
- log_struct->commands.roll = user_input_struct->rc_commands[ROLL];
- log_struct->commands.throttle = user_input_struct->rc_commands[THROTTLE];
- log_struct->commands.yaw = user_input_struct->rc_commands[YAW];
-
- //create setpoints for manual flight
- // currently in units of radians
- user_input_struct->yaw_manual_setpoint = convert_from_receiver_cmd(user_input_struct->rc_commands[YAW], YAW_MAX, YAW_CENTER, YAW_MIN, YAW_RAD_TARGET, -(YAW_RAD_TARGET));
- user_input_struct->roll_angle_manual_setpoint = convert_from_receiver_cmd(user_input_struct->rc_commands[ROLL], ROLL_MAX, ROLL_CENTER, ROLL_MIN, ROLL_RAD_TARGET, -(ROLL_RAD_TARGET));
- user_input_struct->pitch_angle_manual_setpoint = convert_from_receiver_cmd(user_input_struct->rc_commands[PITCH], PITCH_MAX, PITCH_CENTER, PITCH_MIN, PITCH_RAD_TARGET, -(PITCH_RAD_TARGET));
-
- // Listen on bluetooth and if there's a packet,
- // then receive the packet and set hasPacket for later processing
- // "update packet" type processing is done in sensor.c
- // "command packet" type processing is done in control_algorithm.c
- user_input_struct->hasPacket = tryReceivePacket(user_input_struct->sb, 0);
-
- return 0;
-}
-
-int kill_condition(user_input_t* user_input_struct)
-{
- return read_kill(user_input_struct->rc_commands[GEAR]);
-}
-
-/*
- * Converts an RC receiver command to whatever units that max_target and min_target are in. This function first centers the receiver command at 0.
- * It creates a windowed linear function, based on the sign of the centered receiver command.
- *
- * -
- * / (x - center_receiver_cmd) * (min_target / (min_receiver_cmd - center_receiver_cmd)) when x <= 0
- * convert_receiver_cmd(x = receiver_cmd) = <
- * \ (x - center_receiver_cmd) * (max_target / (max_receiver_cmd - center_receiver_cmd) when x > 0
- * -
- *
- */
-float convert_from_receiver_cmd(int receiver_cmd, int max_receiver_cmd, int center_receiver_cmd, int min_receiver_cmd, float max_target, float min_target)
-{
- // centers the receiver command by subtracting the given center value. This means that if receiver_cmd == center then receiver_cmd_centered should be 0.
- int receiver_cmd_centered = receiver_cmd - center_receiver_cmd;
-
- if(receiver_cmd_centered <= 0) {
- float ret = ((float)(receiver_cmd_centered * min_target)) / ((float) (min_receiver_cmd - center_receiver_cmd));
-
- if(ret < min_target)
- ret = min_target;
-
- return ret;
- }
-
- else {
- float ret = ((float)(receiver_cmd_centered * max_target)) / ((float) (max_receiver_cmd - center_receiver_cmd));
-
- if(ret > max_target)
- ret = max_target;
-
- return ret;
- }
-
- return 0.0;
-}
-
diff --git a/quad/xsdk_workspace/imu_logger/src/user_input.h b/quad/xsdk_workspace/imu_logger/src/user_input.h
deleted file mode 100644
index 282e15dbedbf9a874d72ab692f069a4cca2da0cd..0000000000000000000000000000000000000000
--- a/quad/xsdk_workspace/imu_logger/src/user_input.h
+++ /dev/null
@@ -1,88 +0,0 @@
-/*
- * user_input.h
- *
- * Created on: Feb 20, 2016
- * Author: ucart
- */
-
-#ifndef USER_INPUT_H_
-#define USER_INPUT_H_
-
-#include <stdio.h>
-#include "type_def.h"
-#include "log_data.h"
-#include "util.h"
-#include "stringBuilder.h"
-
-/*
- * Aero channel declaration
- */
-
-#define THROTTLE 0
-#define ROLL 1
-#define PITCH 2
-#define YAW 3
-#define GEAR 4
-#define FLAP 5
-
-//////TARGETS
-
-#define YAW_DEG_TARGET 60.0f
-#define YAW_RAD_TARGET ((float) ((YAW_DEG_TARGET * 3.141592) / ((float) 180)))
-
-#define ROLL_DEG_TARGET 10.0f
-#define ROLL_RAD_TARGET ((float) ((ROLL_DEG_TARGET * 3.141592) / ((float) 180)))
-
-#define PITCH_DEG_TARGET 12.0f
-#define PITCH_RAD_TARGET ((float) ((PITCH_DEG_TARGET * 3.141592) / ((float) 180)))
-
-/////// Signals from the Rx mins, maxes and ranges
-
-//#define THROTTLE_MAX 191900
-//#define THROTTLE_MIN 110200
-//#define THROTTLE_RANGE THROTTLE_MAX - THROTTLE_MIN
-//
-//#define ROLL_MAX 170200
-//#define ROLL_MIN 129400
-//#define ROLL_CENTER 149800
-//#define ROLL_RANGE ROLL_MAX - ROLL_MIN
-//
-//#define PITCH_MAX 169900
-//#define PITCH_MIN 129500
-//#define PITCH_CENTER 149700
-//#define PITCH_RANGE PITCH_MAX - PITCH_MIN
-//
-//#define YAW_MAX 169400
-//#define YAW_MIN 129300
-//#define YAW_CENTER (YAW_MIN + YAW_MAX)/2 //149800
-//#define YAW_RANGE YAW_MAX - YAW_MIN
-//
-//#define GEAR_1 170800
-//#define GEAR_0 118300
-//
-//#define FLAP_1 192000
-//#define FLAP_0 107600
-//
-//#define GEAR_KILL GEAR_0 // The kill point for the program
-//#define BASE 150000
-
-/**
- * @brief
- * Receives user input to the system.
- *
- * @param log_struct
- * structure of the data to be logged
- *
- * @param user_input_struct
- * structure of the data inputed by the user
- *
- * @return
- * error message
- *
- */
-int get_user_input(log_t* log_struct, user_input_t* user_input_struct);
-int kill_condition(user_input_t* user_input_struct);
-float convert_from_receiver_cmd(int receiver_cmd, int max_receiver_cmd, int center_receiver_cmd, int min_receiver_cmd, float max_target, float min_target);
-
-
-#endif /* USER_INPUT_H_ */
diff --git a/quad/xsdk_workspace/imu_logger/src/util.c b/quad/xsdk_workspace/imu_logger/src/util.c
deleted file mode 100644
index f01c044f18e36cb7f84c80a31997ade43d144f58..0000000000000000000000000000000000000000
--- a/quad/xsdk_workspace/imu_logger/src/util.c
+++ /dev/null
@@ -1,514 +0,0 @@
-/*
- * util.c
- *
- * Created on: Oct 11, 2014
- * Author: ucart
- */
-
-#include "util.h"
-
-extern int motor0_bias, motor1_bias, motor2_bias, motor3_bias;
-//Global variable representing the current pulseW
-int pulseW = pulse_throttle_low;
-
-/**
- * Initializes the PWM output components.
- * Default pulse length = 1 ms
- * Default period length = 2.33 ms
- */
-void pwm_init() {
- printf("Period initialization starting\r\n");
-
- int* pwm_0 = (int*) PWM_0_ADDR + PWM_PERIOD;
- int* pwm_1 = (int*) PWM_1_ADDR + PWM_PERIOD;
- int* pwm_2 = (int*) PWM_2_ADDR + PWM_PERIOD;
- int* pwm_3 = (int*) PWM_3_ADDR + PWM_PERIOD;
-
- // Initializes all the PWM address to have the correct period width at 450 hz
- *pwm_0 = period_width;
- *pwm_1 = period_width;
- *pwm_2 = period_width;
- *pwm_3 = period_width;
- printf("Period initialization successful %d\n", period_width);
- // Initializes the PWM pulse lengths to be 1 ms
- *(int*) (PWM_0_ADDR + PWM_PULSE) = pulse_throttle_low;
- *(int*) (PWM_1_ADDR + PWM_PULSE) = pulse_throttle_low;
- *(int*) (PWM_2_ADDR + PWM_PULSE) = pulse_throttle_low;
- *(int*) (PWM_3_ADDR + PWM_PULSE) = pulse_throttle_low;
- printf("Pulse initialization successful %d\n", pulse_throttle_low);
-
-#ifdef X_CONFIG
- printf("In x config mode\n");
-#else
- printf("In + config mode\n");
-#endif
- usleep(1000000);
-}
-
-/**
- * Writes all PWM components to be the same given pulsewidth
- */
-void pwm_write_all(int pulseWidth) {
- // Check lower and upper bounds
- if (pulseWidth > pulse_throttle_high)
- pulseWidth = pulse_throttle_high;
- if (pulseWidth < pulse_throttle_low)
- pulseWidth = pulse_throttle_low;
- // Set all the pulse widths
- *(int*) (PWM_0_ADDR + PWM_PULSE) = pulseWidth;
- *(int*) (PWM_1_ADDR + PWM_PULSE) = pulseWidth;
- *(int*) (PWM_2_ADDR + PWM_PULSE) = pulseWidth;
- *(int*) (PWM_3_ADDR + PWM_PULSE) = pulseWidth;
-}
-/**
- * Write a given pulseWidth to a channel
- */
-void pwm_write_channel(int pulseWidth, int channel){
- // Check lower and upper bounds
- if (pulseWidth > pulse_throttle_high)
- pulseWidth = pulse_throttle_high;
- if (pulseWidth < pulse_throttle_low)
- pulseWidth = pulse_throttle_low;
-
- switch(channel){
- case 0:
- *(int*) (PWM_0_ADDR + PWM_PULSE) = pulseWidth;
- break;
- case 1:
- *(int*) (PWM_1_ADDR + PWM_PULSE) = pulseWidth;
- break;
- case 2:
- *(int*) (PWM_2_ADDR + PWM_PULSE) = pulseWidth;
- break;
- case 3:
- *(int*) (PWM_3_ADDR + PWM_PULSE) = pulseWidth;
- break;
- default:
- break;
- }
-}
-/**
- * Reads the registers from the PWM_Recorders, and returns the pulse width
- * of the last PWM signal to come in
- */
-int read_rec(int channel) {
- switch (channel) {
- case 0:
- return *((int*) PWM_REC_0_ADDR);
- case 1:
- return *((int*) PWM_REC_1_ADDR);
- case 2:
- return *((int*) PWM_REC_2_ADDR);
- case 3:
- return *((int*) PWM_REC_3_ADDR);
- case 4:
- return *((int*) PWM_REC_4_ADDR);
- case 5:
- return *((int*) PWM_REC_5_ADDR);
- default:
- return 0;
- }
-}
-/**
- * Reads all 4 receiver channels at once
- */
-void read_rec_all(int* mixer){
- int i;
- for(i = 0; i < 6; i++){
- mixer[i] = read_rec(i);
- }
-}
-
-int hexStrToInt(char *buf, int startIdx, int endIdx) {
- int result = 0;
- int i;
- int power = 0;
- for (i=endIdx; i >= startIdx; i--) {
- int value = buf[i];
- if ('0' <= value && value <= '9') {
- value -= '0';
- } else if ('a' <= value && value <= 'f') {
- value -= 'a';
- value += 10;
- } else if ('A' <= value && value <= 'F') {
- value -= 'A';
- value += 10;
- }
-
- result += (2 << (4 * power)) * value;
- power++;
- }
-
- return result;
-}
-
-void read_bluetooth_all(int* mixer) {
- char buffer[32];
-
- int done = 0;
- int gotS = 0;
- char c = 0;
- while (!done) {
- int counter = 0;
- if (!gotS) {
- c = uart0_recvByte();
- }
- if (c == 'S') {
-
- while (1) {
- char cc = uart0_recvByte();
- if (cc == 'S') {
- counter = 0;
- gotS = 1;
- break;
- }
- printf("C=%c,\r\n",cc);
- buffer[counter++] = cc;
-
-
- if (counter == 12) {
- buffer[12] = 0;
- done = 1;
- gotS = 0;
- }
- }
- //uart0_recvBytes(buffer, 12);
- //buffer[12] = 0;
-
-
- }
- }
-
-// // data := "XX XX XX XX XX"
-// uart0_recvBytes(buffer, 12);
-// buffer[12] = 0;
-//
-//
- int i;
- for(i=0; i < 5; i++) {
- mixer[i] = 0;
- }
-
- for (i=0; i < 4; i++) {
- //mixer[i] = hexStrToInt(buffer, 3*i, 3*i + 1);
- mixer[i] = (buffer[i*3] << 8) | buffer[i*3 + 1];
- }
-
- printf("mixer: \"%s\" -> %d %d %d %d %d\r\n", buffer, mixer[0], mixer[1], mixer[2], mixer[3], mixer[4]);
-
-}
-
-/**
- * Use the buttons to drive the pulse length of the channels
- */
-void b_drive_pulse() {
- int* btns = (int *) XPAR_BTNS_BASEADDR;
-
- // Increment the pulse width by 5% throttle
- if (*btns & 0x1) {
- pulseW += 1000;
- if (pulseW > 200000)
- pulseW = 200000;
- pwm_write_all(pulseW);
- while (*btns & 0x1)
- ;
- } //Decrease the pulse width by 5% throttle
- else if (*btns & 0x2) {
- pulseW -= 1000;
- if (pulseW < 100000) {
- pulseW = 100000;
- }
- pwm_write_all(pulseW);
- while (*btns & 0x2)
- ;
- }
- // Set the pulses back to default
- else if (*btns & 0x4) {
- pulseW = MOTOR_0_PERCENT;
- pwm_write_all(pulseW);
- }
- // Read the pulse width of pwm_recorder 0
- else if (*btns & 0x8) {
- int i;
- for(i = 0; i < 4; i++){
- xil_printf("Channel %d: %d\n", i, read_rec(i));
- }
- //xil_printf("%d\n",pulseW);
- while (*btns & 0x8)
- ;
- }
-}
-
-/**
- * Creates a sine wave driving the motors from 0 to 100% throttle
- */
-void sine_example(){
-
- int* btns = (int *) XPAR_BTNS_BASEADDR;
- /* Sine Wave */
- static double time = 0;
-
- time += .0001;
- pulseW = (int)fabs(sin(time)*(100000)) + 100000;
- //pulseW = (int) (sin(time) + 1)*50000 + 100000;
- if (*btns & 0x1){
- printf("%d", pulseW);
- printf(" %d\n", *(int*) (PWM_0_ADDR + PWM_PULSE));
- }
- pwm_write_all(pulseW);
- usleep(300);
-}
-
-void print_mixer(int * mixer){
- int i;
- for(i = 0; i < 4; i++){
- xil_printf("%d : %d ", i, mixer[i]);
- }
- xil_printf("\n");
-}
-
-/**
- * Argument is the reading from the pwm_recorder4 which is connected to the gear pwm
- * If the message from the receiver is 0 - gear, kill the system by sending a 1
- * Otherwise, do nothing
- */
-int read_kill(int gear){
- if(gear > 115000 && gear < 125000)
- return 1;
- return 0;
-}
-
-int read_flap(int flap)
-{
- // flap '0' is 108,000 CC (Up)
- // flap '1' is 192,000 CC (Down)
- // here we say if the reading is greater than 150,000 than its '1'; '0' otherwise
- if(flap > 150000)
- return 1;
- return 0;
-}
-
-/**
- * Turns off the motors
- */
-void pwm_kill(){
- // Initializes the PWM pulse lengths to be 1 ms
- *(int*) (PWM_0_ADDR + PWM_PULSE) = pulse_throttle_low;
- *(int*) (PWM_1_ADDR + PWM_PULSE) = pulse_throttle_low;
- *(int*) (PWM_2_ADDR + PWM_PULSE) = pulse_throttle_low;
- *(int*) (PWM_3_ADDR + PWM_PULSE) = pulse_throttle_low;
- xil_printf("Kill switch was touched, shutting off the motors and ending the program\n");
-}
-
-/**
- * Useful stuff in here in regards to PID tuning, and motor tuning
- * TODO : Scanf string stuff
- * TODO : Make adam do it :DDDDDDDD
- */
-void bluetoothTunePID(char instr, gam_t* gam, PID_t* CFpid, PID_t* Gpid){
- int wasX = 0;
- int wasPid = 0;
- int wasSetpoint = 0;
- int wasLog = 0;
- char buf[100];
-
- switch (instr) {
- case 'P':
- // Change this if tuning other PIDs
- CFpid->Kp += .5;
- wasPid = 1;
- break;
- case 'p':
- CFpid->Kp -= .5;
- wasPid = 1;
- break;
- case 'O':
- CFpid->Kp += .2;
- wasPid = 1;
- break;
- case 'o':
- CFpid->Kp -= .2;
- wasPid = 1;
- break;
-
- case 'I':
- CFpid->Kp += 0.1;
- wasPid = 1;
- break;
- case 'i':
- CFpid->Kp -= 0.1;
- wasPid = 1;
- break;
- case 'W':
- Gpid->Kp += 1;
- wasPid = 1;
- break;
- case 'w':
- Gpid->Kp -= 1;
- wasPid = 1;
- break;
- case 'E':
- Gpid->Kp += 2;
- wasPid = 1;
- break;
- case 'e':
- Gpid->Kp -= 2;
- wasPid = 1;
- break;
- case 'R':
- Gpid->Kp += 5;
- wasPid = 1;
- break;
- case 'r':
- Gpid->Kp -= 5;
- wasPid = 1;
- break;
- case 'D':
- CFpid->Kd += .1;
- wasPid = 1;
- break;
- case 'd':
- CFpid->Kd -= .1;
- wasPid = 1;
- break;
- case 'S':
- CFpid->setpoint += 1;
- wasSetpoint = 1;
- break;
- case 's':
- CFpid->setpoint -= 1;
- wasSetpoint = 1;
- break;
- case '0':
- motor0_bias += 100;
- wasPid = 0;
- break;
- case '1':
- motor1_bias += 100;
- wasPid = 0;
- break;
- case '2':
- motor2_bias += 100;
- wasPid = 0;
- break;
- case '3':
- motor3_bias += 100;
- wasPid = 0;
- break;
- case ')':
- motor0_bias -= 100;
- wasPid = 0;
- break;
- case '!':
- motor1_bias -= 100;
- wasPid = 0;
- break;
- case '@':
- motor2_bias -= 100;
- wasPid = 0;
- break;
- case '#':
- motor3_bias -= 100;
- wasPid = 0;
- break;
- case 'x':
- wasX = 1;
- break;
- case ' ':
- wasLog = 1;
- break;
-/* case 'm':
- pid->setpoint = -45.0;
- // Change set point
- break;
- case 'n':
- pid->setpoint = 45.0;
- */ // Change set point
- default:
- wasPid = 0;
- break;
- }
-
- if(wasX){
- return;
- }
- else if(wasSetpoint){
- sprintf(buf, "Setpoint: %4.1f\n\r", CFpid->setpoint);
- uart0_sendBytes(buf, strlen(buf));
- usleep(5000);
- }
- else if (wasPid) {
- /*sprintf(buf,
- "PAngle: %8.3f RAngle: %8.3f PID p: %8.3f d: %8.3f\r\n",
- compY, compX, pitchPID.Kp, pitchPID.Kd);*/
-
- sprintf(buf, "CFP Coeff: %4.1f D %4.1f GP Coeff: %4.1f\n\r", CFpid->Kp, CFpid->Kd, Gpid->Kp);
- uart0_sendBytes(buf, strlen(buf));
- usleep(5000);
- }
- else if (wasLog){
- sprintf(buf, "CX %5.2f GP GX: %5.2f\n\r", 0.0, gam->gyro_xVel_p);
- uart0_sendBytes(buf, strlen(buf));
- usleep(5000);
- }
- else {
- sprintf(buf, "Motor bias's \t\t0: %d 1: %d 2: %d 3: %d \r\n", motor0_bias,
- motor1_bias, motor2_bias, motor3_bias);
- uart0_sendBytes(buf, strlen(buf));
-
-// sprintf(buf, "P: %3.2f I: %3.2f D: %3.2f\r\n", log_struct.ang_vel_pitch_PID_values.P, log_struct.ang_vel_pitch_PID_values.I, log_struct.ang_vel_pitch_PID_values.D);
- uart0_sendBytes(buf, strlen(buf));
- usleep(1e4);
- }
-}
-/*
-void flash_MIO_7_led(int how_many_times, int ms_between_flashes)
-{
- if(how_many_times <= 0)
- return;
-
- while(how_many_times)
- {
- MIO7_led_on();
-
- usleep(ms_between_flashes * 500);
-
- MIO7_led_off();
-
- usleep(ms_between_flashes * 500);
-
- how_many_times--;
- }
-}
-
-void MIO7_led_off()
-{
- XGpioPs Gpio;
-
- XGpioPs_Config * ConfigPtr = XGpioPs_LookupConfig(XPAR_PS7_GPIO_0_DEVICE_ID);
- XGpioPs_CfgInitialize(&Gpio, ConfigPtr, ConfigPtr->BaseAddr);
-
- XGpioPs_SetDirectionPin(&Gpio, 7, 1);
-
- // Disable LED
- XGpioPs_WritePin(&Gpio, 7, 0x00);
-}
-
-void MIO7_led_on()
-{
- XGpioPs Gpio;
-
- XGpioPs_Config * ConfigPtr = XGpioPs_LookupConfig(XPAR_PS7_GPIO_0_DEVICE_ID);
- XGpioPs_CfgInitialize(&Gpio, ConfigPtr, ConfigPtr->BaseAddr);
-
- XGpioPs_SetDirectionPin(&Gpio, 7, 1);
-
- // Enable LED
- XGpioPs_WritePin(&Gpio, 7, 0x01);
-}
-*/
-void msleep(int msecs)
-{
- usleep(msecs * 1000);
-}
-
diff --git a/quad/xsdk_workspace/imu_logger/src/util.h b/quad/xsdk_workspace/imu_logger/src/util.h
deleted file mode 100644
index d14ae4da41a1c3481e1f5bf93ad3dac5f9762687..0000000000000000000000000000000000000000
--- a/quad/xsdk_workspace/imu_logger/src/util.h
+++ /dev/null
@@ -1,102 +0,0 @@
-/*
- * util.h
- *
- * Created on: Oct 11, 2014
- * Author: ucart
- */
-#ifndef _UTIL_H
-#define _UTIL_H
-
-#include <stdlib.h>
-#include <stdio.h>
-#include <string.h>
-#include <math.h>
-#include <xgpiops.h>
-#include "PID.h"
-#include "log_data.h"
-#include <sleep.h>
-#include "controllers.h"
-#include "xparameters.h"
-#include "uart.h"
-
-
-#define clock_rate 100000000
-#define frequency 450
-#define period_width clock_rate/frequency
-#define pulse_throttle_low clock_rate / 1000
-#define pulse_throttle_high clock_rate / 500
-#define MOTOR_0_PERCENT 115000
-
-
-#define XPAR_BTNS_BASEADDR 0x41200000
-
-/**
- * Various Addresses of custom IP components
- */
-#define PWM_0_ADDR XPAR_PWM_SIGNAL_OUT_WKILLSWITCH_0_BASEADDR
-#define PWM_1_ADDR XPAR_PWM_SIGNAL_OUT_WKILLSWITCH_1_BASEADDR
-#define PWM_2_ADDR XPAR_PWM_SIGNAL_OUT_WKILLSWITCH_2_BASEADDR
-#define PWM_3_ADDR XPAR_PWM_SIGNAL_OUT_WKILLSWITCH_3_BASEADDR
-#define PWM_REC_0_ADDR XPAR_PWM_RECORDER_0_BASEADDR
-#define PWM_REC_1_ADDR XPAR_PWM_RECORDER_1_BASEADDR
-#define PWM_REC_2_ADDR XPAR_PWM_RECORDER_2_BASEADDR
-#define PWM_REC_3_ADDR XPAR_PWM_RECORDER_3_BASEADDR
-#define PWM_REC_4_ADDR XPAR_PWM_RECORDER_4_BASEADDR
-#define PWM_REC_5_ADDR XPAR_PWM_RECORDER_5_BASEADDR
-
-/**
- * Register offsets within the custom IP
- */
-#define PWM_PERIOD 0
-#define PWM_PULSE 4
-
-void pwm_init();
-void pwm_write_all(int pulseWidth);
-void pwm_write_channel(int pulseWidth, int channel);
-
-int read_rec(int channel);
-void read_rec_all(int* mixer);
-
-void read_bluetooth_all(int* mixer);
-
-void b_drive_pulse();
-
-void sine_example();
-
-void print_mixer(int* mixer);
-int read_kill(int gear);
-int read_flap(int flap);
-void pwm_kill();
-
-void printLogging();
-void bluetoothTunePID(char instr, gam_t* gam, PID_t* CFpid, PID_t* Gpid);
-void msleep(int msecs);
-
-/**
- * @brief
- * Flashes the MIO7 LED how_many_times times and with ms_between_flashes between the flashes.
- *
- * @param how_many_times
- * times the LED should be flashed
- *
- * @param ms_between_flashes
- * time between flashes in milliseconds
- *
- */
-void flash_MIO_7_led(int how_many_times, int ms_between_flashes);
-
-/**
- * @brief
- * Turns off MIO7 LED.
- *
- */
-void MIO7_led_off();
-
-/**
- * @brief
- * Turns on MIO7 LED.
- *
- */
-void MIO7_led_on();
-
-#endif //_UTIL_H