Merge branch 'master' of https://git.ece.iastate.edu/danc/MicroCART_17-18

quad/Makefile

@@ -44,9 +44,6 @@ test: all
 
 clean:
 	rm -rf $(INCDIR) $(LIBDIR) $(OUTDIR) $(EXEDIR)
-
-deep-clean:
-	make clean
 	$(MAKE) -C src/test clean
 	$(MAKE) -C src/queue clean
 	$(MAKE) -C src/computation_graph clean

quad/executable.mk

@@ -36,7 +36,7 @@ $(TARGET): $(OBJECTS) | $(EXEDIR)
 	$(GCC) -g -o $(TARGET) $^ -I$(INCDIR) -L$(LIBDIR) $(REQLIBS)
 
 $(OBJDIR)/%.o : %.c | $(OBJDIR) $(INCDIR)
-	$(GCC) -c -g -o $@ $< -I$(INCDIR)
+	$(GCC) -c -g -o $@ $< -I$(INCDIR) -Wall
 
 $(OBJDIR):
 	mkdir $(OBJDIR)

quad/library.mk

@@ -37,7 +37,7 @@ $(TARGET): $(OBJECTS) | $(LIBDIR)
 	$(AR) rcs $@ $^
 
 $(OBJDIR)/%.o : %.c | $(OBJDIR) $(INCDIR)
-	$(GCC) -c -g -o $@ $< -I$(INCDIR)
+	$(GCC) -c -g -o $@ $< -I$(INCDIR) -Wall
 
 $(INCDIR)/%.h : %.h | $(INCDIR)
 	cp $^ $(INCDIR)

quad/src/gen_diagram/network.dot

@@ -31,7 +31,7 @@ label="<f0>Yaw Rate PID  |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f
 "Yaw PID" -> "Yaw Rate PID":f2 [label="Correction"]
 "Ts_IMU" -> "Yaw Rate PID":f3 [label="Constant"]
 "X pos PID"[shape=record
-label="<f0>X pos PID  |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=-0.550] |<f5> [Ki=-0.007] |<f6> [Kd=0.000] |<f7> [alpha=0.000]"]
+label="<f0>X pos PID  |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=0.550] |<f5> [Ki=0.007] |<f6> [Kd=0.000] |<f7> [alpha=0.000]"]
 "VRPN X" -> "X pos PID":f1 [label="Constant"]
 "X Setpoint" -> "X pos PID":f2 [label="Constant"]
 "Ts_VRPN" -> "X pos PID":f3 [label="Constant"]
@@ -41,7 +41,7 @@ label="<f0>Y pos PID  |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4>
 "Y Setpoint" -> "Y pos PID":f2 [label="Constant"]
 "Ts_VRPN" -> "Y pos PID":f3 [label="Constant"]
 "Altitude PID"[shape=record
-label="<f0>Altitude PID  |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=-9804.000] |<f5> [Ki=-817.000] |<f6> [Kd=-7353.000] |<f7> [alpha=0.000]"]
+label="<f0>Altitude PID  |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=-9804.000] |<f5> [Ki=-817.000] |<f6> [Kd=-7353.000] |<f7> [alpha=0.880]"]
 "VRPN Alt" -> "Altitude PID":f1 [label="Constant"]
 "Alt Setpoint" -> "Altitude PID":f2 [label="Constant"]
 "Ts_VRPN" -> "Altitude PID":f3 [label="Constant"]
@@ -65,6 +65,8 @@ label="<f0>Pitch  |<f1> [Constant=0.000]"]
 label="<f0>Roll  |<f1> [Constant=0.000]"]
 "Yaw"[shape=record
 label="<f0>Yaw  |<f1> [Constant=0.000]"]
+"Lidar"[shape=record
+label="<f0>Lidar  |<f1> [Constant=0.000]"]
 "Pitch trim"[shape=record
 label="<f0>Pitch trim  |<f1> [Constant=0.045]"]
 "Pitch trim add"[shape=record
@@ -105,22 +107,22 @@ label="<f0>RC Yaw  |<f1> [Constant=0.000]"]
 "RC Throttle"[shape=record
 label="<f0>RC Throttle  |<f1> [Constant=0.000]"]
 "X Vel PID"[shape=record
-label="<f0>X Vel PID  |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=0.100] |<f5> [Ki=0.000] |<f6> [Kd=0.020] |<f7> [alpha=0.880]"]
+label="<f0>X Vel PID  |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=-0.100] |<f5> [Ki=0.000] |<f6> [Kd=-0.020] |<f7> [alpha=0.000]"]
 "X Vel" -> "X Vel PID":f1 [label="Correction"]
 "X Vel Clamp" -> "X Vel PID":f2 [label="Bounded"]
 "Ts_VRPN" -> "X Vel PID":f3 [label="Constant"]
 "Y Vel PID"[shape=record
-label="<f0>Y Vel PID  |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=-0.100] |<f5> [Ki=0.000] |<f6> [Kd=-0.020] |<f7> [alpha=0.880]"]
+label="<f0>Y Vel PID  |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=0.100] |<f5> [Ki=0.000] |<f6> [Kd=0.020] |<f7> [alpha=0.000]"]
 "Y Vel" -> "Y Vel PID":f1 [label="Correction"]
 "Y vel Clamp" -> "Y Vel PID":f2 [label="Bounded"]
 "Ts_VRPN" -> "Y Vel PID":f3 [label="Constant"]
 "X Vel"[shape=record
-label="<f0>X Vel  |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=0.000] |<f5> [Ki=0.000] |<f6> [Kd=-1.000] |<f7> [alpha=0.000]"]
+label="<f0>X Vel  |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=0.000] |<f5> [Ki=0.000] |<f6> [Kd=-1.000] |<f7> [alpha=0.880]"]
 "VRPN X" -> "X Vel":f1 [label="Constant"]
 "zero" -> "X Vel":f2 [label="Constant"]
 "Ts_VRPN" -> "X Vel":f3 [label="Constant"]
 "Y Vel"[shape=record
-label="<f0>Y Vel  |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=0.000] |<f5> [Ki=0.000] |<f6> [Kd=-1.000] |<f7> [alpha=0.000]"]
+label="<f0>Y Vel  |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=0.000] |<f5> [Ki=0.000] |<f6> [Kd=-1.000] |<f7> [alpha=0.880]"]
 "VRPN Y" -> "Y Vel":f1 [label="Constant"]
 "zero" -> "Y Vel":f2 [label="Constant"]
 "Ts_VRPN" -> "Y Vel":f3 [label="Constant"]

quad/src/quad_app/control_algorithm.c

@@ -13,6 +13,8 @@
 #include "util.h"
 #include "timer.h"
 
+//#define USE_LIDAR
+
 #define ROLL_PITCH_MAX_ANGLE 0.35 // 20 degrees
 #define PWM_DIFF_BOUNDS 20000
 #define VRPN_REFRESH_TIME 0.01f // 10ms
@@ -61,6 +63,7 @@ int control_algorithm_init(parameter_t * ps)
     ps->cur_pitch = graph_add_defined_block(graph, BLOCK_CONSTANT, "Pitch"); // ID 15
     ps->cur_roll = graph_add_defined_block(graph, BLOCK_CONSTANT, "Roll");
     ps->cur_yaw = graph_add_defined_block(graph, BLOCK_CONSTANT, "Yaw");
+    ps->lidar = graph_add_defined_block(graph, BLOCK_CONSTANT, "Lidar");
     // Sensor trims
     ps->pitch_trim = graph_add_defined_block(graph, BLOCK_CONSTANT, "Pitch trim");
     ps->pitch_trim_add = graph_add_defined_block(graph, BLOCK_ADD, "Pitch trim add");
@@ -160,11 +163,16 @@ int control_algorithm_init(parameter_t * ps)
     graph_set_source(graph, ps->y_pos_pid, PID_DT, ps->pos_time, CONST_VAL);
     graph_set_source(graph, ps->y_pos_pid, PID_CUR_POINT, ps->vrpn_y, CONST_VAL);
     graph_set_source(graph, ps->y_pos_pid, PID_SETPOINT, ps->y_set, CONST_VAL);
-    // Alt position
+
+    // Alt autonomous
+#ifdef USE_LIDAR
+    graph_set_source(graph, ps->alt_pid, PID_DT, ps->angle_time, CONST_VAL);
+    graph_set_source(graph, ps->alt_pid, PID_CUR_POINT, ps->lidar, CONST_VAL);
+#else
     graph_set_source(graph, ps->alt_pid, PID_DT, ps->pos_time, CONST_VAL);
     graph_set_source(graph, ps->alt_pid, PID_CUR_POINT, ps->vrpn_alt, CONST_VAL);
+#endif
     graph_set_source(graph, ps->alt_pid, PID_SETPOINT, ps->alt_set, CONST_VAL);
-    graph_set_source(graph, ps->alt_set, CONST_VAL, ps->vrpn_alt, CONST_VAL);
     graph_set_source(graph, ps->throttle_trim_add, ADD_SUMMAND1, ps->alt_pid, PID_CORRECTION);
     graph_set_source(graph, ps->throttle_trim_add, ADD_SUMMAND2, ps->throttle_trim, CONST_VAL);
     // Yaw angle
@@ -343,6 +351,7 @@ int control_algorithm_init(parameter_t * ps)
     graph_set_param_val(graph, ps->gyro_y, CONST_SET, sensor_struct->gyr_y);
     graph_set_param_val(graph, ps->gyro_x, CONST_SET, sensor_struct->gyr_x);
     graph_set_param_val(graph, ps->gyro_z, CONST_SET, sensor_struct->gyr_z);
+    graph_set_param_val(graph, ps->lidar, CONST_SET, sensor_struct->lidar_altitude);
 
     // RC values
     graph_set_param_val(graph, ps->rc_pitch, CONST_SET, user_input_struct->pitch_angle_manual_setpoint);
@@ -351,8 +360,8 @@ int control_algorithm_init(parameter_t * ps)
     graph_set_param_val(graph, ps->rc_throttle, CONST_SET, user_input_struct->rc_commands[THROTTLE]);
 
     // Compute VRPN blocks so they can be logged
-    int outputs[4] = {ps->mixer, ps->vrpn_alt, ps->vrpn_pitch, ps->vrpn_roll};
-    graph_compute_nodes(graph, outputs, 4);
+    int outputs[5] = {ps->mixer, ps->vrpn_alt, ps->vrpn_pitch, ps->vrpn_roll, ps->lidar};
+    graph_compute_nodes(graph, outputs, 5);
 
 	 // here for now so in case any flight command is not PID controlled, it will default to rc_command value:
 	//memcpy(raw_actuator_struct->controller_corrected_motor_commands, user_input_struct->rc_commands, sizeof(int) * 6);

quad/src/quad_app/iic_utils.c

@@ -240,7 +240,90 @@ int iic0_lidarlite_read_distance(lidar_t *lidar) {
 
 	// Read the sensor value
 	status = iic0_lidarlite_read(buf, 0x8f, 2);
-	lidar->distance_cm = buf[0] << 8 | buf[1];
+	float distance_cm = (float)(buf[0] << 8 | buf[1]) - LIDAR_OFFSET;
+	lidar->distance_m = 0.01 * distance_cm;
+
+	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

@@ -119,6 +119,7 @@ int iic0_mpu9150_read_gam(gam_t* gam);
 ////////////////////////////////////////////////////////////////////////////////////////////
 
 #define LIDARLITE_DEVICE_ADDR		0x62
+#define LIDAR_OFFSET 0.02 // Distance from LiDAR sensor to ground, in meters
 
 int iic0_lidarlite_write(u8 register_addr, u8 data);
 int iic0_lidarlite_read(u8* recv_buffer, u8 register_addr, int size);
@@ -128,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/log_data.c

@@ -113,6 +113,7 @@ void initialize_logging(log_t* log_struct, parameter_t* ps) {
 	addOutputToLog(log_struct, ps->vrpn_alt, CONST_VAL, m);
 	addOutputToLog(log_struct, ps->vrpn_pitch, CONST_VAL, rad);
 	addOutputToLog(log_struct, ps->vrpn_roll, CONST_VAL, rad);
+	addOutputToLog(log_struct, ps->lidar, CONST_VAL, m);
 	addOutputToLog(log_struct, ps->x_set, CONST_VAL, m);
 	addOutputToLog(log_struct, ps->y_set, CONST_VAL, m);
 	addOutputToLog(log_struct, ps->alt_set, CONST_VAL, m);

quad/src/quad_app/sensor.c

@@ -8,11 +8,17 @@
 #include "sensor.h"
 #include "communication.h"
 #include "commands.h"
+#include "type_def.h"
 
 int sensor_init(raw_sensor_t * raw_sensor_struct, sensor_t * sensor_struct)
 {
-	if(iic0_mpu9150_start() == -1)
+	if (iic0_lidarlite_init()) { // init LiDAR
 		return -1;
+	}
+
+	if(iic0_mpu9150_start() == -1) {
+		return -1;
+	}
 
 	// read sensor board and fill in gryo/accelerometer/magnetometer struct
 	iic0_mpu9150_read_gam(&(raw_sensor_struct->gam));
@@ -67,6 +73,10 @@ int get_sensors(log_t* log_struct, user_input_t* user_input_struct, raw_sensor_t
 
 	log_struct->gam = raw_sensor_struct->gam;
 
+	static lidar_t lidar_val;
+	iic0_lidarlite_read_distance(&lidar_val);
+	raw_sensor_struct->lidar_distance_m = lidar_val.distance_m;
+
     return 0;
 }
  

quad/src/quad_app/sensor_processing.c

@@ -117,16 +117,9 @@ int sensor_processing(log_t* log_struct, user_input_t *user_input_struct, raw_se
 	sensor_struct->roll_angle_filtered = ALPHA * (sensor_struct->roll_angle_filtered + sensor_struct->phi_dot* get_last_loop_time())
 			+ (1. - ALPHA) * accel_roll;
 
-//	static int loop_counter = 0;
-//	loop_counter++;
-//
-//	if(loop_counter == 50)
-//	{
-//		char dMsg[100] = {};
-//		sprintf(dMsg, "Loop time: %.4f\n", LOOP_TIME);
-//		uart0_sendStr(dMsg);
-//		loop_counter = 0;
-//	}
+
+	// Z-axis points upward, so negate distance
+	sensor_struct->lidar_altitude = -raw_sensor_struct->lidar_distance_m;
 	return 0;
 }
 

quad/src/quad_app/type_def.h

@@ -112,9 +112,14 @@ typedef struct {
 }gam_t;
 
 typedef struct {
-	unsigned short distance_cm;
+	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
@@ -239,6 +244,10 @@ typedef struct raw_sensor {
 	// Structures to hold the current quad position & orientation
 	quadPosition_t currentQuadPosition;
 
+	// Distance from the ground, in meters, that the quadcopter is.
+	// Ideally equals 0 when landed
+	float lidar_distance_m;
+
 } raw_sensor_t;
 
 /**
@@ -266,11 +275,13 @@ typedef struct sensor {
 	float gyr_z;		// gyroscope z data
 	int gyr_z_t;	// time of gyroscope z data
 
-	int ldr_z;		//lidar z data (altitude)
-	int ldr_z_t;	//time of lidar z data
 
+	// Complementary filter outputs
 	float pitch_angle_filtered;
 	float roll_angle_filtered;
+
+	// Z-axis value obtained from LiDAR
+	// Note that this is not distance, as our Z-axis points upwards.
 	float lidar_altitude;
 
 	float phi_dot, theta_dot, psi_dot;
@@ -319,6 +330,7 @@ typedef struct parameter_t {
 	int gyro_y;
 	int gyro_x;
 	int gyro_z;
+	int lidar;
 	// VRPN blocks
 	int vrpn_x;
 	int vrpn_y;

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.
@@ -62,10 +59,10 @@ int test_zybo_i2c() {
   lidar_t lidar = { };
   iic_set_globals(&i2c, &sys);
   if (iic0_lidarlite_init()) return 0;
-  short x;
+  float x;
   while (1) {
     iic0_lidarlite_read_distance(&lidar);
-    x = lidar.distance_cm;
+    x = lidar.distance_m;
   }
   return 0;
 }
@@ -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_m > 50) {
+    	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();