diff --git a/quad/sw/modular_quad_pid/src/iic_utils.c b/quad/sw/modular_quad_pid/src/iic_utils.c
index be2c0f7e720d22b915d5b61c3864007e750147a1..38d2db8dd3ccf19cee789c270544ec924ea891cb 100644
--- a/quad/sw/modular_quad_pid/src/iic_utils.c
+++ b/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;
}
diff --git a/quad/sw/modular_quad_pid/src/iic_utils.h b/quad/sw/modular_quad_pid/src/iic_utils.h
index 891e349460998fb7134bdb758aab28b98b28ae03..8a8a9a3bbb37bccf40a44a80efe69719358377dd 100644
--- a/quad/sw/modular_quad_pid/src/iic_utils.h
+++ b/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*/
diff --git a/quad/sw/modular_quad_pid/src/initialize_components.c b/quad/sw/modular_quad_pid/src/initialize_components.c
index a3fcd616e0b23580a9443b64ed8aa3f318820280..9f62885187110efd6fa4f779274491e39d59349e 100644
--- a/quad/sw/modular_quad_pid/src/initialize_components.c
+++ b/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;
}
diff --git a/quad/sw/modular_quad_pid/src/type_def.h b/quad/sw/modular_quad_pid/src/type_def.h
index c6f89b496581f5241b2bfa392d50f1fbda319fd5..0f610762a183e81aefb078895ab3b823f1b702c1 100644
--- a/quad/sw/modular_quad_pid/src/type_def.h
+++ b/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