quad: implement iic functions for lidar

quad/sw/modular_quad_pid/src/iic_utils.c

@@ -18,7 +18,7 @@ XIicPs_Config* i2c_config;
 XIicPs I2C0;
 double magX_correction = -1, magY_correction, magZ_correction;
 
-int init_iic0(){
+int iic0_init(){
 
 	//Make sure CPU_1x clk is enabled for I2C controller
 	Xuint16* aper_ctrl = (Xuint16*) IO_CLK_CONTROL_REG_ADDR;
@@ -230,5 +230,52 @@ int iic0_mpu9150_read_gam(gam_t* gam) {
 	gam->gyro_zVel_r = ((gyro_z / GYRO_SENS) * DEG_TO_RAD) + GYRO_Z_BIAS;
 
 	return 0;
+}
+
+//////////////////////////////////////////////////
+// LIDAR
+//////////////////////////////////////////////////
+
+int iic0_lidarlite_write(u8 register_addr, u8 data) {
+	u8 buf[] = {register_addr, data};
+
+	return XIicPs_MasterSendPolled(&I2C0, buf, 2, LIDARLITE_DEVICE_ADDR);
+}
+
+int iic0_lidarlite_read(u8* recv_buffer, u8 register_addr, int size) {
+	u8 buf[] = {register_addr};
+	int status = 0;
+
+	status = XIicPs_MasterSendPolled(&I2C0, buf, 1, LIDARLITE_DEVICE_ADDR);
+	status |= XIicPs_MasterRecvPolled(&I2C0, recv_buffer,size, LIDARLITE_DEVICE_ADDR);
+	return status;
+}
+
+int iic0_lidarlite_init() {
+	int status = 0;
+
+	// Device Reset & Wake up with default settings
+	status = iic0_lidarlite_write(0x00, 0x00);
+	usleep(15000);
+
+	// Enable Free Running Mode and distance measurements with correction
+	status |= iic0_lidarlite_write(0x11, 0xff);
+	status |= iic0_lidarlite_write(0x00, 0x04);
+
+	return status;
+}
+
+int iic0_lidarlite_sleep() {
+	return iic0_lidarlite_write(0x65, 0x84);
+}
+
+int iic0_lidarlite_read_distance(lidar_t *lidar) {
+	u8 buf[2];
+	int status = 0;
+
+	// Read the sensor value
+	status = iic0_lidarlite_read(buf, 0x8f, 2);
+	lidar->distance_cm = buf[0] << 8 | buf[1];
 
+	return status;
 }

quad/sw/modular_quad_pid/src/iic_utils.h

@@ -34,9 +34,26 @@
 #define IIC0_INTR_EN			    (0xE0004024)
 #define IIC0_TIMEOUT_REG_ADDR 		(0xE000401C)
 
-//
+//Interrupt Status Register Masks
+#define ARB_LOST       (0x200)
+#define RX_UNF          (0x80)
+#define TX_OVF          (0x40)
+#define RX_OVF          (0x20)
+#define SLV_RDY         (0x10)
+#define TIME_OUT        (0x08)
+#define NACK      		(0x04)
+#define MORE_DAT 	    (0x02)
+#define TRANS_COMPLETE 	(0x01)
+
+#define WRITE_INTR_MASK (ARB_LOST | TIME_OUT | RX_OVF | TX_OVF | NACK)
+#define READ_INTR_MASK (ARB_LOST | TIME_OUT | RX_OVF | RX_UNF | NACK)
+
+// Initialize hardware; Call this FIRST before calling any other functions
+int iic0_init();
+
+///////////////////////////////////////////////////////////////////////////////
 // MPU9150 Sensor Defines (Address is defined on the Sparkfun MPU9150 Datasheet)
-//
+///////////////////////////////////////////////////////////////////////////////
 
 #define MPU9150_DEVICE_ADDR 		0b01101000
 #define MPU9150_COMPASS_ADDR 		0x0C
@@ -72,21 +89,6 @@
 #define MAG_Z_L 4
 #define MAG_Z_H 5
 
-//Interrupt Status Register Masks
-#define ARB_LOST       (0x200)
-#define RX_UNF          (0x80)
-#define TX_OVF          (0x40)
-#define RX_OVF          (0x20)
-#define SLV_RDY         (0x10)
-#define TIME_OUT        (0x08)
-#define NACK      		(0x04)
-#define MORE_DAT 	    (0x02)
-#define TRANS_COMPLETE 	(0x01)
-
-#define WRITE_INTR_MASK (ARB_LOST | TIME_OUT | RX_OVF | TX_OVF | NACK)
-#define READ_INTR_MASK (ARB_LOST | TIME_OUT | RX_OVF | RX_UNF | NACK)
-
-
 // Gyro is configured for +/-2000dps
 // Sensitivity gain is based off MPU9150 datasheet (pg. 11)
 #define GYRO_SENS 16.4
@@ -99,13 +101,9 @@
 #define ACCEL_Y_BIAS	0.009f
 #define ACCEL_Z_BIAS	0.087f
 
-// Initialize hardware; Call this FIRST before calling any other functions
-int init_iic0();
-
 void iic0_mpu9150_write(u8 register_addr, u8 data);
 void iic0_mpu9150_read(u8* recv_buffer, u8 register_addr, int size);
 
-
 // Wake up the MPU for data collection
 // Configure Gyro/Accel/Mag
 int iic0_mpu9150_start();
@@ -119,4 +117,18 @@ void iic0_mpu9150_read_gyro_accel(gam_t* gam);
 
 int iic0_mpu9150_read_gam(gam_t* gam);
 
+////////////////////////////////////////////////////////////////////////////////////////////
+// LIDAR lite sensor defines (Addressing and registers specified on LIDAR lite v2 datasheet)
+////////////////////////////////////////////////////////////////////////////////////////////
+
+#define LIDARLITE_DEVICE_ADDR		0x62
+
+int iic0_lidarlite_write(u8 register_addr, u8 data);
+int iic0_lidarlite_read(u8* recv_buffer, u8 register_addr, int size);
+
+int iic0_lidarlite_read_distance(lidar_t *lidar);
+
+int iic0_lidarlite_init();
+int iic0_lidarlite_sleep();
+
 #endif /*IIC_UTILS_H*/

quad/sw/modular_quad_pid/src/initialize_components.c

@@ -64,7 +64,7 @@ int initializeAllComponents(user_input_t * user_input_struct, log_t * log_struct
 	}
 
 	// Initialize I2C controller and start the sensor board
-	if (init_iic0() == -1) {
+	if (iic0_init() == -1) {
 		return -1;
 	}
 

quad/sw/modular_quad_pid/src/type_def.h

@@ -107,6 +107,10 @@ typedef struct {
 
 }gam_t;
 
+typedef struct {
+	unsigned short distance_cm;
+} lidar_t;
+
 typedef struct PID_t {
 	float current_point;	// Current value of the system
 	float setpoint;		// Desired value of the system