Merge branch 'optical_flow' into lidar_integration

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;
+}

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*/

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

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) {
@@ -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,

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.
  *

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();