diff --git a/quad/src/quad_app/quad_app.c b/quad/src/quad_app/quad_app.c
index 561df5b1a26cb6394d487751cab9aeef5c227b7a..a098f374ad2a782daf5faafd1812d232e5efecce 100644
--- a/quad/src/quad_app/quad_app.c
+++ b/quad/src/quad_app/quad_app.c
@@ -19,7 +19,7 @@
int quad_main(int (*setup_hardware)(hardware_t *hardware_struct))
{
// Structures to be used throughout
- modular_structs_t structs = { };
+ modular_structs_t structs = { };
// Wire up hardware
setup_hardware(&structs.hardware_struct);
diff --git a/quad/xsdk_workspace_vivado/real_quad/src/hw_impl_zybo.h b/quad/xsdk_workspace_vivado/real_quad/src/hw_impl_zybo.h
index 1f9b0fb651be858b26250012f5396fe6abe3558a..5555a4adf8d527bd30b1d2837b3884e929f001d9 100644
--- a/quad/xsdk_workspace_vivado/real_quad/src/hw_impl_zybo.h
+++ b/quad/xsdk_workspace_vivado/real_quad/src/hw_impl_zybo.h
@@ -29,26 +29,27 @@
#define QUAD_NUM 1
#if QUAD_NUM==0
-#define IMU_ACCX_SIGN 1
+#define IMU_ACCX_SIGN -1
#define IMU_ACCY_SIGN 1
#define IMU_ACCZ_SIGN 1
-#define IMU_GYRX_SIGN 1
+#define IMU_GYRX_SIGN -1
#define IMU_GYRY_SIGN 1
#define IMU_GYRZ_SIGN 1
-#define MOTOR_FRONTLEFT_BASEADDR XPAR_PWM_SIGNAL_OUT_3_S_AXI_BASEADDR
-#define MOTOR_FRONTRIGHT_BASEADDR XPAR_PWM_SIGNAL_OUT_1_S_AXI_BASEADDR
-#define MOTOR_BACKLEFT_BASEADDR XPAR_PWM_SIGNAL_OUT_2_S_AXI_BASEADDR
-#define MOTOR_BACKRIGHT_BASEADDR XPAR_PWM_SIGNAL_OUT_0_S_AXI_BASEADDR
-#define ACCEL_X_BIAS 0.023f
-#define ACCEL_Y_BIAS 0.009f
-#define ACCEL_Z_BIAS 0.0686f
+#define MOTOR_FRONTLEFT_BASEADDR XPAR_PWM_SIGNAL_OUT_3_S_AXI_BASEADDR
+#define MOTOR_FRONTRIGHT_BASEADDR XPAR_PWM_SIGNAL_OUT_1_S_AXI_BASEADDR
+#define MOTOR_BACKLEFT_BASEADDR XPAR_PWM_SIGNAL_OUT_2_S_AXI_BASEADDR
+#define MOTOR_BACKRIGHT_BASEADDR XPAR_PWM_SIGNAL_OUT_0_S_AXI_BASEADDR
+
+#define ACCEL_X_BIAS 0.08f
+#define ACCEL_Y_BIAS -0.057f
+#define ACCEL_Z_BIAS 0.0f
-#define GYRO_X_BIAS 0.005f
-#define GYRO_Y_BIAS -0.014f
-#define GYRO_Z_BIAS 0.0534//0.0541f
+#define GYRO_X_BIAS -0.006f
+#define GYRO_Y_BIAS 0.0545f
+#define GYRO_Z_BIAS 0.005//0.0541f
#endif
@@ -68,13 +69,13 @@
#define MOTOR_BACKLEFT_BASEADDR XPAR_PWM_SIGNAL_OUT_2_S_AXI_BASEADDR
#define MOTOR_BACKRIGHT_BASEADDR XPAR_PWM_SIGNAL_OUT_0_S_AXI_BASEADDR
-#define ACCEL_X_BIAS -0.005f
-#define ACCEL_Y_BIAS 0.03f
+#define ACCEL_X_BIAS -0.01f
+#define ACCEL_Y_BIAS 0.041f
#define ACCEL_Z_BIAS 0.0f
-#define GYRO_X_BIAS -0.001f
-#define GYRO_Y_BIAS -0.0135f
-#define GYRO_Z_BIAS 0.035//0.0541f
+#define GYRO_X_BIAS -0.024f
+#define GYRO_Y_BIAS -0.013f
+#define GYRO_Z_BIAS 0.02//0.0541f
#endif
diff --git a/quad/xsdk_workspace_vivado/real_quad/src/hw_impl_zybo_imu.c b/quad/xsdk_workspace_vivado/real_quad/src/hw_impl_zybo_imu.c
index bf8bfeb2abce0485da4f03723be6a10b9e3a6a0b..5cf36172d7440dd44057acf6ad1c62026d0e56a8 100644
--- a/quad/xsdk_workspace_vivado/real_quad/src/hw_impl_zybo_imu.c
+++ b/quad/xsdk_workspace_vivado/real_quad/src/hw_impl_zybo_imu.c
@@ -144,6 +144,9 @@ int zybo_imu_read(struct IMUDriver *self, gam_t *gam) {
raw_accel_x = sensor_raw_accel_y;
raw_accel_y = -sensor_raw_accel_x;
raw_accel_z = sensor_raw_accel_z;
+// raw_accel_x = sensor_raw_accel_y;
+// raw_accel_y = sensor_raw_accel_x;
+// raw_accel_z = sensor_raw_accel_z;
// put in G's
gam->accel_x = IMU_ACCX_SIGN * (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
@@ -159,6 +162,9 @@ int zybo_imu_read(struct IMUDriver *self, gam_t *gam) {
gyro_x = sensor_gyro_y;
gyro_y = -sensor_gyro_x;
gyro_z = sensor_gyro_z;
+// gyro_x = sensor_gyro_y;
+// gyro_y = sensor_gyro_x;
+// gyro_z = sensor_gyro_z;
//Get the number of degrees
//javey: converted to radians to following SI units
diff --git a/quad/xsdk_workspace_vivado/real_quad/src/hw_impl_zybo_lidar.c b/quad/xsdk_workspace_vivado/real_quad/src/hw_impl_zybo_lidar.c
index 8b4fe996034d0ce2a66fcd1f69530e2298fb510f..62caca7532156520b9cd8a05df605de7bd18d123 100644
--- a/quad/xsdk_workspace_vivado/real_quad/src/hw_impl_zybo_lidar.c
+++ b/quad/xsdk_workspace_vivado/real_quad/src/hw_impl_zybo_lidar.c
@@ -18,7 +18,7 @@ int zybo_lidar_reset(struct LidarDriver *self, lidar_t *lidar) {
int error = 0;
//Device wakeup if asleep.
lidarlite_write(i2c, 0x00, 0x00);
-
+ usleep(15000);
// Device Reset & Wake up with default settings
error = lidarlite_write(i2c, 0x00, 0x00);
if (error) return error;
diff --git a/quad/xsdk_workspace_vivado/real_quad/src/main.c b/quad/xsdk_workspace_vivado/real_quad/src/main.c
index ab116d90b27b5011b605f68e6f9cf322bdfda03d..0bbe1ed3cbd0be5f9a98b491e07428c1af3b9a65 100644
--- a/quad/xsdk_workspace_vivado/real_quad/src/main.c
+++ b/quad/xsdk_workspace_vivado/real_quad/src/main.c
@@ -49,6 +49,7 @@ int main()
//test_zybo_mio7_led_and_system();
//!test_zybo_i2c();
//!test_zybo_i2c_imu();
+ zybo_i2c_imu_self_test();
//!test_zybo_i2c_px4flow();
//!test_zybo_i2c_lidar();
//!test_zybo_i2c_all();
@@ -59,7 +60,6 @@ int main()
return 0;
#endif
- usleep(2*1000000);
// Run the main quad application
quad_main(setup_hardware);
diff --git a/quad/xsdk_workspace_vivado/system_bsp/.cproject b/quad/xsdk_workspace_vivado/system_bsp/.cproject
index 8ca4d5f8b1a9d9c24f915b38f1444fd532781eb0..20bc66eca62b633b9c6459543730f5560d0c56f7 100644
--- a/quad/xsdk_workspace_vivado/system_bsp/.cproject
+++ b/quad/xsdk_workspace_vivado/system_bsp/.cproject
@@ -1,8 +1,8 @@
<?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.1532584570">
- <storageModule buildSystemId="org.eclipse.cdt.core.defaultConfigDataProvider" id="org.eclipse.cdt.core.default.config.1532584570" moduleId="org.eclipse.cdt.core.settings" name="Configuration">
+ <cconfiguration id="org.eclipse.cdt.core.default.config.1229250672">
+ <storageModule buildSystemId="org.eclipse.cdt.core.defaultConfigDataProvider" id="org.eclipse.cdt.core.default.config.1229250672" moduleId="org.eclipse.cdt.core.settings" name="Configuration">
<externalSettings/>
<extensions/>
</storageModule>
@@ -10,10 +10,4 @@
</cconfiguration>
</storageModule>
<storageModule moduleId="org.eclipse.cdt.core.LanguageSettingsProviders"/>
- <storageModule moduleId="org.eclipse.cdt.make.core.buildtargets"/>
- <storageModule moduleId="scannerConfiguration"/>
- <storageModule moduleId="org.eclipse.cdt.core.pathentry">
- <pathentry kind="src" path=""/>
- <pathentry kind="out" path=""/>
- </storageModule>
</cproject>
diff --git a/quad/xsdk_workspace_vivado/system_bsp/.project b/quad/xsdk_workspace_vivado/system_bsp/.project
index 55ab2caf3f47d646d65b0f04a248954f54fb928b..1773c9f5fef32c05d645b542adf3376c708cb66a 100644
--- a/quad/xsdk_workspace_vivado/system_bsp/.project
+++ b/quad/xsdk_workspace_vivado/system_bsp/.project
@@ -24,10 +24,6 @@
<key>org.eclipse.cdt.make.core.build.command</key>
<value>make</value>
</dictionary>
- <dictionary>
- <key>org.eclipse.cdt.make.core.build.location</key>
- <value></value>
- </dictionary>
<dictionary>
<key>org.eclipse.cdt.make.core.build.target.auto</key>
<value>all</value>
diff --git a/quad/xsdk_workspace_vivado/system_bsp/.sdkproject b/quad/xsdk_workspace_vivado/system_bsp/.sdkproject
index ddc03c1e87e6437c14b4ebc865dc30f39d77fc7c..dd82e57dd062516fb69638e72b1609d20aa13292 100644
--- a/quad/xsdk_workspace_vivado/system_bsp/.sdkproject
+++ b/quad/xsdk_workspace_vivado/system_bsp/.sdkproject
@@ -1,4 +1,4 @@
THIRPARTY=false
+HW_PROJECT_REFERENCE=design_1_wrapper_hw_platform_0
PROCESSOR=ps7_cortexa9_0
-HW_PROJECT_REFERENCE=design_1_wrapper_hw_platform_1
MSS_FILE=system.mss
diff --git a/quad/xsdk_workspace_vivado/system_bsp/ps7_cortexa9_0/include/xparameters.h b/quad/xsdk_workspace_vivado/system_bsp/ps7_cortexa9_0/include/xparameters.h
index f1700485ebe3a0b73e3583f424872c53dc802160..7fa149261c84b349f2992182d0dcd155d3e8bdae 100644
--- a/quad/xsdk_workspace_vivado/system_bsp/ps7_cortexa9_0/include/xparameters.h
+++ b/quad/xsdk_workspace_vivado/system_bsp/ps7_cortexa9_0/include/xparameters.h
@@ -622,4 +622,11 @@
/******************************************************************/
+/* Xilinx FAT File System Library (XilFFs) User Settings */
+#define FILE_SYSTEM_INTERFACE_SD
+#define FILE_SYSTEM_USE_MKFS
+#define FILE_SYSTEM_NUM_LOGIC_VOL 2
+#define FILE_SYSTEM_USE_STRFUNC 0
+#define FILE_SYSTEM_SET_FS_RPATH 0
+#define FILE_SYSTEM_WORD_ACCESS
#endif /* end of protection macro */
diff --git a/quad/xsdk_workspace_vivado/system_bsp/ps7_cortexa9_0/libsrc/standalone_v6_7/src/bspconfig.h b/quad/xsdk_workspace_vivado/system_bsp/ps7_cortexa9_0/libsrc/standalone_v6_7/src/bspconfig.h
index b977241ce0281c8722d789ce6adf66e27d21a8ae..7859b0eef626654613361272366cfb818a2a0bab 100644
--- a/quad/xsdk_workspace_vivado/system_bsp/ps7_cortexa9_0/libsrc/standalone_v6_7/src/bspconfig.h
+++ b/quad/xsdk_workspace_vivado/system_bsp/ps7_cortexa9_0/libsrc/standalone_v6_7/src/bspconfig.h
@@ -5,7 +5,7 @@
* Version:
* DO NOT EDIT.
*
-* Copyright (C) 2010-2018 Xilinx, Inc. All Rights Reserved.*
+* Copyright (C) 2010-2019 Xilinx, Inc. All Rights Reserved.*
*Permission is hereby granted, free of charge, to any person obtaining a copy
*of this software and associated documentation files (the Software), to deal
*in the Software without restriction, including without limitation the rights
diff --git a/quad/xsdk_workspace_vivado/system_bsp/system.mss b/quad/xsdk_workspace_vivado/system_bsp/system.mss
index 76803d91aa096c0a5ad8ef675de49e7682a0d508..ecb14d2adb87bf499ef16182745346911cb91be6 100644
--- a/quad/xsdk_workspace_vivado/system_bsp/system.mss
+++ b/quad/xsdk_workspace_vivado/system_bsp/system.mss
@@ -325,3 +325,10 @@ BEGIN DRIVER
END
+BEGIN LIBRARY
+ PARAMETER LIBRARY_NAME = xilffs
+ PARAMETER LIBRARY_VER = 3.9
+ PARAMETER PROC_INSTANCE = ps7_cortexa9_0
+END
+
+