From d0b6a9a54370332d72ed67186b32c6743f010547 Mon Sep 17 00:00:00 2001
From: Eric Middleton <ericm@iastate.edu>
Date: Thu, 23 Mar 2017 19:40:07 -0500
Subject: [PATCH] quad: Added optical flow driver and test. I2C bus frequency
was lowered
---
quad/src/quad_app/iic_utils.c | 82 +++++++++++++++++++
quad/src/quad_app/iic_utils.h | 16 ++++
quad/src/quad_app/type_def.h | 5 ++
.../real_quad/src/hw_impl_zybo_i2c.c | 23 +++++-
.../real_quad/src/hw_impl_zybo_tests.c | 53 +++++++++++-
quad/xsdk_workspace/real_quad/src/main.c | 2 +
6 files changed, 175 insertions(+), 6 deletions(-)
diff --git a/quad/src/quad_app/iic_utils.c b/quad/src/quad_app/iic_utils.c
index a9fbd272c..44af3357c 100644
--- a/quad/src/quad_app/iic_utils.c
+++ b/quad/src/quad_app/iic_utils.c
@@ -244,3 +244,85 @@ int iic0_lidarlite_read_distance(lidar_t *lidar) {
return status;
}
+
+//////////////////////////////////////////////////
+// PX4FLOW
+//////////////////////////////////////////////////
+
+//TODO: Replace device-specific iic0_device_write/read with generic ones
+
+int iic0_px4flow_write(u8 register_addr, u8 data) {
+ u8 buf[] = {register_addr, data};
+
+ return i2c->write(i2c, PX4FLOW_DEVICE_ADDR, buf, 2);
+}
+
+int iic0_px4flow_read(u8* recv_buffer, u8 register_addr, int size) {
+ u8 buf[] = {register_addr};
+ int status = 0;
+
+ status |= i2c->write(i2c, PX4FLOW_DEVICE_ADDR, buf, 1);
+ status |= i2c->read(i2c, PX4FLOW_DEVICE_ADDR, recv_buffer, size);
+ return status;
+}
+
+int iic0_px4flow_init(px4flow_t *of) {
+ //Clear struct
+ of->xPos = 0;
+ of->yPos = 0;
+ of->xVel = 0;
+ of->yVel = 0;
+
+ //Perform initial update
+ return iic0_px4flow_update(of, 0.);
+}
+
+int iic0_px4flow_update(px4flow_t *of, double dt) {
+ static double time = 0.;
+
+ time += dt;
+ if(time >= 10.) {
+ time = 0.;
+ }
+
+ int status = 0;
+
+ struct {
+ uint16_t frameCount;
+
+ int16_t pixelFlowXSum;
+ int16_t pixelFlowYSum;
+ int16_t flowCompX;
+ int16_t flowCompY;
+ int16_t qual;
+
+ int16_t gyroXRate;
+ int16_t gyroYRate;
+ int16_t gyroZRate;
+
+ uint8_t gyroRange;
+ uint8_t sonarTimestamp;
+ int16_t groundDistance;
+ } i2cFrame;
+
+ u8 buf[sizeof(i2cFrame)];
+
+ // Read the sensor value
+ status = iic0_px4flow_read(buf, 0x00, sizeof(i2cFrame));
+
+ if(status == 0) {
+ //Copy into struct
+ memcpy(&i2cFrame, buf, sizeof(i2cFrame));
+
+ //As per documentation, disregard frames with low quality, as they contain unreliable data
+ if(i2cFrame.qual >= PX4FLOW_QUAL_MIN) {
+ of->xVel = i2cFrame.flowCompX / 1000.;
+ of->yVel = i2cFrame.flowCompY / 1000.;
+
+ of->xPos += of->xVel * dt;
+ of->yPos += of->yVel * dt;
+ }
+ }
+
+ return status;
+}
diff --git a/quad/src/quad_app/iic_utils.h b/quad/src/quad_app/iic_utils.h
index b1ccb6861..ccf88dde0 100644
--- a/quad/src/quad_app/iic_utils.h
+++ b/quad/src/quad_app/iic_utils.h
@@ -128,4 +128,20 @@ int iic0_lidarlite_read_distance(lidar_t *lidar);
int iic0_lidarlite_init();
int iic0_lidarlite_sleep();
+////////////////////////////////////////////////////////////////////////////////////////////
+// PX4FLOW optical flow sensor functions/defines (based on information on from pixhawk.org)
+////////////////////////////////////////////////////////////////////////////////////////////
+
+#define PX4FLOW_DEVICE_ADDR 0x42
+#define PX4FLOW_QUAL_MIN (100)
+
+
+int iic0_px4flow_write(u8 register_addr, u8 data);
+int iic0_px4flow_read(u8* recv_buffer, u8 register_addr, int size);
+
+int iic0_px4flow_update(px4flow_t *of, double dt);
+
+int iic0_px4flow_init(px4flow_t *of);
+
+
#endif /*IIC_UTILS_H*/
diff --git a/quad/src/quad_app/type_def.h b/quad/src/quad_app/type_def.h
index d01613443..9f1ef52bf 100644
--- a/quad/src/quad_app/type_def.h
+++ b/quad/src/quad_app/type_def.h
@@ -115,6 +115,11 @@ typedef struct {
unsigned short distance_cm;
} lidar_t;
+typedef struct {
+ double xVel, yVel;
+ double xPos, yPos;
+} px4flow_t;
+
typedef struct PID_t {
float current_point; // Current value of the system
float setpoint; // Desired value of the system
diff --git a/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_i2c.c b/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_i2c.c
index c24a2b7a7..368adf3c4 100644
--- a/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_i2c.c
+++ b/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_i2c.c
@@ -1,4 +1,5 @@
#include "hw_impl_zybo.h"
+#include "iic_utils.h"
#define IO_CLK_CONTROL_REG_ADDR (0xF800012C)
@@ -45,7 +46,16 @@ int zybo_i2c_write(struct I2CDriver *self,
unsigned char *data,
unsigned int length) {
XIicPs *inst = self->state;
- return XIicPs_MasterSendPolled_ours(inst, data, length, device_addr);
+ if (device_addr == PX4FLOW_DEVICE_ADDR) {
+ // If we are sending a request to optical flow, drop down to 100kHz
+ XIicPs_SetSClk(inst, 100000);
+ }
+ int error = XIicPs_MasterSendPolled_ours(inst, data, length, device_addr);
+ if (device_addr == PX4FLOW_DEVICE_ADDR) {
+ // Put it back to 400kHz
+ XIicPs_SetSClk(inst, 400000);
+ }
+ return error;
}
int zybo_i2c_read(struct I2CDriver *self,
@@ -53,7 +63,16 @@ int zybo_i2c_read(struct I2CDriver *self,
unsigned char *buff,
unsigned int length) {
XIicPs *inst = self->state;
- return XIicPs_MasterRecvPolled(inst, buff, length, device_addr);
+ if (device_addr == PX4FLOW_DEVICE_ADDR) {
+ // If we are sending a request to optical flow, drop down to 100kHz
+ XIicPs_SetSClk(inst, 100000);
+ }
+ int error = XIicPs_MasterRecvPolled(inst, buff, length, device_addr);
+ if (device_addr == PX4FLOW_DEVICE_ADDR) {
+ // Put it back to 400kHz
+ XIicPs_SetSClk(inst, 400000);
+ }
+ return error;
}
/*****************************************************************************/
diff --git a/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_tests.c b/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_tests.c
index 2f5e5ab43..0d669fb66 100644
--- a/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_tests.c
+++ b/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_tests.c
@@ -33,10 +33,7 @@ int test_zybo_mio7_led_and_system() {
}
/**
- * Test for the I2CDriver.
- *
- * Raw I2C tests are tedius, so we are going to cheat a bit, and use
- * some of the LIDAR code we have.
+ * Tests for the I2CDriver (one for each I2C device we use)
*
* Instructions:
* 1) Connect Zybo Board to computer by USB cable.
@@ -86,6 +83,54 @@ int test_zybo_i2c_imu() {
return 0;
}
+int test_zybo_i2c_px4flow() {
+ struct I2CDriver i2c = create_zybo_i2c();
+ struct SystemDriver sys = create_zybo_system();
+ i2c.reset(&i2c);
+ sys.reset(&sys);
+
+ px4flow_t of;
+ iic_set_globals(&i2c, &sys);
+
+ if(iic0_px4flow_init(&of)) return 0;
+
+ for(;;) {
+ unsigned int delay = 5;
+
+ sys.sleep(&sys, delay * 1000);
+
+ iic0_px4flow_update(&of, delay / 1000.);
+ }
+ return 0;
+}
+
+int test_zybo_i2c_all() {
+ struct I2CDriver i2c = create_zybo_i2c();
+ struct SystemDriver sys = create_zybo_system();
+ i2c.reset(&i2c);
+ sys.reset(&sys);
+
+ lidar_t lidar = { };
+ px4flow_t of = { };
+ gam_t gam = { };
+ iic_set_globals(&i2c, &sys);
+
+ if (iic0_px4flow_init(&of)) return 0;
+ if (iic0_mpu9150_start()) return 0;
+ if (iic0_lidarlite_init()) return 0;
+
+ for(;;) {
+ unsigned int delay = 5;
+
+ sys.sleep(&sys, delay * 1000);
+
+ iic0_px4flow_update(&of, delay / 1000.);
+ iic0_mpu9150_read_gam(&gam);
+ iic0_lidarlite_read_distance(&lidar);
+ }
+ return 0;
+}
+
/**
* Test for the PWMInputDriver.
*
diff --git a/quad/xsdk_workspace/real_quad/src/main.c b/quad/xsdk_workspace/real_quad/src/main.c
index 337212a68..ef3971feb 100644
--- a/quad/xsdk_workspace/real_quad/src/main.c
+++ b/quad/xsdk_workspace/real_quad/src/main.c
@@ -27,6 +27,8 @@ int main()
//test_zybo_mio7_led_and_system();
//test_zybo_i2c();
//test_zybo_i2c_imu();
+ //test_zybo_i2c_px4flow();
+ //test_zybo_i2c_all();
//test_zybo_pwm_inputs();
//test_zybo_pwm_outputs();
//test_zybo_uart();
--
GitLab