Using iic read "ours" now.

quad/xsdk_workspace/real_quad/src/hw_impl_zybo_i2c.c

 #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) {
@@ -55,6 +58,7 @@ int zybo_i2c_write(struct I2CDriver *self,
 	  // Put it back to 400kHz
 	  XIicPs_SetSClk(inst, 400000);
   }
+  usleep(5);
   return error;
 }
 
@@ -67,11 +71,12 @@ int zybo_i2c_read(struct I2CDriver *self,
 	  // 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);
+  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;
 }
 
@@ -188,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,

quad/xsdk_workspace/real_quad/src/hw_impl_zybo_tests.c

@@ -119,14 +119,30 @@ int test_zybo_i2c_all() {
   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);
 
-    iic0_px4flow_update(&of, 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;
 }