From bbedc613ca7b8f42e35df5b3f9450e449cde2735 Mon Sep 17 00:00:00 2001
From: James Talbert <jtalbert@iastate.edu>
Date: Fri, 26 Oct 2018 16:23:34 -0500
Subject: [PATCH] The quad seems close to working (maybe). It takes off mostly
straight, but doesn't stay stable. It's hard to say if this is beacuse of
tuning issues, or a setup problem.
---
.../real_quad/src/hw_impl_zybo.h | 51 ++++++++++++
.../real_quad/src/hw_impl_zybo_imu.c | 20 ++---
.../real_quad/src/hw_impl_zybo_motor.c | 8 +-
.../system_bsp/.cproject | 4 +-
.../ps7_cortexa9_0/include/xparameters.h | 80 +++++++++----------
.../libsrc/gpio_v4_3/src/xgpio_g.c | 32 ++++----
.../system_bsp/system.mss | 8 +-
7 files changed, 123 insertions(+), 80 deletions(-)
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 8f439cdf1..08b654cfb 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 31b68f49d..2b5abff25 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 8e8e27ab7..869bb7973 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 86a0aeef4..6bfbc95be 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 50527d62c..f1700485e 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 57aa942dc..0afd3205d 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 c0e89a892..76803d91a 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
--
GitLab