diff --git a/quad/xsdk_workspace/modular_quad_pid/.cproject b/quad/xsdk_workspace/modular_quad_pid/.cproject
index 63ab92a73ddc5e18f02c14a2a0a35a750e67eee4..b6594e32f340de9327b789312fb975bd37009524 100644
--- a/quad/xsdk_workspace/modular_quad_pid/.cproject
+++ b/quad/xsdk_workspace/modular_quad_pid/.cproject
@@ -27,7 +27,9 @@
<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"/>
+ <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"/>
<option id="xilinx.gnu.compiler.dircategory.includes.1211006365" name="Include Paths" superClass="xilinx.gnu.compiler.dircategory.includes" valueType="includePath">
<listOptionValue builtIn="false" value="../../system_bsp/ps7_cortexa9_0/include"/>
@@ -106,11 +108,13 @@
<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"/>
+ <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>
<option id="xilinx.gnu.compiler.symbols.defined.1562495938" name="Defined symbols (-D)" superClass="xilinx.gnu.compiler.symbols.defined" valueType="definedSymbols">
<listOptionValue builtIn="false" value="NDEBUG=1"/>
</option>
- <option id="xilinx.gnu.compiler.dircategory.includes.1873624761" superClass="xilinx.gnu.compiler.dircategory.includes" valueType="includePath">
+ <option id="xilinx.gnu.compiler.dircategory.includes.1873624761" name="Include Paths" superClass="xilinx.gnu.compiler.dircategory.includes" valueType="includePath">
<listOptionValue builtIn="false" value="../../system_bsp/ps7_cortexa9_0/include"/>
<listOptionValue builtIn="false" value=""${workspace_loc:/modular_quad_pid/ext/computation_graph}""/>
<listOptionValue builtIn="false" value=""${workspace_loc:/modular_quad_pid/ext/quad_app}""/>
diff --git a/quad/xsdk_workspace/modular_quad_pid/.project b/quad/xsdk_workspace/modular_quad_pid/.project
index 61b0bf7e40e000c5d6a0812b1b46aa8230584d8a..7ae4ca25c0f0cd671784aad1590a7cc52299f7f1 100644
--- a/quad/xsdk_workspace/modular_quad_pid/.project
+++ b/quad/xsdk_workspace/modular_quad_pid/.project
@@ -4,7 +4,7 @@
<comment></comment>
<projects>
<project>system_bsp</project>
- <project>system_bsp_new</project>
+ <project>system_hw_platform</project>
</projects>
<buildSpec>
<buildCommand>
diff --git a/quad/xsdk_workspace/modular_quad_pid/src/hw_impl_zybo.c b/quad/xsdk_workspace/modular_quad_pid/src/hw_impl_zybo.c
index 9e74c42c34fb303a1112534fb750680305e2cf15..e8fceffa661ca92e0e1581d291a6f2b07d635819 100644
--- a/quad/xsdk_workspace/modular_quad_pid/src/hw_impl_zybo.c
+++ b/quad/xsdk_workspace/modular_quad_pid/src/hw_impl_zybo.c
@@ -60,3 +60,11 @@ struct LEDDriver create_zybo_mio7_led() {
mio7_led.turn_off = zybo_mio7_led_turn_off;
return mio7_led;
}
+
+struct SystemDriver create_zybo_system() {
+ struct SystemDriver sys;
+ sys.state = NULL;
+ sys.reset = zybo_system_reset;
+ sys.sleep = zybo_system_sleep;
+ return sys;
+}
diff --git a/quad/xsdk_workspace/modular_quad_pid/src/hw_impl_zybo.h b/quad/xsdk_workspace/modular_quad_pid/src/hw_impl_zybo.h
index 077869a2b05161679ed5d19dd157b886b36e2d52..9e30ddb024b0f247fdac100f71a5e15e965a4845 100644
--- a/quad/xsdk_workspace/modular_quad_pid/src/hw_impl_zybo.h
+++ b/quad/xsdk_workspace/modular_quad_pid/src/hw_impl_zybo.h
@@ -15,6 +15,8 @@
#include "xtime_l.h"
#include "xuartps.h"
#include "queue.h"
+#include "platform.h"
+#include "sleep.h"
int zybo_uart_reset(struct UARTDriver *self);
int zybo_uart_write(struct UARTDriver *self, unsigned char *data, unsigned int length);
@@ -48,6 +50,9 @@ int zybo_mio7_led_reset(struct LEDDriver *self);
int zybo_mio7_led_turn_on(struct LEDDriver *self);
int zybo_mio7_led_turn_off(struct LEDDriver *self);
+int zybo_system_reset(struct SystemDriver *self);
+int zybo_system_sleep(struct SystemDriver *self, unsigned long us);
+
struct UARTDriver create_zybo_uart();
struct PWMOutputDriver create_zybo_pwm_outputs();
struct PWMInputDriver create_zybo_pwm_inputs();
@@ -55,5 +60,9 @@ struct I2CDriver create_zybo_i2c();
struct TimerDriver create_zybo_global_timer();
struct TimerDriver create_zybo_axi_timer();
struct LEDDriver create_zybo_mio7_led();
+struct SystemDriver create_zybo_system();
+
+int test_zybo_i2c();
+int test_zybo_mio7_led_and_system();
#endif
diff --git a/quad/xsdk_workspace/modular_quad_pid/src/hw_impl_zybo_system.c b/quad/xsdk_workspace/modular_quad_pid/src/hw_impl_zybo_system.c
new file mode 100644
index 0000000000000000000000000000000000000000..2adcc4007d4891b0f830b58cc03258abf63ef62d
--- /dev/null
+++ b/quad/xsdk_workspace/modular_quad_pid/src/hw_impl_zybo_system.c
@@ -0,0 +1,11 @@
+#include "hw_impl_zybo.h"
+
+int zybo_system_reset(struct SystemDriver *sys) {
+ return 0;
+}
+
+int zybo_system_sleep(struct SystemDriver *sys, unsigned long us) {
+ usleep(us);
+ return 0;
+}
+
diff --git a/quad/xsdk_workspace/modular_quad_pid/src/hw_impl_zybo_tests.c b/quad/xsdk_workspace/modular_quad_pid/src/hw_impl_zybo_tests.c
new file mode 100644
index 0000000000000000000000000000000000000000..865830b39a86aa30e5e7f1dcf2653c0c0e742123
--- /dev/null
+++ b/quad/xsdk_workspace/modular_quad_pid/src/hw_impl_zybo_tests.c
@@ -0,0 +1,48 @@
+#include "hw_impl_zybo.h"
+#include "type_def.h"
+#include "iic_utils.h"
+#include <stdio.h>
+
+/**
+ * Use the LEDDriver and SystemDriver to create a basic "blink" program,
+ * using the mio7 LED on the Zybo board.
+ *
+ * Instructions:
+ * 1) Use the USB cable and plug Zybo board into computer.
+ * 2) Run.
+ * 3) Observe MIO7 LED on board blinking at 1 second intervals.
+ */
+int test_zybo_mio7_led_and_system() {
+ struct LEDDriver mio7 = create_zybo_mio7_led();
+ struct SystemDriver sys = create_zybo_system();
+ mio7.reset(&mio7);
+ sys.reset(&sys);
+
+ while (1) {
+ mio7.turn_on(&mio7);
+ sys.sleep(&sys, 1000000);
+ mio7.turn_off(&mio7);
+ sys.sleep(&sys, 1000000);
+ }
+
+ return 0;
+}
+
+int test_zybo_i2c() {
+ struct I2CDriver i2c = create_zybo_i2c();
+ struct SystemDriver sys = create_zybo_system();
+ i2c.reset(&i2c);
+ sys.reset(&sys);
+
+ lidar_t lidar = { };
+ iic_set_globals(&i2c, &sys);
+ if (iic0_lidarlite_init()) return 0;
+ short x;
+ while (1) {
+ iic0_lidarlite_read_distance(&lidar);
+ x = lidar.distance_cm;
+ }
+ return 0;
+}
+
+
diff --git a/quad/xsdk_workspace/modular_quad_pid/src/main.c b/quad/xsdk_workspace/modular_quad_pid/src/main.c
index 2d50f0b4a7b2313e893f90d63a66f150b6369ef4..c62e717ed23e50ad696da047d3de139fb2da920e 100644
--- a/quad/xsdk_workspace/modular_quad_pid/src/main.c
+++ b/quad/xsdk_workspace/modular_quad_pid/src/main.c
@@ -2,6 +2,9 @@
#include "hw_impl_zybo.h"
#include "quad_app.h"
#include "type_def.h"
+#include "platform.h"
+
+#define RUN_TESTS
int setup_hardware(hardware_t *hardware) {
hardware->i2c = create_zybo_i2c();
@@ -16,6 +19,20 @@ int setup_hardware(hardware_t *hardware) {
int main()
{
+ // Zynq initialization
+ init_platform();
+
+#ifdef RUN_TESTS
+ test_zybo_mio7_led_and_system();
+ //test_zybo_i2c();
+ //test_zybo_pwm_inputs();
+ //test_zybo_pwm_outputs();
+ //test_zybo_uart();
+ //test_zybo_axi_timer();
+ return 0;
+#endif
+
+ // Run the main quad application
quad_main(setup_hardware);
return 0;
}