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_utils.c b/quad/sw/modular_quad_pid/src/iic_utils.c
index d3773f2715d4916f226f8c1846cbe23d0dc22efd..be2c0f7e720d22b915d5b61c3864007e750147a1 100644
--- a/quad/sw/modular_quad_pid/src/iic_utils.c
+++ b/quad/sw/modular_quad_pid/src/iic_utils.c
@@ -1,13 +1,8 @@
/**
- * 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,7 +10,7 @@
#include <math.h>
#include "xparameters.h"
-#include "iic_mpu9150_utils.h"
+#include "iic_utils.h"
#include "xbasic_types.h"
#include "xiicps.h"
@@ -23,7 +18,7 @@ XIicPs_Config* i2c_config;
XIicPs I2C0;
double magX_correction = -1, magY_correction, magZ_correction;
-int initI2C0(){
+int init_iic0(){
//Make sure CPU_1x clk is enabled for I2C controller
Xuint16* aper_ctrl = (Xuint16*) IO_CLK_CONTROL_REG_ADDR;
@@ -56,26 +51,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 4 low pass on gyroscope
+ iic0_mpu9150_write(0x1A,0x06); // Level 4 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 +79,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 +88,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};
@@ -117,7 +112,7 @@ void iic0Write(u8 register_addr, u8 data){
}
-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};
@@ -136,17 +131,17 @@ void iic0Read(u8* recv_buffer, u8 register_addr, int size){
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 +153,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 +190,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 +201,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];
diff --git a/quad/sw/modular_quad_pid/src/iic_utils.h b/quad/sw/modular_quad_pid/src/iic_utils.h
index e95fc0846d52a91def076706169b75fd5e0c6e4a..891e349460998fb7134bdb758aab28b98b28ae03 100644
--- a/quad/sw/modular_quad_pid/src/iic_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"
@@ -100,69 +100,23 @@
#define ACCEL_Z_BIAS 0.087f
// Initialize hardware; Call this FIRST before calling any other functions
-int initI2C0();
+int init_iic0();
-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();
+int iic0_mpu9150_start();
// Put MPU back to sleep
-void stopMPU9150();
+void iic0_mpu9150_stop();
-void CalcMagSensitivity();
-void ReadMag(gam_t* gam);
-void ReadGyroAccel(gam_t* gam);
+void iic0_mpu9150_calc_mag_sensitivity();
+void iic0_mpu9150_read_mag(gam_t* gam);
+void iic0_mpu9150_read_gyro_accel(gam_t* gam);
-int get_gam_reading(gam_t* gam);
+int iic0_mpu9150_read_gam(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();
-
-// 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();
-
-
-// Clears the interrupt status register
-// Called by configuration functions
-void iic0_clear_intr_status();
-
-
-
-// Configure I2C0 controller on Zybo to receive data
-void iic0_config_ctrl_to_receive();
-
-// Configure I2C0 controller to transmit data
-void iic0_config_ctrl_to_transmit();
-
-
-void wake_mag();
-
-#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 c548d26d2151f6923fc91421211a2f938f3e9a4b..a3fcd616e0b23580a9443b64ed8aa3f318820280 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 (init_iic0() == -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"