diff --git a/quad/src/quad_app/iic_utils.c b/quad/src/quad_app/iic_utils.c
index a023e48095229d08433b7de84931a8dcb49654d3..035085dfe81ad94f37648888e26bbe329c97d372 100644
--- a/quad/src/quad_app/iic_utils.c
+++ b/quad/src/quad_app/iic_utils.c
@@ -245,3 +245,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 1bfd0ae0993f51283586ec8428f882991d3cfa39..637c9d7ab3b564be33b6ffa2922e3ba1e4ddaadd 100644
--- a/quad/src/quad_app/iic_utils.h
+++ b/quad/src/quad_app/iic_utils.h
@@ -129,4 +129,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 952f317a7f22fa3933a4300675ec395f6b2696fb..d77d8eb938a13f1a64e44aedf838c395ecabe667 100644
--- a/quad/src/quad_app/type_def.h
+++ b/quad/src/quad_app/type_def.h
@@ -115,6 +115,11 @@ typedef struct {
float distance_m; // distance in meters
} 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 c24a2b7a706db55469e50b56584120a1def9701a..7748962c3f12530595b6664d64e3a712c9a9993e 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,9 +1,13 @@
#include "hw_impl_zybo.h"
+#include "iic_utils.h"
+#include "xiicps.h"
#define IO_CLK_CONTROL_REG_ADDR (0xF800012C)
int XIicPs_MasterSendPolled_ours(XIicPs *InstancePtr, u8 *MsgPtr,
int ByteCount, u16 SlaveAddr);
+int XIicPs_MasterRecvPolled_ours(XIicPs *InstancePtr, u8 *MsgPtr,
+ int ByteCount, u16 SlaveAddr);
int XIicPs_SetupMaster(XIicPs *InstancePtr, int Role);
int zybo_i2c_reset(struct I2CDriver *self) {
@@ -45,7 +49,17 @@ 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);
+ }
+ usleep(5);
+ return error;
}
int zybo_i2c_read(struct I2CDriver *self,
@@ -53,7 +67,17 @@ 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_ours(inst, buff, length, device_addr);
+ if (device_addr == PX4FLOW_DEVICE_ADDR) {
+ // Put it back to 400kHz
+ XIicPs_SetSClk(inst, 400000);
+ }
+ usleep(5);
+ return error;
}
/*****************************************************************************/
@@ -169,6 +193,176 @@ int XIicPs_MasterSendPolled_ours(XIicPs *InstancePtr, u8 *MsgPtr,
return XST_SUCCESS;
}
+/*****************************************************************************/
+/**
+* This function initiates a polled mode receive in master mode.
+*
+* It repeatedly sets the transfer size register so the slave can
+* send data to us. It polls the data register for data to come in.
+* If slave fails to send us data, it fails with time out.
+*
+* @param InstancePtr is a pointer to the XIicPs instance.
+* @param MsgPtr is the pointer to the receive buffer.
+* @param ByteCount is the number of bytes to be received.
+* @param SlaveAddr is the address of the slave we are receiving from.
+*
+* @return
+* - XST_SUCCESS if everything went well.
+* - XST_FAILURE if timed out.
+*
+* @note This receive routine is for polled mode transfer only.
+*
+****************************************************************************/
+int XIicPs_MasterRecvPolled_ours(XIicPs *InstancePtr, u8 *MsgPtr,
+ int ByteCount, u16 SlaveAddr)
+{
+ u32 IntrStatusReg;
+ u32 Intrs;
+ u32 StatusReg;
+ u32 BaseAddr;
+ int BytesToRecv;
+ int BytesToRead;
+ int TransSize;
+ int Tmp;
+
+ /*
+ * Assert validates the input arguments.
+ */
+ Xil_AssertNonvoid(InstancePtr != NULL);
+ Xil_AssertNonvoid(MsgPtr != NULL);
+ Xil_AssertNonvoid(InstancePtr->IsReady == XIL_COMPONENT_IS_READY);
+ Xil_AssertNonvoid(XIICPS_ADDR_MASK >= SlaveAddr);
+
+ BaseAddr = InstancePtr->Config.BaseAddress;
+ InstancePtr->RecvBufferPtr = MsgPtr;
+ InstancePtr->RecvByteCount = ByteCount;
+
+ XIicPs_SetupMaster(InstancePtr, RECVING_ROLE);
+
+ XIicPs_WriteReg(BaseAddr, XIICPS_ADDR_OFFSET, SlaveAddr);
+
+ /*
+ * Intrs keeps all the error-related interrupts.
+ */
+ Intrs = XIICPS_IXR_ARB_LOST_MASK | XIICPS_IXR_RX_OVR_MASK |
+ XIICPS_IXR_RX_UNF_MASK | XIICPS_IXR_TO_MASK |
+ XIICPS_IXR_NACK_MASK;
+
+ /*
+ * Clear the interrupt status register before use it to monitor.
+ */
+ IntrStatusReg = XIicPs_ReadReg(BaseAddr, XIICPS_ISR_OFFSET);
+ XIicPs_WriteReg(BaseAddr, XIICPS_ISR_OFFSET, IntrStatusReg);
+
+ /*
+ * Set up the transfer size register so the slave knows how much
+ * to send to us.
+ */
+ if (ByteCount > XIICPS_FIFO_DEPTH) {
+ XIicPs_WriteReg(BaseAddr, XIICPS_TRANS_SIZE_OFFSET,
+ XIICPS_FIFO_DEPTH);
+ }else {
+ XIicPs_WriteReg(BaseAddr, XIICPS_TRANS_SIZE_OFFSET,
+ ByteCount);
+ }
+
+ /*
+ * Pull the interrupt status register to find the errors.
+ */
+ IntrStatusReg = XIicPs_ReadReg(BaseAddr, XIICPS_ISR_OFFSET);
+ while ((InstancePtr->RecvByteCount > 0) &&
+ ((IntrStatusReg & Intrs) == 0) && !(IntrStatusReg & XIICPS_IXR_COMP_MASK)) {
+ StatusReg = XIicPs_ReadReg(BaseAddr, XIICPS_SR_OFFSET);
+
+ /*
+ * If there is no data in the FIFO, check the interrupt
+ * status register for error, and continue.
+ */
+ if ((StatusReg & XIICPS_SR_RXDV_MASK) == 0) {
+ IntrStatusReg = XIicPs_ReadReg(BaseAddr,
+ XIICPS_ISR_OFFSET);
+ continue;
+ }
+
+ /*
+ * The transfer size register shows how much more data slave
+ * needs to send to us.
+ */
+ TransSize = XIicPs_ReadReg(BaseAddr,
+ XIICPS_TRANS_SIZE_OFFSET);
+
+ BytesToRead = InstancePtr->RecvByteCount;
+
+ /*
+ * If expected number of bytes is greater than FIFO size,
+ * the master needs to wait for data comes in and set the
+ * transfer size register for slave to send more.
+ */
+ if (InstancePtr->RecvByteCount > XIICPS_FIFO_DEPTH) {
+ /* wait slave to send data */
+ while ((TransSize > 2) &&
+ ((IntrStatusReg & Intrs) == 0)) {
+ TransSize = XIicPs_ReadReg(BaseAddr,
+ XIICPS_TRANS_SIZE_OFFSET);
+ IntrStatusReg = XIicPs_ReadReg(BaseAddr,
+ XIICPS_ISR_OFFSET);
+ }
+
+ /*
+ * If timeout happened, it is an error.
+ */
+ if (IntrStatusReg & XIICPS_IXR_TO_MASK) {
+ return XST_FAILURE;
+ }
+ TransSize = XIicPs_ReadReg(BaseAddr,
+ XIICPS_TRANS_SIZE_OFFSET);
+
+ /*
+ * Take trans size into account of how many more should
+ * be received.
+ */
+ BytesToRecv = InstancePtr->RecvByteCount -
+ XIICPS_FIFO_DEPTH + TransSize;
+
+ /* Tell slave to send more to us */
+ if (BytesToRecv > XIICPS_FIFO_DEPTH) {
+ XIicPs_WriteReg(BaseAddr,
+ XIICPS_TRANS_SIZE_OFFSET,
+ XIICPS_FIFO_DEPTH);
+ } else{
+ XIicPs_WriteReg(BaseAddr,
+ XIICPS_TRANS_SIZE_OFFSET, BytesToRecv);
+ }
+
+ BytesToRead = XIICPS_FIFO_DEPTH - TransSize;
+ }
+
+ Tmp = 0;
+ IntrStatusReg = XIicPs_ReadReg(BaseAddr, XIICPS_ISR_OFFSET);
+ while ((Tmp < BytesToRead) &&
+ ((IntrStatusReg & Intrs) == 0)) {
+ StatusReg = XIicPs_ReadReg(BaseAddr,
+ XIICPS_SR_OFFSET);
+ IntrStatusReg = XIicPs_ReadReg(BaseAddr,
+ XIICPS_ISR_OFFSET);
+
+ if ((StatusReg & XIICPS_SR_RXDV_MASK) == 0) {
+ /* No data in fifo */
+ continue;
+ }
+ XIicPs_RecvByte(InstancePtr);
+ Tmp ++;
+ }
+ }
+
+ if ((IntrStatusReg & Intrs) || InstancePtr->RecvByteCount > 0) {
+ return XST_FAILURE;
+ }
+
+ return XST_SUCCESS;
+}
+
+
/*****************************************************************************/
/*
* NOTE to MicroCART: This function is required by the send polling method above,
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 2f5e5ab43e06ae7091ba3b0b1eba5d66ffbf0e7c..eb0597de8277eb2d2e1bd327c4ac739e8fe9e0fb 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,70 @@ 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;
+
+ int lidarErrors = 0;
+ int gamErrors = 0;
+ int nLoops = 0;
+ int of_errors = 0;
+
+ for(;;) {
+ unsigned int delay = 5;
+
+ sys.sleep(&sys, delay * 1000);
+
+ if (iic0_px4flow_update(&of, delay / 1000.)) {
+ of_errors += 1;
+ }
+
+ iic0_mpu9150_read_gam(&gam);
+ iic0_lidarlite_read_distance(&lidar);
+
+ if (lidar.distance_cm > 5000) {
+ lidarErrors += 1;
+ }
+ if (gam.accel_z > -0.8) {
+ gamErrors += 1;
+ }
+ nLoops += 1;
+ }
+ 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 337212a68b5c4829b95de861b9a97f08d4bb93be..ef3971feb9c63c531073afa30651d7cb9f3e9959 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();