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 368adf3c4e51db81c1a4760bd4a282a4b909a38d..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,10 +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) {
@@ -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,
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 0d669fb66a69ed1898314e6a3a31970d40528155..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
@@ -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;
}