Merge branch 'quad-impl-lidar' into 'master'

Implement LIDAR functions

Closes #1

See merge request !6

quad/sw/modular_quad_pid/src/controllers.c

@@ -9,7 +9,7 @@
  * Lots of useful information in controllers.h, look in there first
  */
 #include "controllers.h"
-#include "iic_mpu9150_utils.h"
+#include "iic_utils.h"
 #include "quadposition.h"
 #include "util.h"
 #include "uart.h"

quad/sw/modular_quad_pid/src/iic_mpu9150_utils.c → quad/sw/modular_quad_pid/src/iic_utils.c

 /**
- * IIC_MPU9150_UTILS.c
+ * IIC_UTILS.c
  *
- * Utility functions for using I2C on a Diligent Zybo board and
- * focused on the SparkFun MPU9150
- *
- * For function descriptions please see iic_mpu9150_utils.h
- *
- * Author: 	ucart
- * Created: 01/20/2015
+ * Utility functions for using I2C on a Diligent Zybo board.
+ * Supports the SparkFun MPU9150 and the LiDAR lite v2 sensor
  */
 
 #include <stdio.h>
@@ -15,15 +10,18 @@
 #include <math.h>
 
 #include "xparameters.h"
-#include "iic_mpu9150_utils.h"
+#include "iic_utils.h"
 #include "xbasic_types.h"
 #include "xiicps.h"
 
 XIicPs_Config* i2c_config;
 XIicPs I2C0;
 double magX_correction = -1, magY_correction, magZ_correction;
+int XIicPs_MasterSendPolled_ours(XIicPs *InstancePtr, u8 *MsgPtr, int ByteCount, u16 SlaveAddr);
+int XIicPs_SetupMaster(XIicPs *InstancePtr, int Role);
+s32 TransmitFifoFill(XIicPs *InstancePtr);
 
-int initI2C0(){
+int iic0_init(){
 
 	//Make sure CPU_1x clk is enabled for I2C controller
 	Xuint16* aper_ctrl = (Xuint16*) IO_CLK_CONTROL_REG_ADDR;
@@ -56,26 +54,26 @@ int initI2C0(){
 	return 0;
 }
 
-int startMPU9150(){
+int iic0_mpu9150_start(){
 
 	// Device Reset & Wake up
-	iic0Write(0x6B, 0x80);
+	iic0_mpu9150_write(0x6B, 0x80);
 	usleep(5000);
 
 	// Set clock reference to Z Gyro
-	iic0Write(0x6B, 0x03);
+	iic0_mpu9150_write(0x6B, 0x03);
 	// Configure Digital Low/High Pass filter
-	iic0Write(0x1A,0x06); // Level 6 low pass on gyroscope
+	iic0_mpu9150_write(0x1A,0x06); // Level 6 low pass on gyroscope
 
 	// Configure Gyro to 2000dps, Accel. to +/-8G
-	iic0Write(0x1B, 0x18);
-	iic0Write(0x1C, 0x10);
+	iic0_mpu9150_write(0x1B, 0x18);
+	iic0_mpu9150_write(0x1C, 0x10);
 
 	// Enable I2C bypass for AUX I2C (Magnetometer)
-	iic0Write(0x37, 0x02);
+	iic0_mpu9150_write(0x37, 0x02);
 
 	// Setup Mag
-	iic0Write(0x37, 0x02);             //INT_PIN_CFG   -- INT_LEVEL=0 ; INT_OPEN=0 ; LATCH_INT_EN=0 ; INT_RD_CLEAR=0 ; FSYNC_INT_LEVEL=0 ; FSYNC_INT_EN=0 ; I2C_BYPASS_EN=0 ; CLKOUT_EN=0
+	iic0_mpu9150_write(0x37, 0x02);             //INT_PIN_CFG   -- INT_LEVEL=0 ; INT_OPEN=0 ; LATCH_INT_EN=0 ; INT_RD_CLEAR=0 ; FSYNC_INT_LEVEL=0 ; FSYNC_INT_EN=0 ; I2C_BYPASS_EN=0 ; CLKOUT_EN=0
 
 	usleep(100000);
 
@@ -84,7 +82,7 @@ int startMPU9150(){
 
 	// Do about 20 reads to warm up the device
 	for(i=0; i < 20; ++i){
-		if(get_gam_reading(&temp_gam) == -1){
+		if(iic0_mpu9150_read_gam(&temp_gam) == -1){
 			return -1;
 		}
 		usleep(1000);
@@ -93,13 +91,13 @@ int startMPU9150(){
 	return 0;
 }
 
-void stopMPU9150(){
+void iic0_mpu9150_stop(){
 
 	//Put MPU to sleep
-	iic0Write(0x6B, 0b01000000);
+	iic0_mpu9150_write(0x6B, 0b01000000);
 }
 
-void iic0Write(u8 register_addr, u8 data){
+void iic0_mpu9150_write(u8 register_addr, u8 data){
 
 	u16 device_addr = MPU9150_DEVICE_ADDR;
 	u8 buf[] = {register_addr, data};
@@ -113,11 +111,11 @@ void iic0Write(u8 register_addr, u8 data){
 		device_addr = MPU9150_COMPASS_ADDR;
 	}
 
-	XIicPs_MasterSendPolled(&I2C0, buf, 2, device_addr);
+	XIicPs_MasterSendPolled_ours(&I2C0, buf, 2, device_addr);
 
 }
 
-void iic0Read(u8* recv_buffer, u8 register_addr, int size){
+void iic0_mpu9150_read(u8* recv_buffer, u8 register_addr, int size){
 
 	u16 device_addr = MPU9150_DEVICE_ADDR;
 	u8 buf[] = {register_addr};
@@ -132,21 +130,21 @@ void iic0Read(u8* recv_buffer, u8 register_addr, int size){
 	}
 
 
-	XIicPs_MasterSendPolled(&I2C0, buf, 1, device_addr);
+	XIicPs_MasterSendPolled_ours(&I2C0, buf, 1, device_addr);
 	XIicPs_MasterRecvPolled(&I2C0, recv_buffer,size,device_addr);
 }
 
-void CalcMagSensitivity(){
+void iic0_mpu9150_calc_mag_sensitivity(){
 
 	u8 buf[3];
 	u8 ASAX, ASAY, ASAZ;
 
 	// Quickly read from the factory ROM to get correction coefficents
-	iic0Write(0x0A, 0x0F);
+	iic0_mpu9150_write(0x0A, 0x0F);
 	usleep(10000);
 
 	// Read raw adjustment values
-	iic0Read(buf, 0x10,3);
+	iic0_mpu9150_read(buf, 0x10,3);
 	ASAX = buf[0];
 	ASAY = buf[1];
 	ASAZ = buf[2];
@@ -158,28 +156,28 @@ void CalcMagSensitivity(){
 }
 
 
-void ReadMag(gam_t* gam){
+void iic0_mpu9150_read_mag(gam_t* gam){
 
 	u8 mag_data[6];
 	Xint16 raw_magX, raw_magY, raw_magZ;
 
 	// Grab calibrations if not done already
 	if(magX_correction == -1){
-		CalcMagSensitivity();
+		iic0_mpu9150_calc_mag_sensitivity();
 	}
 
 	// Set Mag to single read mode
-	iic0Write(0x0A, 0x01);
+	iic0_mpu9150_write(0x0A, 0x01);
 	usleep(10000);
 	mag_data[0] = 0;
 
 	// Keep checking if data is ready before reading new mag data
 	while(mag_data[0] == 0x00){
-		iic0Read(mag_data, 0x02, 1);
+		iic0_mpu9150_read(mag_data, 0x02, 1);
 	}
 
 	// Get mag data
-	iic0Read(mag_data, 0x03, 6);
+	iic0_mpu9150_read(mag_data, 0x03, 6);
 
 	raw_magX = (mag_data[1] << 8) | mag_data[0];
 	raw_magY = (mag_data[3] << 8) | mag_data[2];
@@ -195,7 +193,7 @@ void ReadMag(gam_t* gam){
 /**
  * Get Gyro Accel Mag (GAM) information
  */
-int get_gam_reading(gam_t* gam) {
+int iic0_mpu9150_read_gam(gam_t* gam) {
 
 	Xint16 raw_accel_x, raw_accel_y, raw_accel_z;
 	Xint16 gyro_x, gyro_y, gyro_z;
@@ -206,7 +204,7 @@ int get_gam_reading(gam_t* gam) {
 	//Xint8 mag_data[6] = {};
 
 	//readHandler = iic0_read_bytes(sensor_data, ACCEL_GYRO_BASE_ADDR, ACCEL_GYRO_READ_SIZE);
-	iic0Read(sensor_data, ACCEL_GYRO_BASE_ADDR, ACCEL_GYRO_READ_SIZE);
+	iic0_mpu9150_read(sensor_data, ACCEL_GYRO_BASE_ADDR, ACCEL_GYRO_READ_SIZE);
 
 	//Calculate accelerometer data
 	raw_accel_x = sensor_data[ACC_X_H] << 8 | sensor_data[ACC_X_L];
@@ -235,5 +233,226 @@ int get_gam_reading(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_ours(&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_ours(&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;
+}
+
+/*****************************************************************************/
+/**
+* NOTE to MicroCART: This function is originally from the Xilinx library,
+* but we noticed that the original function didn't check for a NACK, which
+* would cause the original polling function to enter an infinite loop in the
+* event of a NACK. Notice that we have added that NACK check at the final
+* while loop of this function.
+*
+*
+* This function initiates a polled mode send in master mode.
+*
+* It sends data to the FIFO and waits for the slave to pick them up.
+* If slave fails to remove data from FIFO, the send fails with
+* time out.
+*
+* @param	InstancePtr is a pointer to the XIicPs instance.
+* @param	MsgPtr is the pointer to the send buffer.
+* @param	ByteCount is the number of bytes to be sent.
+* @param	SlaveAddr is the address of the slave we are sending to.
+*
+* @return
+*		- XST_SUCCESS if everything went well.
+*		- XST_FAILURE if timed out.
+*
+* @note		This send routine is for polled mode transfer only.
+*
+****************************************************************************/
+int XIicPs_MasterSendPolled_ours(XIicPs *InstancePtr, u8 *MsgPtr,
+		 int ByteCount, u16 SlaveAddr)
+{
+	u32 IntrStatusReg;
+	u32 StatusReg;
+	u32 BaseAddr;
+	u32 Intrs;
+
+	/*
+	 * 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->SendBufferPtr = MsgPtr;
+	InstancePtr->SendByteCount = ByteCount;
+
+	XIicPs_SetupMaster(InstancePtr, SENDING_ROLE);
+
+	XIicPs_WriteReg(BaseAddr, XIICPS_ADDR_OFFSET, SlaveAddr);
+
+	/*
+	 * Intrs keeps all the error-related interrupts.
+	 */
+	Intrs = XIICPS_IXR_ARB_LOST_MASK | XIICPS_IXR_TX_OVR_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);
+
+	/*
+	 * Transmit first FIFO full of data.
+	 */
+	TransmitFifoFill(InstancePtr);
+
+	IntrStatusReg = XIicPs_ReadReg(BaseAddr, XIICPS_ISR_OFFSET);
+
+	/*
+	 * Continue sending as long as there is more data and
+	 * there are no errors.
+	 */
+	while ((InstancePtr->SendByteCount > 0) &&
+		((IntrStatusReg & Intrs) == 0)) {
+		StatusReg = XIicPs_ReadReg(BaseAddr, XIICPS_SR_OFFSET);
+
+		/*
+		 * Wait until transmit FIFO is empty.
+		 */
+		if ((StatusReg & XIICPS_SR_TXDV_MASK) != 0) {
+			IntrStatusReg = XIicPs_ReadReg(BaseAddr,
+					XIICPS_ISR_OFFSET);
+			continue;
+		}
+
+		/*
+		 * Send more data out through transmit FIFO.
+		 */
+		TransmitFifoFill(InstancePtr);
+	}
+
+	/*
+	 * Check for completion of transfer.
+	 */
+	// NOTE for MicroCART: Corrected function. Original left for reference.
+//	while ((XIicPs_ReadReg(BaseAddr, XIICPS_ISR_OFFSET) &
+//		XIICPS_IXR_COMP_MASK) != XIICPS_IXR_COMP_MASK);
+	while (!(IntrStatusReg & (Intrs | XIICPS_IXR_COMP_MASK))) {
+		IntrStatusReg = XIicPs_ReadReg(BaseAddr, XIICPS_ISR_OFFSET);
+	}
+
+	/*
+	 * If there is an error, tell the caller.
+	 */
+	if (IntrStatusReg & Intrs) {
+		return XST_FAILURE;
+	}
+
+	return XST_SUCCESS;
+}
+
+/*****************************************************************************/
+/*
+* NOTE to MicroCART: This function is required by the send polling method above,
+* but it was originally static, so we had to copy it word-for-word here.
+*
+* This function prepares a device to transfers as a master.
+*
+* @param	InstancePtr is a pointer to the XIicPs instance.
+*
+* @param	Role specifies whether the device is sending or receiving.
+*
+* @return
+*		- XST_SUCCESS if everything went well.
+*		- XST_FAILURE if bus is busy.
+*
+* @note		Interrupts are always disabled, device which needs to use
+*		interrupts needs to setup interrupts after this call.
+*
+****************************************************************************/
+int XIicPs_SetupMaster(XIicPs *InstancePtr, int Role)
+{
+	u32 ControlReg;
+	u32 BaseAddr;
+	u32 EnabledIntr = 0x0;
+
+	Xil_AssertNonvoid(InstancePtr != NULL);
+
+	BaseAddr = InstancePtr->Config.BaseAddress;
+	ControlReg = XIicPs_ReadReg(BaseAddr, XIICPS_CR_OFFSET);
+
+
+	/*
+	 * Only check if bus is busy when repeated start option is not set.
+	 */
+	if ((ControlReg & XIICPS_CR_HOLD_MASK) == 0) {
+		if (XIicPs_BusIsBusy(InstancePtr)) {
+			return XST_FAILURE;
+		}
+	}
+
+	/*
+	 * Set up master, AckEn, nea and also clear fifo.
+	 */
+	ControlReg |= XIICPS_CR_ACKEN_MASK | XIICPS_CR_CLR_FIFO_MASK |
+		 	XIICPS_CR_NEA_MASK | XIICPS_CR_MS_MASK;
+
+	if (Role == RECVING_ROLE) {
+		ControlReg |= XIICPS_CR_RD_WR_MASK;
+		EnabledIntr = XIICPS_IXR_DATA_MASK |XIICPS_IXR_RX_OVR_MASK;
+	}else {
+		ControlReg &= ~XIICPS_CR_RD_WR_MASK;
+	}
+	EnabledIntr |= XIICPS_IXR_COMP_MASK | XIICPS_IXR_ARB_LOST_MASK;
+
+	XIicPs_WriteReg(BaseAddr, XIICPS_CR_OFFSET, ControlReg);
+
+	XIicPs_DisableAllInterrupts(BaseAddr);
 
+	return XST_SUCCESS;
 }

quad/sw/modular_quad_pid/src/iic_mpu9150_utils.h → quad/sw/modular_quad_pid/src/iic_utils.h

@@ -11,8 +11,8 @@
  */
 
 
-#ifndef IIC_MPU9150_UTILS_H
-#define IIC_MPU9150_UTILS_H
+#ifndef IIC_UTILS_H
+#define IIC_UTILS_H
 
 
 #include "xbasic_types.h"
@@ -34,11 +34,30 @@
 #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
 
-
 #define ACCEL_GYRO_READ_SIZE 		14		//Bytes
 #define ACCEL_GYRO_BASE_ADDR		0x3B	//Starting register address
 
@@ -70,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
@@ -97,70 +101,34 @@
 #define ACCEL_Y_BIAS	-0.0045f
 #define ACCEL_Z_BIAS	-0.008f
 
-// Initialize hardware; Call this FIRST before calling any other functions
-int initI2C0();
-
-void iic0Write(u8 register_addr, u8 data);
-void iic0Read(u8* recv_buffer, u8 register_addr, int size);
-
+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 startMPU9150();
-
-// Put MPU back to sleep
-void stopMPU9150();
-
-void CalcMagSensitivity();
-void ReadMag(gam_t* gam);
-void ReadGyroAccel(gam_t* gam);
-
-int get_gam_reading(gam_t* gam);
-
-
-/////////////
-// Deprecated functions below
-/////////////
-
-
-// Initialize hardware; Call this FIRST before calling any other functions
-void init_iic0();
-
-// Wake up the MPU for data collection
-void start_mpu9150();
+int iic0_mpu9150_start();
 
 // Put MPU back to sleep
-void stop_mpu9150();
-
-
-// Write a byte of data at the given register address on the MPU
-void iic0_write(Xuint16 reg_addr, Xuint8 data);
-
-// Read a single byte at a given register address on the MPU
-Xuint8 iic0_read(Xuint16 reg_addr);
-
-// Read multiple bytes consecutively at a starting register address
-// places the resulting bytes in rv
-int iic0_read_bytes(Xuint8* rv, Xuint16 reg_addr, int bytes);
-
-// Helper function to initialize I2C0 controller on the Zybo board
-// Called by init_iic0
-void iic0_hw_init();
-
+void iic0_mpu9150_stop();
 
-// Clears the interrupt status register
-// Called by configuration functions
-void iic0_clear_intr_status();
+void iic0_mpu9150_calc_mag_sensitivity();
+void iic0_mpu9150_read_mag(gam_t* gam);
+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)
+////////////////////////////////////////////////////////////////////////////////////////////
 
-// Configure I2C0 controller on Zybo to receive data
-void iic0_config_ctrl_to_receive();
+#define LIDARLITE_DEVICE_ADDR		0x62
 
-// Configure I2C0 controller to transmit data
-void iic0_config_ctrl_to_transmit();
+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);
 
-void wake_mag();
+int iic0_lidarlite_init();
+int iic0_lidarlite_sleep();
 
-#endif /*IIC_MPU9150_UTILS_H*/
+#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 (initI2C0() == -1) {
+	if (iic0_init() == -1) {
 		return -1;
 	}
 

quad/sw/modular_quad_pid/src/initialize_components.h

@@ -12,7 +12,7 @@
 #include "control_algorithm.h"
 #include "platform.h"
 #include "uart.h"
-#include "iic_mpu9150_utils.h"
+#include "iic_utils.h"
 #include "util.h"
 #include "controllers.h"
 

quad/sw/modular_quad_pid/src/sensor.c

@@ -11,11 +11,11 @@
 
 int sensor_init(raw_sensor_t * raw_sensor_struct, sensor_t * sensor_struct)
 {
-	if(startMPU9150() == -1)
+	if(iic0_mpu9150_start() == -1)
 		return -1;
 
 	// read sensor board and fill in gryo/accelerometer/magnetometer struct
-	get_gam_reading(&(raw_sensor_struct->gam));
+	iic0_mpu9150_read_gam(&(raw_sensor_struct->gam));
 
 	// Sets the first iteration to be at the accelerometer value since gyro initializes to {0,0,0} regardless of orientation
 	sensor_struct->pitch_angle_filtered = raw_sensor_struct->gam.accel_roll;
@@ -63,7 +63,7 @@ int get_sensors(log_t* log_struct, user_input_t* user_input_struct, raw_sensor_t
 
 
 	// the the sensor board and fill in the readings into the GAM struct
-	get_gam_reading(&(raw_sensor_struct->gam));
+	iic0_mpu9150_read_gam(&(raw_sensor_struct->gam));
 
 	log_struct->gam = raw_sensor_struct->gam;
 

quad/sw/modular_quad_pid/src/sensor.h

@@ -12,7 +12,7 @@
 
 #include "log_data.h"
 #include "user_input.h"
-#include "iic_mpu9150_utils.h"
+#include "iic_utils.h"
 #include "packet_processing.h"
 #include "uart.h"
 

quad/sw/modular_quad_pid/src/type_def.h

@@ -108,6 +108,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