iic_utils.c

/**
 * 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 <math.h>

#include "iic_utils.h"


static struct I2CDriver *i2c;
static struct SystemDriver *sys;

double magX_correction = -1, magY_correction, magZ_correction;

void iic_set_globals(struct I2CDriver *given_i2c, struct SystemDriver *given_system) {
  i2c = given_i2c;
  sys = given_system;
}

int iic0_mpu9150_start(){

	// Device Reset & Wake up
	iic0_mpu9150_write(0x6B, 0x80);
	sys->sleep(sys, 5000);

	// Set clock reference to Z Gyro
	iic0_mpu9150_write(0x6B, 0x03);
	// Configure Digital Low/High Pass filter
	iic0_mpu9150_write(0x1A,0x03); // Level 3 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

	sys->sleep(sys, 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;
		}
		sys->sleep(sys, 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;
	}

	i2c->write(i2c, device_addr, buf, 2);
}

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;
	}


	i2c->write(i2c, device_addr, buf, 1);
	i2c->read(i2c, device_addr, recv_buffer, size);
}

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);
	sys->sleep(sys, 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];
	i16 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);
	sys->sleep(sys, 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) {

	i16 raw_accel_x, raw_accel_y, raw_accel_z;
	i16 gyro_x, gyro_y, gyro_z;

	u8 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;


	//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 i2c->write(i2c, LIDARLITE_DEVICE_ADDR, buf, 2);
}

int iic0_lidarlite_read(u8* recv_buffer, u8 register_addr, int size) {
	u8 buf[] = {register_addr};
	int status = 0;

	status |= i2c->write(i2c, LIDARLITE_DEVICE_ADDR, buf, 1);
	// Only read if write succeeded
	if (status == 0) {
		status |= i2c->read(i2c, LIDARLITE_DEVICE_ADDR, recv_buffer, size);
	}
	return status;
}

int iic0_lidarlite_init() {
	int status = 0;

	// Device Reset & Wake up with default settings
	status = iic0_lidarlite_write(0x00, 0x00);
	sys->sleep(sys, 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);
	float distance_cm = (float)(buf[0] << 8 | buf[1]) - LIDAR_OFFSET;
	lidar->distance_m = 0.01 * distance_cm;

	return status;
}

//////////////////////////////////////////////////
// PX4FLOW
//////////////////////////////////////////////////

//TODO: Replace device-specific iic0_device_write/read with generic ones

int iic0_px4flow_write(u8 register_addr, u8 data) {
	u8 buf[] = {register_addr, data};

	return i2c->write(i2c, PX4FLOW_DEVICE_ADDR, buf, 2);
}

int iic0_px4flow_read(u8* recv_buffer, u8 register_addr, int size) {
	u8 buf[] = {register_addr};
	int status = 0;

	status |= i2c->write(i2c, PX4FLOW_DEVICE_ADDR, buf, 1);
	if (status == 0) {
		status |= i2c->read(i2c, PX4FLOW_DEVICE_ADDR, recv_buffer, size);
	}
	return status;
}

int iic0_px4flow_init(px4flow_t *of) {
	//Clear struct
	of->xVel = 0;
	of->yVel = 0;

	//Perform initial update
	// TODO: Re-enable this initial read once virtual mock-up is made
	//return iic0_px4flow_update(of);
	return 0;
}

int iic0_px4flow_update(px4flow_t *of) {
	int status = 0;

	struct {
		uint16_t frameCount;

		int16_t pixelFlowXSum;
		int16_t pixelFlowYSum;
		int16_t flowCompX;
		int16_t flowCompY;
		int16_t qual;

		int16_t gyroXRate;
		int16_t gyroYRate;
		int16_t gyroZRate;

		uint8_t gyroRange;
		uint8_t sonarTimestamp;
		int16_t groundDistance;
	} i2cFrame;

	u8 buf[sizeof(i2cFrame)];

	// Read the sensor value
	status = iic0_px4flow_read(buf, 0x00, sizeof(i2cFrame));

	if(status == 0) {
		//Copy into struct
		memcpy(&i2cFrame, buf, sizeof(i2cFrame));

		of->xVel = i2cFrame.flowCompX / 1000.;
		of->yVel = i2cFrame.flowCompY / 1000.;
		of->quality = i2cFrame.qual;
		of->distance = i2cFrame.groundDistance / 1000.;
	}

	return status;
}