diff --git a/quad/sw/modular_quad_pid/src/controllers.c b/quad/sw/modular_quad_pid/src/controllers.c
index 31f70c3673c15e9e4ca56a03fb5c7ce6804a7b99..7be7616e1a527ebb327c73609a8d93b84bf36508 100644
--- a/quad/sw/modular_quad_pid/src/controllers.c
+++ b/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"
diff --git a/quad/sw/modular_quad_pid/src/iic_mpu9150_utils.c b/quad/sw/modular_quad_pid/src/iic_mpu9150_utils.c
deleted file mode 100644
index 75a63cc90ea57d28a08b9e5ca057c833ec88c4e4..0000000000000000000000000000000000000000
--- a/quad/sw/modular_quad_pid/src/iic_mpu9150_utils.c
+++ /dev/null
@@ -1,239 +0,0 @@
-/**
- * IIC_MPU9150_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
- */
-
-#include <stdio.h>
-#include <sleep.h>
-#include <math.h>
-
-#include "xparameters.h"
-#include "iic_mpu9150_utils.h"
-#include "xbasic_types.h"
-#include "xiicps.h"
-
-XIicPs_Config* i2c_config;
-XIicPs I2C0;
-double magX_correction = -1, magY_correction, magZ_correction;
-
-int initI2C0(){
-
- //Make sure CPU_1x clk is enabled for I2C controller
- Xuint16* aper_ctrl = (Xuint16*) IO_CLK_CONTROL_REG_ADDR;
-
- if(*aper_ctrl & 0x00040000){
- xil_printf("CPU_1x is set to I2C0\r\n");
- }
-
- else{
- xil_printf("CPU_1x is not set to I2C0..Setting now\r\n");
- *aper_ctrl |= 0x00040000;
- }
-
-
- // Look up
- i2c_config = XIicPs_LookupConfig(XPAR_PS7_I2C_0_DEVICE_ID);
-
- XStatus status = XIicPs_CfgInitialize(&I2C0, i2c_config, i2c_config->BaseAddress);
-
- // Check if initialization was successful
- if(status != XST_SUCCESS){
- return -1;
- }
-
- // Reset the controller and set the clock to 400kHz
- XIicPs_Reset(&I2C0);
- XIicPs_SetSClk(&I2C0, 400000);
-
-
- return 0;
-}
-
-int startMPU9150(){
-
- // Device Reset & Wake up
- iic0Write(0x6B, 0x80);
- usleep(5000);
-
- // Set clock reference to Z Gyro
- iic0Write(0x6B, 0x03);
- // Configure Digital Low/High Pass filter
- iic0Write(0x1A,0x06); // Level 6 low pass on gyroscope
-
- // Configure Gyro to 2000dps, Accel. to +/-8G
- iic0Write(0x1B, 0x18);
- iic0Write(0x1C, 0x10);
-
- // Enable I2C bypass for AUX I2C (Magnetometer)
- iic0Write(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
-
- usleep(100000);
-
- int i;
- gam_t temp_gam;
-
- // Do about 20 reads to warm up the device
- for(i=0; i < 20; ++i){
- if(get_gam_reading(&temp_gam) == -1){
- return -1;
- }
- usleep(1000);
- }
-
- return 0;
-}
-
-void stopMPU9150(){
-
- //Put MPU to sleep
- iic0Write(0x6B, 0b01000000);
-}
-
-void iic0Write(u8 register_addr, u8 data){
-
- u16 device_addr = MPU9150_DEVICE_ADDR;
- u8 buf[] = {register_addr, data};
-
- // Check if within register range
- if(register_addr < 0 || register_addr > 0x75){
- return;
- }
-
- if(register_addr <= 0x12){
- device_addr = MPU9150_COMPASS_ADDR;
- }
-
- XIicPs_MasterSendPolled(&I2C0, buf, 2, device_addr);
-
-}
-
-void iic0Read(u8* recv_buffer, u8 register_addr, int size){
-
- u16 device_addr = MPU9150_DEVICE_ADDR;
- u8 buf[] = {register_addr};
-
- // Check if within register range
- if(register_addr < 0 || register_addr > 0x75){
- }
-
- // Set device address to the if 0x00 <= register address <= 0x12
- if(register_addr <= 0x12){
- device_addr = MPU9150_COMPASS_ADDR;
- }
-
-
- XIicPs_MasterSendPolled(&I2C0, buf, 1, device_addr);
- XIicPs_MasterRecvPolled(&I2C0, recv_buffer,size,device_addr);
-}
-
-void CalcMagSensitivity(){
-
- u8 buf[3];
- u8 ASAX, ASAY, ASAZ;
-
- // Quickly read from the factory ROM to get correction coefficents
- iic0Write(0x0A, 0x0F);
- usleep(10000);
-
- // Read raw adjustment values
- iic0Read(buf, 0x10,3);
- ASAX = buf[0];
- ASAY = buf[1];
- ASAZ = buf[2];
-
- // Set the correction coefficients
- magX_correction = (ASAX-128)*0.5/128 + 1;
- magY_correction = (ASAY-128)*0.5/128 + 1;
- magZ_correction = (ASAZ-128)*0.5/128 + 1;
-}
-
-
-void ReadMag(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();
- }
-
- // Set Mag to single read mode
- iic0Write(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);
- }
-
- // Get mag data
- iic0Read(mag_data, 0x03, 6);
-
- raw_magX = (mag_data[1] << 8) | mag_data[0];
- raw_magY = (mag_data[3] << 8) | mag_data[2];
- raw_magZ = (mag_data[5] << 8) | mag_data[4];
-
- // Set magnetometer data to output
- gam->mag_x = raw_magX * magX_correction;
- gam->mag_y = raw_magY * magY_correction;
- gam->mag_z = raw_magZ * magZ_correction;
-
-}
-
-/**
- * Get Gyro Accel Mag (GAM) information
- */
-int get_gam_reading(gam_t* gam) {
-
- Xint16 raw_accel_x, raw_accel_y, raw_accel_z;
- Xint16 gyro_x, gyro_y, gyro_z;
-
- Xuint8 sensor_data[ACCEL_GYRO_READ_SIZE] = {};
-
- // We should only get mag_data ~10Hz
- //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);
-
- //Calculate accelerometer data
- raw_accel_x = sensor_data[ACC_X_H] << 8 | sensor_data[ACC_X_L];
- raw_accel_y = sensor_data[ACC_Y_H] << 8 | sensor_data[ACC_Y_L];
- raw_accel_z = sensor_data[ACC_Z_H] << 8 | sensor_data[ACC_Z_L];
-
- // put in G's
- gam->accel_x = (raw_accel_x / 4096.0) + ACCEL_X_BIAS; // 4,096 is the gain per LSB of the measurement reading based on a configuration range of +-8g
- gam->accel_y = (raw_accel_y / 4096.0) + ACCEL_Y_BIAS;
- gam->accel_z = (raw_accel_z / 4096.0) + ACCEL_Z_BIAS;
-
- //Get X and Y angles
- // javey: this assigns accel_(pitch/roll) in units of radians
- gam->accel_pitch = atan(gam->accel_x / sqrt(gam->accel_y*gam->accel_y + gam->accel_z*gam->accel_z));
- gam->accel_roll = -atan(gam->accel_y / sqrt(gam->accel_x*gam->accel_x + gam->accel_z*gam->accel_z)); // negative because sensor board is upside down
-
- //Convert gyro data to rate (we're only using the most 12 significant bits)
- gyro_x = (sensor_data[GYR_X_H] << 8) | (sensor_data[GYR_X_L]); //* G_GAIN;
- gyro_y = (sensor_data[GYR_Y_H] << 8 | sensor_data[GYR_Y_L]);// * G_GAIN;
- gyro_z = (sensor_data[GYR_Z_H] << 8 | sensor_data[GYR_Z_L]);// * G_GAIN;
-
- //Get the number of degrees
- //javey: converted to radians to following SI units
- gam->gyro_xVel_p = ((gyro_x / GYRO_SENS) * DEG_TO_RAD) + GYRO_X_BIAS;
- gam->gyro_yVel_q = ((gyro_y / GYRO_SENS) * DEG_TO_RAD) + GYRO_Y_BIAS;
- gam->gyro_zVel_r = ((gyro_z / GYRO_SENS) * DEG_TO_RAD) + GYRO_Z_BIAS;
-
- return 0;
-
-}
diff --git a/quad/sw/modular_quad_pid/src/iic_utils.c b/quad/sw/modular_quad_pid/src/iic_utils.c
new file mode 100644
index 0000000000000000000000000000000000000000..290d5f9b60e5861d10d1e2770870fbc98e020b30
--- /dev/null
+++ b/quad/sw/modular_quad_pid/src/iic_utils.c
@@ -0,0 +1,458 @@
+/**
+ * IIC_UTILS.c
+ *
+ * Utility functions for using I2C on a Diligent Zybo board.
+ * Supports the SparkFun MPU9150 and the LiDAR lite v2 sensor
+ */
+
+#include <stdio.h>
+#include <sleep.h>
+#include <math.h>
+
+#include "xparameters.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 iic0_init(){
+
+ //Make sure CPU_1x clk is enabled for I2C controller
+ Xuint16* aper_ctrl = (Xuint16*) IO_CLK_CONTROL_REG_ADDR;
+
+ if(*aper_ctrl & 0x00040000){
+ xil_printf("CPU_1x is set to I2C0\r\n");
+ }
+
+ else{
+ xil_printf("CPU_1x is not set to I2C0..Setting now\r\n");
+ *aper_ctrl |= 0x00040000;
+ }
+
+
+ // Look up
+ i2c_config = XIicPs_LookupConfig(XPAR_PS7_I2C_0_DEVICE_ID);
+
+ XStatus status = XIicPs_CfgInitialize(&I2C0, i2c_config, i2c_config->BaseAddress);
+
+ // Check if initialization was successful
+ if(status != XST_SUCCESS){
+ return -1;
+ }
+
+ // Reset the controller and set the clock to 400kHz
+ XIicPs_Reset(&I2C0);
+ XIicPs_SetSClk(&I2C0, 400000);
+
+
+ return 0;
+}
+
+int iic0_mpu9150_start(){
+
+ // Device Reset & Wake up
+ iic0_mpu9150_write(0x6B, 0x80);
+ usleep(5000);
+
+ // Set clock reference to Z Gyro
+ iic0_mpu9150_write(0x6B, 0x03);
+ // Configure Digital Low/High Pass filter
+ iic0_mpu9150_write(0x1A,0x06); // Level 6 low pass on gyroscope
+
+ // Configure Gyro to 2000dps, Accel. to +/-8G
+ iic0_mpu9150_write(0x1B, 0x18);
+ iic0_mpu9150_write(0x1C, 0x10);
+
+ // Enable I2C bypass for AUX I2C (Magnetometer)
+ iic0_mpu9150_write(0x37, 0x02);
+
+ // Setup Mag
+ 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);
+
+ int i;
+ gam_t temp_gam;
+
+ // Do about 20 reads to warm up the device
+ for(i=0; i < 20; ++i){
+ if(iic0_mpu9150_read_gam(&temp_gam) == -1){
+ return -1;
+ }
+ usleep(1000);
+ }
+
+ return 0;
+}
+
+void iic0_mpu9150_stop(){
+
+ //Put MPU to sleep
+ iic0_mpu9150_write(0x6B, 0b01000000);
+}
+
+void iic0_mpu9150_write(u8 register_addr, u8 data){
+
+ u16 device_addr = MPU9150_DEVICE_ADDR;
+ u8 buf[] = {register_addr, data};
+
+ // Check if within register range
+ if(register_addr < 0 || register_addr > 0x75){
+ return;
+ }
+
+ if(register_addr <= 0x12){
+ device_addr = MPU9150_COMPASS_ADDR;
+ }
+
+ XIicPs_MasterSendPolled_ours(&I2C0, buf, 2, device_addr);
+
+}
+
+void iic0_mpu9150_read(u8* recv_buffer, u8 register_addr, int size){
+
+ u16 device_addr = MPU9150_DEVICE_ADDR;
+ u8 buf[] = {register_addr};
+
+ // Check if within register range
+ if(register_addr < 0 || register_addr > 0x75){
+ }
+
+ // Set device address to the if 0x00 <= register address <= 0x12
+ if(register_addr <= 0x12){
+ device_addr = MPU9150_COMPASS_ADDR;
+ }
+
+
+ XIicPs_MasterSendPolled_ours(&I2C0, buf, 1, device_addr);
+ XIicPs_MasterRecvPolled(&I2C0, recv_buffer,size,device_addr);
+}
+
+void iic0_mpu9150_calc_mag_sensitivity(){
+
+ u8 buf[3];
+ u8 ASAX, ASAY, ASAZ;
+
+ // Quickly read from the factory ROM to get correction coefficents
+ iic0_mpu9150_write(0x0A, 0x0F);
+ usleep(10000);
+
+ // Read raw adjustment values
+ iic0_mpu9150_read(buf, 0x10,3);
+ ASAX = buf[0];
+ ASAY = buf[1];
+ ASAZ = buf[2];
+
+ // Set the correction coefficients
+ magX_correction = (ASAX-128)*0.5/128 + 1;
+ magY_correction = (ASAY-128)*0.5/128 + 1;
+ magZ_correction = (ASAZ-128)*0.5/128 + 1;
+}
+
+
+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){
+ iic0_mpu9150_calc_mag_sensitivity();
+ }
+
+ // Set Mag to single read mode
+ 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){
+ iic0_mpu9150_read(mag_data, 0x02, 1);
+ }
+
+ // Get mag data
+ 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];
+ raw_magZ = (mag_data[5] << 8) | mag_data[4];
+
+ // Set magnetometer data to output
+ gam->mag_x = raw_magX * magX_correction;
+ gam->mag_y = raw_magY * magY_correction;
+ gam->mag_z = raw_magZ * magZ_correction;
+
+}
+
+/**
+ * Get Gyro Accel Mag (GAM) information
+ */
+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;
+
+ Xuint8 sensor_data[ACCEL_GYRO_READ_SIZE] = {};
+
+ // We should only get mag_data ~10Hz
+ //Xint8 mag_data[6] = {};
+
+ //readHandler = iic0_read_bytes(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];
+ raw_accel_y = sensor_data[ACC_Y_H] << 8 | sensor_data[ACC_Y_L];
+ raw_accel_z = sensor_data[ACC_Z_H] << 8 | sensor_data[ACC_Z_L];
+
+ // put in G's
+ gam->accel_x = (raw_accel_x / 4096.0) + ACCEL_X_BIAS; // 4,096 is the gain per LSB of the measurement reading based on a configuration range of +-8g
+ gam->accel_y = (raw_accel_y / 4096.0) + ACCEL_Y_BIAS;
+ gam->accel_z = (raw_accel_z / 4096.0) + ACCEL_Z_BIAS;
+
+ //Get X and Y angles
+ // javey: this assigns accel_(pitch/roll) in units of radians
+ gam->accel_pitch = atan(gam->accel_x / sqrt(gam->accel_y*gam->accel_y + gam->accel_z*gam->accel_z));
+ gam->accel_roll = -atan(gam->accel_y / sqrt(gam->accel_x*gam->accel_x + gam->accel_z*gam->accel_z)); // negative because sensor board is upside down
+
+ //Convert gyro data to rate (we're only using the most 12 significant bits)
+ gyro_x = (sensor_data[GYR_X_H] << 8) | (sensor_data[GYR_X_L]); //* G_GAIN;
+ gyro_y = (sensor_data[GYR_Y_H] << 8 | sensor_data[GYR_Y_L]);// * G_GAIN;
+ gyro_z = (sensor_data[GYR_Z_H] << 8 | sensor_data[GYR_Z_L]);// * G_GAIN;
+
+ //Get the number of degrees
+ //javey: converted to radians to following SI units
+ gam->gyro_xVel_p = ((gyro_x / GYRO_SENS) * DEG_TO_RAD) + GYRO_X_BIAS;
+ gam->gyro_yVel_q = ((gyro_y / GYRO_SENS) * DEG_TO_RAD) + GYRO_Y_BIAS;
+ 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;
+}
diff --git a/quad/sw/modular_quad_pid/src/iic_mpu9150_utils.h b/quad/sw/modular_quad_pid/src/iic_utils.h
similarity index 64%
rename from quad/sw/modular_quad_pid/src/iic_mpu9150_utils.h
rename to quad/sw/modular_quad_pid/src/iic_utils.h
index f93b7686b8f62d843b26e5ecaed667ed6b5b08f6..b4185b19da3a9fbf49ee46ea3e592948570a1ed7 100644
--- a/quad/sw/modular_quad_pid/src/iic_mpu9150_utils.h
+++ b/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*/
diff --git a/quad/sw/modular_quad_pid/src/initialize_components.c b/quad/sw/modular_quad_pid/src/initialize_components.c
index 9c79db823252d961b57acd621fb7a09d662f1d2d..91af69ad0b10063fb397bbbe3cd3dd010eaf6391 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 (initI2C0() == -1) {
+ if (iic0_init() == -1) {
return -1;
}
diff --git a/quad/sw/modular_quad_pid/src/initialize_components.h b/quad/sw/modular_quad_pid/src/initialize_components.h
index 8a1c66e7545871ba3f35101030d245b0039604f8..151f68296bd554a5d7f47b93691801090c6106ea 100644
--- a/quad/sw/modular_quad_pid/src/initialize_components.h
+++ b/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"
diff --git a/quad/sw/modular_quad_pid/src/sensor.c b/quad/sw/modular_quad_pid/src/sensor.c
index 9f24a3b76f52eef86538396a75cac32f72d8e733..2f1b3e68061f7a35da2406e73172eb73529f35bb 100644
--- a/quad/sw/modular_quad_pid/src/sensor.c
+++ b/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;
diff --git a/quad/sw/modular_quad_pid/src/sensor.h b/quad/sw/modular_quad_pid/src/sensor.h
index 6561402a1e766dfdd7c11e0f4dcab21f25513fa7..d9592b0ea5f5e1514307541590277ea4a1f8b07b 100644
--- a/quad/sw/modular_quad_pid/src/sensor.h
+++ b/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"
diff --git a/quad/sw/modular_quad_pid/src/type_def.h b/quad/sw/modular_quad_pid/src/type_def.h
index 2157f3b0319920c779c31c40e6c9db4bea68341e..40c0db97f613724b0fa92fef886bc359b0c9bd7f 100644
--- a/quad/sw/modular_quad_pid/src/type_def.h
+++ b/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