diff --git a/quad/Makefile b/quad/Makefile
index 4e4e57905ec359589323239514d60ae25436bd46..6e37fd855364b726613d5ce1d7a8e3d9c841db42 100644
--- a/quad/Makefile
+++ b/quad/Makefile
@@ -5,7 +5,9 @@ WS = $(CURDIR)/xsdk_workspace
 
 BOOT = $(OUTDIR)/BOOT.bin
 
-.PHONY: default zybo boot test clean deep-clean
+.PHONY: all default zybo boot test clean deep-clean
+
+all: default zybo
 
 default:
 	$(MAKE) -C src/test
@@ -31,16 +33,10 @@ deep-clean:
 	$(MAKE) -C src/queue clean
 	$(MAKE) -C src/computation_graph clean
 	$(MAKE) -C src/quad_app clean
+	bash scripts/clean_xsdk_workspace.sh
 
 $(OUTDIR):
 	mkdir $(OUTDIR)
 
 $(BOOT): zybo | $(OUTDIR)
-	echo "the_ROM_image:" > zybo_fsbl.bif
-	echo "{" >> zybo_fsbl.bif
-	echo $(WS)/zybo_fsbl/Release/zybo_fsbl.elf >> zybo_fsbl.bif
-	echo $(WS)/system_hw_platform/system.bit >> zybo_fsbl.bif
-	echo $(WS)/modular_quad_pid/Release/modular_quad_pid.elf >> zybo_fsbl.bif
-	echo "}" >> zybo_fsbl.bif
-	/remote/Xilinx/2015.4/SDK/2015.4/bin/bootgen -arch zynq -image zybo_fsbl.bif -o $(BOOT)
-	rm zybo_fsbl.bif
+	bash scripts/create_zybo_boot.sh
\ No newline at end of file
diff --git a/quad/scripts/build_zybo.sh b/quad/scripts/build_zybo.sh
index 85cf328a8179459d11d33e657b9444587757d143..29a7361b6910034de360407e44c39fefe28df642 100644
--- a/quad/scripts/build_zybo.sh
+++ b/quad/scripts/build_zybo.sh
@@ -1,10 +1,12 @@
 #!/bin/bash
 
-DIR="$(dirname "${BASH_SOURCE[0]}")"
-
 set -e
 
+QUADDIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )"/.. && pwd )"
+WS=$QUADDIR/xsdk_workspace
+
 PATH=$PATH:/remote/Xilinx/14.7:/opt/Xilinx/14.7/ISE_DS
 source settings64.sh
 
-cd $DIR/../xsdk_workspace/modular_quad_pid/Release && make all
\ No newline at end of file
+make -C $WS/system_bsp all
+make -C $WS/modular_quad_pid/Release all
diff --git a/quad/scripts/clean_xsdk_workspace.sh b/quad/scripts/clean_xsdk_workspace.sh
new file mode 100644
index 0000000000000000000000000000000000000000..84b88cc96a4a448d7d64493e6e8f6b6a98578fa3
--- /dev/null
+++ b/quad/scripts/clean_xsdk_workspace.sh
@@ -0,0 +1,10 @@
+#!/bin/bash
+
+set -e
+
+QUADDIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )"/.. && pwd )"
+WS=$QUADDIR/xsdk_workspace
+
+make -C $WS/modular_quad_pid/Debug clean
+make -C $WS/modular_quad_pid/Release  clean
+make -C $WS/system_bsp clean
\ No newline at end of file
diff --git a/quad/scripts/create_zybo_boot.sh b/quad/scripts/create_zybo_boot.sh
new file mode 100644
index 0000000000000000000000000000000000000000..5b1d5af820c752b9e1e6a548d1047ff300f75f60
--- /dev/null
+++ b/quad/scripts/create_zybo_boot.sh
@@ -0,0 +1,20 @@
+#!/bin/bash
+set -e
+
+QUADDIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )"/.. && pwd )"
+WS=$QUADDIR/xsdk_workspace
+
+PATH=$PATH:/remote/Xilinx/14.7:/opt/Xilinx/14.7/ISE_DS
+source settings64.sh
+
+# Working directory:
+cd $QUADDIR/out
+
+echo "the_ROM_image:" > zybo_fsbl.bif
+echo "{" >> zybo_fsbl.bif
+echo $WS/zybo_fsbl/Release/zybo_fsbl.elf >> zybo_fsbl.bif
+echo $WS/system_hw_platform/system.bit >> zybo_fsbl.bif
+echo $WS/modular_quad_pid/Release/modular_quad_pid.elf >> zybo_fsbl.bif
+echo "}" >> zybo_fsbl.bif
+bootgen -image zybo_fsbl.bif -o BOOT.bin
+rm zybo_fsbl.bif
\ No newline at end of file
diff --git a/quad/xsdk_workspace/.gitignore b/quad/xsdk_workspace/.gitignore
index 250db3108c4526ba13eb8032ce565a1844418e08..9dca879c3508256ce6e5b3de1c1e16aa7b91893c 100644
--- a/quad/xsdk_workspace/.gitignore
+++ b/quad/xsdk_workspace/.gitignore
@@ -1,5 +1,6 @@
 SDK.log
 test.log
 .metadata/
-system_bsp/
+system_bsp/ps7_cortexa9_0/
+system_bsp/libgen.log
 TAGS
\ No newline at end of file
diff --git a/quad/xsdk_workspace/system_bsp/.cproject b/quad/xsdk_workspace/system_bsp/.cproject
new file mode 100644
index 0000000000000000000000000000000000000000..0e212ccd1591addcefd8a4373b66c277b1bbc84f
--- /dev/null
+++ b/quad/xsdk_workspace/system_bsp/.cproject
@@ -0,0 +1,15 @@
+<?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="org.eclipse.cdt.core.default.config.1576736349">
+			<storageModule buildSystemId="org.eclipse.cdt.core.defaultConfigDataProvider" id="org.eclipse.cdt.core.default.config.1576736349" moduleId="org.eclipse.cdt.core.settings" name="Configuration">
+				<externalSettings/>
+				<extensions/>
+			</storageModule>
+			<storageModule moduleId="org.eclipse.cdt.core.externalSettings"/>
+		</cconfiguration>
+	</storageModule>
+	<storageModule moduleId="org.eclipse.cdt.core.LanguageSettingsProviders"/>
+</cproject>
diff --git a/quad/xsdk_workspace/system_bsp/.project b/quad/xsdk_workspace/system_bsp/.project
new file mode 100644
index 0000000000000000000000000000000000000000..922cddb11b529d10df5598fa8248359e0be5db72
--- /dev/null
+++ b/quad/xsdk_workspace/system_bsp/.project
@@ -0,0 +1,76 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<projectDescription>
+	<name>system_bsp</name>
+	<comment></comment>
+	<projects>
+		<project>system_hw_platform</project>
+	</projects>
+	<buildSpec>
+		<buildCommand>
+			<name>org.eclipse.cdt.make.core.makeBuilder</name>
+			<arguments>
+				<dictionary>
+					<key>org.eclipse.cdt.core.errorOutputParser</key>
+					<value>org.eclipse.cdt.core.GASErrorParser;org.eclipse.cdt.core.GCCErrorParser;org.eclipse.cdt.core.GLDErrorParser;org.eclipse.cdt.core.GmakeErrorParser;org.eclipse.cdt.core.VCErrorParser;org.eclipse.cdt.core.CWDLocator;org.eclipse.cdt.core.MakeErrorParser;</value>
+				</dictionary>
+				<dictionary>
+					<key>org.eclipse.cdt.make.core.append_environment</key>
+					<value>true</value>
+				</dictionary>
+				<dictionary>
+					<key>org.eclipse.cdt.make.core.build.arguments</key>
+					<value></value>
+				</dictionary>
+				<dictionary>
+					<key>org.eclipse.cdt.make.core.build.command</key>
+					<value>make</value>
+				</dictionary>
+				<dictionary>
+					<key>org.eclipse.cdt.make.core.build.target.auto</key>
+					<value>all</value>
+				</dictionary>
+				<dictionary>
+					<key>org.eclipse.cdt.make.core.build.target.clean</key>
+					<value>clean</value>
+				</dictionary>
+				<dictionary>
+					<key>org.eclipse.cdt.make.core.build.target.inc</key>
+					<value>all</value>
+				</dictionary>
+				<dictionary>
+					<key>org.eclipse.cdt.make.core.enableAutoBuild</key>
+					<value>true</value>
+				</dictionary>
+				<dictionary>
+					<key>org.eclipse.cdt.make.core.enableCleanBuild</key>
+					<value>true</value>
+				</dictionary>
+				<dictionary>
+					<key>org.eclipse.cdt.make.core.enableFullBuild</key>
+					<value>true</value>
+				</dictionary>
+				<dictionary>
+					<key>org.eclipse.cdt.make.core.enabledIncrementalBuild</key>
+					<value>true</value>
+				</dictionary>
+				<dictionary>
+					<key>org.eclipse.cdt.make.core.environment</key>
+					<value></value>
+				</dictionary>
+				<dictionary>
+					<key>org.eclipse.cdt.make.core.stopOnError</key>
+					<value>false</value>
+				</dictionary>
+				<dictionary>
+					<key>org.eclipse.cdt.make.core.useDefaultBuildCmd</key>
+					<value>true</value>
+				</dictionary>
+			</arguments>
+		</buildCommand>
+	</buildSpec>
+	<natures>
+		<nature>com.xilinx.sdk.sw.SwProjectNature</nature>
+		<nature>org.eclipse.cdt.core.cnature</nature>
+		<nature>org.eclipse.cdt.make.core.makeNature</nature>
+	</natures>
+</projectDescription>
diff --git a/quad/xsdk_workspace/system_bsp/.sdkproject b/quad/xsdk_workspace/system_bsp/.sdkproject
new file mode 100644
index 0000000000000000000000000000000000000000..3135ec9f796f86108f3510421221835514fcc15f
--- /dev/null
+++ b/quad/xsdk_workspace/system_bsp/.sdkproject
@@ -0,0 +1,3 @@
+THIRPARTY=false
+PROCESSOR=ps7_cortexa9_0
+MSS_FILE=system.mss
diff --git a/quad/xsdk_workspace/system_bsp/Makefile b/quad/xsdk_workspace/system_bsp/Makefile
new file mode 100644
index 0000000000000000000000000000000000000000..fe2a0efc7cc39ff9edfdbb9064b229a72106cb58
--- /dev/null
+++ b/quad/xsdk_workspace/system_bsp/Makefile
@@ -0,0 +1,21 @@
+# Makefile generated by Xilinx SDK.
+
+-include libgen.options
+
+LIBRARIES = ${PROCESSOR}/lib/libxil.a
+MSS = system.mss
+
+all: libs
+	@echo 'Finished building libraries'
+
+libs: $(LIBRARIES)
+
+$(LIBRARIES): $(MSS)
+	libgen -hw ${HWSPEC}\
+	       ${REPOSITORIES}\
+	       -pe ${PROCESSOR} \
+	       -log libgen.log \
+	       $(MSS)
+
+clean:
+	rm -rf ${PROCESSOR}
diff --git a/quad/xsdk_workspace/system_bsp/libgen.options b/quad/xsdk_workspace/system_bsp/libgen.options
new file mode 100644
index 0000000000000000000000000000000000000000..ac5ba396687b73316827155661d71b9247b953be
--- /dev/null
+++ b/quad/xsdk_workspace/system_bsp/libgen.options
@@ -0,0 +1,3 @@
+PROCESSOR=ps7_cortexa9_0
+REPOSITORIES=
+HWSPEC=../system_hw_platform/system.xml
diff --git a/quad/xsdk_workspace/system_bsp/system.mss b/quad/xsdk_workspace/system_bsp/system.mss
new file mode 100644
index 0000000000000000000000000000000000000000..ceb921a9ac033db1e420df4bdfd1997b061f6f83
--- /dev/null
+++ b/quad/xsdk_workspace/system_bsp/system.mss
@@ -0,0 +1,291 @@
+
+ PARAMETER VERSION = 2.2.0
+
+
+BEGIN OS
+ PARAMETER OS_NAME = standalone
+ PARAMETER OS_VER = 3.11.a
+ PARAMETER PROC_INSTANCE = ps7_cortexa9_0
+ PARAMETER STDIN = ps7_uart_1
+ PARAMETER STDOUT = ps7_uart_1
+END
+
+
+BEGIN PROCESSOR
+ PARAMETER DRIVER_NAME = cpu_cortexa9
+ PARAMETER DRIVER_VER = 1.01.a
+ PARAMETER HW_INSTANCE = ps7_cortexa9_0
+END
+
+
+BEGIN DRIVER
+ PARAMETER DRIVER_NAME = tmrctr
+ PARAMETER DRIVER_VER = 2.05.a
+ PARAMETER HW_INSTANCE = axi_timer_0
+END
+
+BEGIN DRIVER
+ PARAMETER DRIVER_NAME = gpio
+ PARAMETER DRIVER_VER = 3.01.a
+ PARAMETER HW_INSTANCE = btns_4bits_tri_io
+END
+
+BEGIN DRIVER
+ PARAMETER DRIVER_NAME = generic
+ PARAMETER DRIVER_VER = 1.00.a
+ PARAMETER HW_INSTANCE = ps7_afi_0
+END
+
+BEGIN DRIVER
+ PARAMETER DRIVER_NAME = generic
+ PARAMETER DRIVER_VER = 1.00.a
+ PARAMETER HW_INSTANCE = ps7_afi_1
+END
+
+BEGIN DRIVER
+ PARAMETER DRIVER_NAME = generic
+ PARAMETER DRIVER_VER = 1.00.a
+ PARAMETER HW_INSTANCE = ps7_afi_2
+END
+
+BEGIN DRIVER
+ PARAMETER DRIVER_NAME = generic
+ PARAMETER DRIVER_VER = 1.00.a
+ PARAMETER HW_INSTANCE = ps7_afi_3
+END
+
+BEGIN DRIVER
+ PARAMETER DRIVER_NAME = generic
+ PARAMETER DRIVER_VER = 1.00.a
+ PARAMETER HW_INSTANCE = ps7_coresight_comp_0
+END
+
+BEGIN DRIVER
+ PARAMETER DRIVER_NAME = generic
+ PARAMETER DRIVER_VER = 1.00.a
+ PARAMETER HW_INSTANCE = ps7_ddr_0
+END
+
+BEGIN DRIVER
+ PARAMETER DRIVER_NAME = generic
+ PARAMETER DRIVER_VER = 1.00.a
+ PARAMETER HW_INSTANCE = ps7_ddrc_0
+END
+
+BEGIN DRIVER
+ PARAMETER DRIVER_NAME = devcfg
+ PARAMETER DRIVER_VER = 2.04.a
+ PARAMETER HW_INSTANCE = ps7_dev_cfg_0
+END
+
+BEGIN DRIVER
+ PARAMETER DRIVER_NAME = dmaps
+ PARAMETER DRIVER_VER = 1.06.a
+ PARAMETER HW_INSTANCE = ps7_dma_ns
+END
+
+BEGIN DRIVER
+ PARAMETER DRIVER_NAME = dmaps
+ PARAMETER DRIVER_VER = 1.06.a
+ PARAMETER HW_INSTANCE = ps7_dma_s
+END
+
+BEGIN DRIVER
+ PARAMETER DRIVER_NAME = emacps
+ PARAMETER DRIVER_VER = 1.05.a
+ PARAMETER HW_INSTANCE = ps7_ethernet_0
+END
+
+BEGIN DRIVER
+ PARAMETER DRIVER_NAME = generic
+ PARAMETER DRIVER_VER = 1.00.a
+ PARAMETER HW_INSTANCE = ps7_globaltimer_0
+END
+
+BEGIN DRIVER
+ PARAMETER DRIVER_NAME = gpiops
+ PARAMETER DRIVER_VER = 1.02.a
+ PARAMETER HW_INSTANCE = ps7_gpio_0
+END
+
+BEGIN DRIVER
+ PARAMETER DRIVER_NAME = generic
+ PARAMETER DRIVER_VER = 1.00.a
+ PARAMETER HW_INSTANCE = ps7_gpv_0
+END
+
+BEGIN DRIVER
+ PARAMETER DRIVER_NAME = iicps
+ PARAMETER DRIVER_VER = 1.04.a
+ PARAMETER HW_INSTANCE = ps7_i2c_0
+END
+
+BEGIN DRIVER
+ PARAMETER DRIVER_NAME = generic
+ PARAMETER DRIVER_VER = 1.00.a
+ PARAMETER HW_INSTANCE = ps7_intc_dist_0
+END
+
+BEGIN DRIVER
+ PARAMETER DRIVER_NAME = generic
+ PARAMETER DRIVER_VER = 1.00.a
+ PARAMETER HW_INSTANCE = ps7_iop_bus_config_0
+END
+
+BEGIN DRIVER
+ PARAMETER DRIVER_NAME = generic
+ PARAMETER DRIVER_VER = 1.00.a
+ PARAMETER HW_INSTANCE = ps7_l2cachec_0
+END
+
+BEGIN DRIVER
+ PARAMETER DRIVER_NAME = generic
+ PARAMETER DRIVER_VER = 1.00.a
+ PARAMETER HW_INSTANCE = ps7_ocmc_0
+END
+
+BEGIN DRIVER
+ PARAMETER DRIVER_NAME = qspips
+ PARAMETER DRIVER_VER = 2.03.a
+ PARAMETER HW_INSTANCE = ps7_qspi_0
+END
+
+BEGIN DRIVER
+ PARAMETER DRIVER_NAME = generic
+ PARAMETER DRIVER_VER = 1.00.a
+ PARAMETER HW_INSTANCE = ps7_qspi_linear_0
+END
+
+BEGIN DRIVER
+ PARAMETER DRIVER_NAME = generic
+ PARAMETER DRIVER_VER = 1.00.a
+ PARAMETER HW_INSTANCE = ps7_ram_0
+END
+
+BEGIN DRIVER
+ PARAMETER DRIVER_NAME = generic
+ PARAMETER DRIVER_VER = 1.00.a
+ PARAMETER HW_INSTANCE = ps7_ram_1
+END
+
+BEGIN DRIVER
+ PARAMETER DRIVER_NAME = generic
+ PARAMETER DRIVER_VER = 1.00.a
+ PARAMETER HW_INSTANCE = ps7_scuc_0
+END
+
+BEGIN DRIVER
+ PARAMETER DRIVER_NAME = scugic
+ PARAMETER DRIVER_VER = 1.05.a
+ PARAMETER HW_INSTANCE = ps7_scugic_0
+END
+
+BEGIN DRIVER
+ PARAMETER DRIVER_NAME = scutimer
+ PARAMETER DRIVER_VER = 1.02.a
+ PARAMETER HW_INSTANCE = ps7_scutimer_0
+END
+
+BEGIN DRIVER
+ PARAMETER DRIVER_NAME = scuwdt
+ PARAMETER DRIVER_VER = 1.02.a
+ PARAMETER HW_INSTANCE = ps7_scuwdt_0
+END
+
+BEGIN DRIVER
+ PARAMETER DRIVER_NAME = generic
+ PARAMETER DRIVER_VER = 1.00.a
+ PARAMETER HW_INSTANCE = ps7_sd_0
+END
+
+BEGIN DRIVER
+ PARAMETER DRIVER_NAME = generic
+ PARAMETER DRIVER_VER = 1.00.a
+ PARAMETER HW_INSTANCE = ps7_slcr_0
+END
+
+BEGIN DRIVER
+ PARAMETER DRIVER_NAME = uartps
+ PARAMETER DRIVER_VER = 1.05.a
+ PARAMETER HW_INSTANCE = ps7_uart_0
+END
+
+BEGIN DRIVER
+ PARAMETER DRIVER_NAME = uartps
+ PARAMETER DRIVER_VER = 1.05.a
+ PARAMETER HW_INSTANCE = ps7_uart_1
+END
+
+BEGIN DRIVER
+ PARAMETER DRIVER_NAME = usbps
+ PARAMETER DRIVER_VER = 1.05.a
+ PARAMETER HW_INSTANCE = ps7_usb_0
+END
+
+BEGIN DRIVER
+ PARAMETER DRIVER_NAME = xadcps
+ PARAMETER DRIVER_VER = 1.02.a
+ PARAMETER HW_INSTANCE = ps7_xadc_0
+END
+
+BEGIN DRIVER
+ PARAMETER DRIVER_NAME = generic
+ PARAMETER DRIVER_VER = 1.00.a
+ PARAMETER HW_INSTANCE = pwm_recorder_0
+END
+
+BEGIN DRIVER
+ PARAMETER DRIVER_NAME = generic
+ PARAMETER DRIVER_VER = 1.00.a
+ PARAMETER HW_INSTANCE = pwm_recorder_1
+END
+
+BEGIN DRIVER
+ PARAMETER DRIVER_NAME = generic
+ PARAMETER DRIVER_VER = 1.00.a
+ PARAMETER HW_INSTANCE = pwm_recorder_2
+END
+
+BEGIN DRIVER
+ PARAMETER DRIVER_NAME = generic
+ PARAMETER DRIVER_VER = 1.00.a
+ PARAMETER HW_INSTANCE = pwm_recorder_3
+END
+
+BEGIN DRIVER
+ PARAMETER DRIVER_NAME = generic
+ PARAMETER DRIVER_VER = 1.00.a
+ PARAMETER HW_INSTANCE = pwm_recorder_4
+END
+
+BEGIN DRIVER
+ PARAMETER DRIVER_NAME = generic
+ PARAMETER DRIVER_VER = 1.00.a
+ PARAMETER HW_INSTANCE = pwm_recorder_5
+END
+
+BEGIN DRIVER
+ PARAMETER DRIVER_NAME = generic
+ PARAMETER DRIVER_VER = 1.00.a
+ PARAMETER HW_INSTANCE = pwm_signal_out_wkillswitch_0
+END
+
+BEGIN DRIVER
+ PARAMETER DRIVER_NAME = generic
+ PARAMETER DRIVER_VER = 1.00.a
+ PARAMETER HW_INSTANCE = pwm_signal_out_wkillswitch_1
+END
+
+BEGIN DRIVER
+ PARAMETER DRIVER_NAME = generic
+ PARAMETER DRIVER_VER = 1.00.a
+ PARAMETER HW_INSTANCE = pwm_signal_out_wkillswitch_2
+END
+
+BEGIN DRIVER
+ PARAMETER DRIVER_NAME = generic
+ PARAMETER DRIVER_VER = 1.00.a
+ PARAMETER HW_INSTANCE = pwm_signal_out_wkillswitch_3
+END
+
+