quad: Added optical flow driver and test. I2C bus frequency was lowered

quad/src/quad_app/iic_utils.c

@@ -244,3 +244,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

@@ -128,4 +128,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 {
 	unsigned short distance_cm;
 } 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"
 
 #define IO_CLK_CONTROL_REG_ADDR	(0xF800012C)
 
@@ -45,7 +46,16 @@ 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);
+  }
+  return error;
 }
 
 int zybo_i2c_read(struct I2CDriver *self,
@@ -53,7 +63,16 @@ 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(inst, buff, length, device_addr);
+  if (device_addr == PX4FLOW_DEVICE_ADDR) {
+	  // Put it back to 400kHz
+	  XIicPs_SetSClk(inst, 400000);
+  }
+  return error;
 }
 
 /*****************************************************************************/

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,54 @@ 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;
+
+  for(;;) {
+    unsigned int delay = 5;
+
+    sys.sleep(&sys, delay * 1000);
+
+    iic0_px4flow_update(&of, delay / 1000.);
+    iic0_mpu9150_read_gam(&gam);
+    iic0_lidarlite_read_distance(&lidar);
+  }
+  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();