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 8f439cdf1872f1da5bcd0638d76a152f58c27f8f..08b654cfb010a35c1066e4de567bc9335c4e2a68 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
@@ -26,6 +26,57 @@
#define MAX_UART_BUFFER_SIZE 2048
+#define QUAD_NUM 1
+
+#if QUAD_NUM==0
+#define IMU_ACCX_SIGN 1
+#define IMU_ACCY_SIGN 1
+#define IMU_ACCZ_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 GYRO_X_BIAS 0.005f
+#define GYRO_Y_BIAS -0.014f
+#define GYRO_Z_BIAS 0.0534//0.0541f
+
+#endif
+
+#if QUAD_NUM==1
+
+#define IMU_ACCX_SIGN -1
+#define IMU_ACCY_SIGN -1
+#define IMU_ACCZ_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_0_S_AXI_BASEADDR
+#define MOTOR_FRONTRIGHT_BASEADDR XPAR_PWM_SIGNAL_OUT_2_S_AXI_BASEADDR
+#define MOTOR_BACKLEFT_BASEADDR XPAR_PWM_SIGNAL_OUT_3_S_AXI_BASEADDR
+#define MOTOR_BACKRIGHT_BASEADDR XPAR_PWM_SIGNAL_OUT_1_S_AXI_BASEADDR
+
+#define ACCEL_X_BIAS 0.023f
+#define ACCEL_Y_BIAS -0.104f
+#define ACCEL_Z_BIAS 0.0686f
+
+#define GYRO_X_BIAS -0.001f
+#define GYRO_Y_BIAS 0.014f
+#define GYRO_Z_BIAS -0.033//0.0541f
+
+#endif
+
struct ZyboI2CState {
XIicPs *inst;
int busId;
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 31b68f49d66c93896cb02f9f17afa4fe37f4cb34..2b5abff2555418e9ea8e80c1ab97863fb66583b7 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
@@ -44,14 +44,6 @@
// 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.0534//0.0541f
-
-#define ACCEL_X_BIAS 0.023f
-#define ACCEL_Y_BIAS 0.009f
-#define ACCEL_Z_BIAS 0.0686f
-
int mpu9150_write(struct I2CDriver *i2c, u8 register_addr, u8 data);
int mpu9150_read(struct I2CDriver *i2c, u8* recv_buffer, u8 register_addr, int size);
@@ -147,9 +139,9 @@ int zybo_imu_read(struct IMUDriver *self, gam_t *gam) {
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;
+ 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
+ gam->accel_y = IMU_ACCY_SIGN * (raw_accel_y / 4096.0) + ACCEL_Y_BIAS;
+ gam->accel_z = IMU_ACCZ_SIGN * (raw_accel_z / 4096.0) + ACCEL_Z_BIAS;
//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;
@@ -158,9 +150,9 @@ int zybo_imu_read(struct IMUDriver *self, gam_t *gam) {
//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;
+ gam->gyro_xVel_p = IMU_GYRX_SIGN * ((gyro_x / GYRO_SENS) * DEG_TO_RAD) + GYRO_X_BIAS;
+ gam->gyro_yVel_q = IMU_GYRX_SIGN * ((gyro_y / GYRO_SENS) * DEG_TO_RAD) + GYRO_Y_BIAS;
+ gam->gyro_zVel_r = IMU_GYRX_SIGN * ((gyro_z / GYRO_SENS) * DEG_TO_RAD) + GYRO_Z_BIAS;
// Magnometer
mpu9150_read_mag(self, gam);
diff --git a/quad/xsdk_workspace_vivado/real_quad/src/hw_impl_zybo_motor.c b/quad/xsdk_workspace_vivado/real_quad/src/hw_impl_zybo_motor.c
index 8e8e27ab70d1983c3dffeef7207516dc998e0aec..869bb7973be25a2cc2e004c02a900b339949a46a 100644
--- a/quad/xsdk_workspace_vivado/real_quad/src/hw_impl_zybo_motor.c
+++ b/quad/xsdk_workspace_vivado/real_quad/src/hw_impl_zybo_motor.c
@@ -20,10 +20,10 @@ int zybo_motor_reset(struct MotorDriver *self) {
}
struct MotorDriverState *state = self->state;
- state->outputs[0] = (int *) XPAR_PWM_SIGNAL_OUT_0_S_AXI_BASEADDR;
- state->outputs[1] = (int *) XPAR_PWM_SIGNAL_OUT_1_S_AXI_BASEADDR;
- state->outputs[2] = (int *) XPAR_PWM_SIGNAL_OUT_2_S_AXI_BASEADDR;
- state->outputs[3] = (int *) XPAR_PWM_SIGNAL_OUT_3_S_AXI_BASEADDR;
+ state->outputs[0] = (int *) MOTOR_BACKRIGHT_BASEADDR;
+ state->outputs[1] = (int *) MOTOR_FRONTRIGHT_BASEADDR;
+ state->outputs[2] = (int *) MOTOR_BACKLEFT_BASEADDR;
+ state->outputs[3] = (int *) MOTOR_FRONTLEFT_BASEADDR;
// Set period width of PWM pulse
*(state->outputs[0]) = PERIOD_WIDTH;
diff --git a/quad/xsdk_workspace_vivado/system_bsp/.cproject b/quad/xsdk_workspace_vivado/system_bsp/.cproject
index 86a0aeef4132bacd2853a6303480855eacbfb07e..6bfbc95beafba37a0f6c75224604a1429f5c2eac 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.1135924200">
- <storageModule buildSystemId="org.eclipse.cdt.core.defaultConfigDataProvider" id="org.eclipse.cdt.core.default.config.1135924200" moduleId="org.eclipse.cdt.core.settings" name="Configuration">
+ <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">
<externalSettings/>
<extensions/>
</storageModule>
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 50527d62c00d9d25c43226a9dfb8c6be9e879294..f1700485ebe3a0b73e3583f424872c53dc802160 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
@@ -222,65 +222,65 @@
/* Definitions for driver GPIO */
#define XPAR_XGPIO_NUM_INSTANCES 4
-/* Definitions for peripheral AXI_GPIO_0 */
-#define XPAR_AXI_GPIO_0_BASEADDR 0x41200000
-#define XPAR_AXI_GPIO_0_HIGHADDR 0x4120FFFF
-#define XPAR_AXI_GPIO_0_DEVICE_ID 0
-#define XPAR_AXI_GPIO_0_INTERRUPT_PRESENT 0
-#define XPAR_AXI_GPIO_0_IS_DUAL 0
+/* Definitions for peripheral AXI_GPIO_3 */
+#define XPAR_AXI_GPIO_3_BASEADDR 0x41230000
+#define XPAR_AXI_GPIO_3_HIGHADDR 0x4123FFFF
+#define XPAR_AXI_GPIO_3_DEVICE_ID 0
+#define XPAR_AXI_GPIO_3_INTERRUPT_PRESENT 0
+#define XPAR_AXI_GPIO_3_IS_DUAL 0
-/* Definitions for peripheral AXI_GPIO_1 */
-#define XPAR_AXI_GPIO_1_BASEADDR 0x41210000
-#define XPAR_AXI_GPIO_1_HIGHADDR 0x4121FFFF
-#define XPAR_AXI_GPIO_1_DEVICE_ID 1
-#define XPAR_AXI_GPIO_1_INTERRUPT_PRESENT 0
-#define XPAR_AXI_GPIO_1_IS_DUAL 0
+/* Definitions for peripheral AXI_GPIO_BTNS */
+#define XPAR_AXI_GPIO_BTNS_BASEADDR 0x41220000
+#define XPAR_AXI_GPIO_BTNS_HIGHADDR 0x4122FFFF
+#define XPAR_AXI_GPIO_BTNS_DEVICE_ID 1
+#define XPAR_AXI_GPIO_BTNS_INTERRUPT_PRESENT 0
+#define XPAR_AXI_GPIO_BTNS_IS_DUAL 0
-/* Definitions for peripheral AXI_GPIO_2 */
-#define XPAR_AXI_GPIO_2_BASEADDR 0x41220000
-#define XPAR_AXI_GPIO_2_HIGHADDR 0x4122FFFF
-#define XPAR_AXI_GPIO_2_DEVICE_ID 2
-#define XPAR_AXI_GPIO_2_INTERRUPT_PRESENT 0
-#define XPAR_AXI_GPIO_2_IS_DUAL 0
+/* Definitions for peripheral AXI_GPIO_LEDS */
+#define XPAR_AXI_GPIO_LEDS_BASEADDR 0x41210000
+#define XPAR_AXI_GPIO_LEDS_HIGHADDR 0x4121FFFF
+#define XPAR_AXI_GPIO_LEDS_DEVICE_ID 2
+#define XPAR_AXI_GPIO_LEDS_INTERRUPT_PRESENT 0
+#define XPAR_AXI_GPIO_LEDS_IS_DUAL 0
-/* Definitions for peripheral AXI_GPIO_3 */
-#define XPAR_AXI_GPIO_3_BASEADDR 0x41230000
-#define XPAR_AXI_GPIO_3_HIGHADDR 0x4123FFFF
-#define XPAR_AXI_GPIO_3_DEVICE_ID 3
-#define XPAR_AXI_GPIO_3_INTERRUPT_PRESENT 0
-#define XPAR_AXI_GPIO_3_IS_DUAL 0
+/* Definitions for peripheral AXI_GPIO_SWS */
+#define XPAR_AXI_GPIO_SWS_BASEADDR 0x41200000
+#define XPAR_AXI_GPIO_SWS_HIGHADDR 0x4120FFFF
+#define XPAR_AXI_GPIO_SWS_DEVICE_ID 3
+#define XPAR_AXI_GPIO_SWS_INTERRUPT_PRESENT 0
+#define XPAR_AXI_GPIO_SWS_IS_DUAL 0
/******************************************************************/
-/* Canonical definitions for peripheral AXI_GPIO_0 */
-#define XPAR_GPIO_0_BASEADDR 0x41200000
-#define XPAR_GPIO_0_HIGHADDR 0x4120FFFF
-#define XPAR_GPIO_0_DEVICE_ID XPAR_AXI_GPIO_0_DEVICE_ID
+/* Canonical definitions for peripheral AXI_GPIO_3 */
+#define XPAR_GPIO_0_BASEADDR 0x41230000
+#define XPAR_GPIO_0_HIGHADDR 0x4123FFFF
+#define XPAR_GPIO_0_DEVICE_ID XPAR_AXI_GPIO_3_DEVICE_ID
#define XPAR_GPIO_0_INTERRUPT_PRESENT 0
#define XPAR_GPIO_0_IS_DUAL 0
-/* Canonical definitions for peripheral AXI_GPIO_1 */
-#define XPAR_GPIO_1_BASEADDR 0x41210000
-#define XPAR_GPIO_1_HIGHADDR 0x4121FFFF
-#define XPAR_GPIO_1_DEVICE_ID XPAR_AXI_GPIO_1_DEVICE_ID
+/* Canonical definitions for peripheral AXI_GPIO_BTNS */
+#define XPAR_GPIO_1_BASEADDR 0x41220000
+#define XPAR_GPIO_1_HIGHADDR 0x4122FFFF
+#define XPAR_GPIO_1_DEVICE_ID XPAR_AXI_GPIO_BTNS_DEVICE_ID
#define XPAR_GPIO_1_INTERRUPT_PRESENT 0
#define XPAR_GPIO_1_IS_DUAL 0
-/* Canonical definitions for peripheral AXI_GPIO_2 */
-#define XPAR_GPIO_2_BASEADDR 0x41220000
-#define XPAR_GPIO_2_HIGHADDR 0x4122FFFF
-#define XPAR_GPIO_2_DEVICE_ID XPAR_AXI_GPIO_2_DEVICE_ID
+/* Canonical definitions for peripheral AXI_GPIO_LEDS */
+#define XPAR_GPIO_2_BASEADDR 0x41210000
+#define XPAR_GPIO_2_HIGHADDR 0x4121FFFF
+#define XPAR_GPIO_2_DEVICE_ID XPAR_AXI_GPIO_LEDS_DEVICE_ID
#define XPAR_GPIO_2_INTERRUPT_PRESENT 0
#define XPAR_GPIO_2_IS_DUAL 0
-/* Canonical definitions for peripheral AXI_GPIO_3 */
-#define XPAR_GPIO_3_BASEADDR 0x41230000
-#define XPAR_GPIO_3_HIGHADDR 0x4123FFFF
-#define XPAR_GPIO_3_DEVICE_ID XPAR_AXI_GPIO_3_DEVICE_ID
+/* Canonical definitions for peripheral AXI_GPIO_SWS */
+#define XPAR_GPIO_3_BASEADDR 0x41200000
+#define XPAR_GPIO_3_HIGHADDR 0x4120FFFF
+#define XPAR_GPIO_3_DEVICE_ID XPAR_AXI_GPIO_SWS_DEVICE_ID
#define XPAR_GPIO_3_INTERRUPT_PRESENT 0
#define XPAR_GPIO_3_IS_DUAL 0
diff --git a/quad/xsdk_workspace_vivado/system_bsp/ps7_cortexa9_0/libsrc/gpio_v4_3/src/xgpio_g.c b/quad/xsdk_workspace_vivado/system_bsp/ps7_cortexa9_0/libsrc/gpio_v4_3/src/xgpio_g.c
index 57aa942dcf9cd0302f8463ec59f7e060e3673a12..0afd3205d9bb30da9d6ce39a7e5142d65f56c7f3 100644
--- a/quad/xsdk_workspace_vivado/system_bsp/ps7_cortexa9_0/libsrc/gpio_v4_3/src/xgpio_g.c
+++ b/quad/xsdk_workspace_vivado/system_bsp/ps7_cortexa9_0/libsrc/gpio_v4_3/src/xgpio_g.c
@@ -47,28 +47,28 @@
XGpio_Config XGpio_ConfigTable[XPAR_XGPIO_NUM_INSTANCES] =
{
{
- XPAR_AXI_GPIO_0_DEVICE_ID,
- XPAR_AXI_GPIO_0_BASEADDR,
- XPAR_AXI_GPIO_0_INTERRUPT_PRESENT,
- XPAR_AXI_GPIO_0_IS_DUAL
+ XPAR_AXI_GPIO_3_DEVICE_ID,
+ XPAR_AXI_GPIO_3_BASEADDR,
+ XPAR_AXI_GPIO_3_INTERRUPT_PRESENT,
+ XPAR_AXI_GPIO_3_IS_DUAL
},
{
- XPAR_AXI_GPIO_1_DEVICE_ID,
- XPAR_AXI_GPIO_1_BASEADDR,
- XPAR_AXI_GPIO_1_INTERRUPT_PRESENT,
- XPAR_AXI_GPIO_1_IS_DUAL
+ XPAR_AXI_GPIO_BTNS_DEVICE_ID,
+ XPAR_AXI_GPIO_BTNS_BASEADDR,
+ XPAR_AXI_GPIO_BTNS_INTERRUPT_PRESENT,
+ XPAR_AXI_GPIO_BTNS_IS_DUAL
},
{
- XPAR_AXI_GPIO_2_DEVICE_ID,
- XPAR_AXI_GPIO_2_BASEADDR,
- XPAR_AXI_GPIO_2_INTERRUPT_PRESENT,
- XPAR_AXI_GPIO_2_IS_DUAL
+ XPAR_AXI_GPIO_LEDS_DEVICE_ID,
+ XPAR_AXI_GPIO_LEDS_BASEADDR,
+ XPAR_AXI_GPIO_LEDS_INTERRUPT_PRESENT,
+ XPAR_AXI_GPIO_LEDS_IS_DUAL
},
{
- XPAR_AXI_GPIO_3_DEVICE_ID,
- XPAR_AXI_GPIO_3_BASEADDR,
- XPAR_AXI_GPIO_3_INTERRUPT_PRESENT,
- XPAR_AXI_GPIO_3_IS_DUAL
+ XPAR_AXI_GPIO_SWS_DEVICE_ID,
+ XPAR_AXI_GPIO_SWS_BASEADDR,
+ XPAR_AXI_GPIO_SWS_INTERRUPT_PRESENT,
+ XPAR_AXI_GPIO_SWS_IS_DUAL
}
};
diff --git a/quad/xsdk_workspace_vivado/system_bsp/system.mss b/quad/xsdk_workspace_vivado/system_bsp/system.mss
index c0e89a892e0014d48f231ed43cd0e31c49a46f8f..76803d91aa096c0a5ad8ef675de49e7682a0d508 100644
--- a/quad/xsdk_workspace_vivado/system_bsp/system.mss
+++ b/quad/xsdk_workspace_vivado/system_bsp/system.mss
@@ -21,25 +21,25 @@ END
BEGIN DRIVER
PARAMETER DRIVER_NAME = gpio
PARAMETER DRIVER_VER = 4.3
- PARAMETER HW_INSTANCE = axi_gpio_0
+ PARAMETER HW_INSTANCE = axi_gpio_3
END
BEGIN DRIVER
PARAMETER DRIVER_NAME = gpio
PARAMETER DRIVER_VER = 4.3
- PARAMETER HW_INSTANCE = axi_gpio_1
+ PARAMETER HW_INSTANCE = axi_gpio_btns
END
BEGIN DRIVER
PARAMETER DRIVER_NAME = gpio
PARAMETER DRIVER_VER = 4.3
- PARAMETER HW_INSTANCE = axi_gpio_2
+ PARAMETER HW_INSTANCE = axi_gpio_leds
END
BEGIN DRIVER
PARAMETER DRIVER_NAME = gpio
PARAMETER DRIVER_VER = 4.3
- PARAMETER HW_INSTANCE = axi_gpio_3
+ PARAMETER HW_INSTANCE = axi_gpio_sws
END
BEGIN DRIVER