/*

* Copyright (c) 2010, Regents of the University of California

* All rights reserved.

*

* Redistribution and use in source and binary forms, with or without

* modification, are permitted provided that the following conditions are met:

*

* - Redistributions of source code must retain the above copyright notice,

* this list of conditions and the following disclaimer.

* - Redistributions in binary form must reproduce the above copyright notice,

* this list of conditions and the following disclaimer in the documentation

* and/or other materials provided with the distribution.

* - Neither the name of the University of California, Berkeley nor the names

* of its contributors may be used to endorse or promote products derived

* from this software without specific prior written permission.

*

* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"

* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE

* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE

* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE

* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR

* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF

* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS

* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN

* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)

* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE

* POSSIBILITY OF SUCH DAMAGE.

*

*

* InvenSense ITG-3200 3-axis MEMS gyro IC Interface

*

* by Stanley S. Baek

*

* v.1.0

*

* Revisions:

* Stanley S. Baek 2010-05-30 Initial release

* Humphrey Hu 2012-06-27 Refactored, added dead zone

*

* Notes:

* - This module uses an I2C port for communicating with the gyroscope chip

*/

#include "ports.h" // for external interrupt

#include "i2c.h"

#include "gyro.h"

#include "utils.h"

#include "string.h"

// Register parameters

#define GYRO_ADDR_RD (0b11010001)

#define GYRO_ADDR_WR (0b11010000)

// Register addresses

#define REG_WHO_AM_I (0x00)

#define REG_SMPLRT_DIV (0x15)

#define REG_DLPF_FS (0x16)

#define REG_INT_CFG (0x17)

#define REG_INT_STATUS (0x1A)

#define REG_TEMP_OUT_H (0x1B)

#define REG_TEMP_OUT_L (0x1C)

#define REG_GYRO_XOUT_H (0x1D)

#define REG_GYRO_XOUT_L (0x1E)

#define REG_GYRO_YOUT_H (0x1F)

#define REG_GYRO_YOUT_L (0x20)

#define REG_GYRO_ZOUT_H (0x21)

#define REG_GYRO_ZOUT_L (0x22)

#define REG_PWR_MGM (0x3E)

#define LSB2DEG (0.0695652174) // 14.375 LSB/(deg/s)

#define LSB2RAD (0.00121414209)

// Other parameters

#define DEFAULT_DEAD_ZONE (2) // Initial dead zone (0 disabled)

#define INITIAL_CALIB_NUM (200) // Initial calibration samples

#define I2C_TIMEOUT_BYTES (200) // Number of bytes for I2C timeout

/*-----------------------------------------------------------------------------

* Static Variables

-----------------------------------------------------------------------------*/

static unsigned char is_ready = 0;

// data storage for receiving data

static union gyrodata {

unsigned char chr_data[8];

int int_data[4];

} GyroData;

// calibration parameters for gyroscope

static union {

float fdata[3];

unsigned char cdata[12];

} GyroOffset;

static int offsets[3]; // Integer gyro offsets

static int dead_zone; // Dead zone cutoff

/*-----------------------------------------------------------------------------

* Declaration of static functions

-----------------------------------------------------------------------------*/

static void gyroWrite(unsigned char regaddr, unsigned char data );

static void gyroHandleISR(void);

static inline unsigned int gyroReadString(unsigned length, unsigned char * data,

unsigned int data_wait);

static inline void gyroSendByte( unsigned char byte );

static inline unsigned char gyroReceiveByte(void);

static inline void gyroSendNACK(void);

static inline void gyroStartTx(void);

static inline void gyroEndTx(void);

static inline void gyroSetupPeripheral(void);

static int applyDeadZone(int val);

/*-----------------------------------------------------------------------------

* Public functions

-----------------------------------------------------------------------------*/

void gyroSetup(void) {

// setup I2C port

gyroSetupPeripheral();

// external interrupt configuration.

// it is NOT USED at this moment.

ConfigINT1(RISING_EDGE_INT & EXT_INT_DISABLE & EXT_INT_PRI_3);

dead_zone = DEFAULT_DEAD_ZONE;

delay_ms(25); // power up delay, may not need...

gyroReset();

delay_ms(10);

//gyroWrite(REG_DLPF_FS, 0x1A); // 2000 deg/sec, 1 kHz Sampling rate, 98Hz LPF

gyroWrite(REG_DLPF_FS, 0x19); // 2000 deg/sec, 1 kHz Sampling rate, 196Hz LPF

gyroWrite(REG_INT_CFG, 0x00); // interrupt disabled

gyroWrite(REG_PWR_MGM, 0x03); // Set power management?

delay_ms(1); // PLL Settling time

is_ready = 1;

delay_ms(6); //From datasheet, standard settling time

gyroRunCalib(INITIAL_CALIB_NUM); // quick calibration

}

void gyroSetDeadZone(int cutoff) {

if(cutoff < 0) { cutoff = -cutoff; } // Correct negative inputs

dead_zone = cutoff;

}

void gyroSetSampleRate(unsigned char rate) {

gyroWrite(REG_DLPF_FS, rate);

}

void gyroSetIntEn(unsigned char flag) {

_INT1IE = flag;

}

// TODO: Check these register values for sleep/wake

void gyroSleep(void) {

gyroWrite(REG_PWR_MGM, 0x78);

}

void gyroReset(void){

gyroWrite(REG_PWR_MGM, 0x80); //Write H_RESET bit

}

void gyroWake(void) {

gyroWrite(REG_PWR_MGM, 0x03);

}

unsigned char* gyroGetCalibParam(void) {

return GyroOffset.cdata;

}

void gyroGetOffsets(int* data){

data[0] = offsets[0];

data[1] = offsets[1];

data[2] = offsets[2];

}

void gyroRunCalib(unsigned int count){

unsigned int i;

long x_acc, y_acc, z_acc;

x_acc = 0;

y_acc = 0;

z_acc = 0;

CRITICAL_SECTION_START

// throw away first 200 data. Sometimes they are bad at the beginning.

for (i = 0; i < 200; ++i) {

gyroReadXYZ();

delay_us(100);

}

for (i = 0; i < count; i++) {

gyroReadXYZ();

x_acc += GyroData.int_data[1];

y_acc += GyroData.int_data[2];

z_acc += GyroData.int_data[3];

delay_ms(1); // Sample at around 1kHz

}

CRITICAL_SECTION_END

offsets[0] = x_acc/count;

offsets[1] = y_acc/count;

offsets[2] = z_acc/count;

GyroOffset.fdata[0] = 1.0*x_acc/count;

GyroOffset.fdata[1] = 1.0*y_acc/count;

GyroOffset.fdata[2] = 1.0*z_acc/count;

}

float gyroGetFloatTemp(void) {

return (35 + (gyroGetIntTemp() + 13200) / 280.0);

}

int gyroGetIntTemp(void) {

return GyroData.int_data[0];

}

void gyroReadTemp(void) {

unsigned char temp_data[2];

gyroStartTx();

gyroSendByte(GYRO_ADDR_WR);

gyroSendByte(REG_TEMP_OUT_H);

gyroEndTx();

gyroStartTx();

gyroSendByte(GYRO_ADDR_RD);

gyroReadString(2, temp_data, I2C_TIMEOUT_BYTES);

gyroEndTx();

GyroData.chr_data[0] = temp_data[1];

GyroData.chr_data[1] = temp_data[0];

}

void gyroGetIntXYZ(int* data) {

data[0] = gyroGetIntX();

data[1] = gyroGetIntY();

data[2] = gyroGetIntZ();

}

int gyroGetIntX(void) {

return applyDeadZone(GyroData.int_data[1] - offsets[0]);

}

int gyroGetIntY(void) {

return applyDeadZone(GyroData.int_data[2] - offsets[1]);

}

int gyroGetIntZ(void) {

return applyDeadZone(GyroData.int_data[3] - offsets[2]);

}

void gyroGetRadXYZ(float* data) {

data[0] = gyroGetRadX();

data[1] = gyroGetRadY();

data[2] = gyroGetRadZ();

}

float gyroGetRadX(void) {

return LSB2RAD*gyroGetIntX();

}

float gyroGetRadY(void) {

return LSB2RAD*gyroGetIntY();

}

float gyroGetRadZ(void) {

return LSB2RAD*gyroGetIntZ();

}

void gyroGetDegXYZ(float* data) {

data[0] = gyroGetDegX();

data[1] = gyroGetDegY();

data[2] = gyroGetDegZ();

}

float gyroGetDegX(void) {

return LSB2DEG*gyroGetIntX();

}

float gyroGetDegY(void) {

return LSB2DEG*gyroGetIntY();

}

float gyroGetDegZ(void) {

return LSB2DEG*gyroGetIntZ();

}

unsigned char* gyroToString(void) {

return GyroData.chr_data + 2;

}

void gyroDumpData(unsigned char* buffer) {

memcpy(buffer, GyroData.chr_data + 2, 6);

}

unsigned char* gyroReadXYZ(void) {

unsigned char gyro_data[6];

gyroStartTx();

gyroSendByte(GYRO_ADDR_WR);

gyroSendByte(0x1d);

gyroEndTx();

gyroStartTx();

gyroSendByte(GYRO_ADDR_RD);

gyroReadString(6, gyro_data, I2C_TIMEOUT_BYTES);

gyroEndTx();

GyroData.chr_data[2] = gyro_data[1];

GyroData.chr_data[3] = gyro_data[0];

GyroData.chr_data[4] = gyro_data[3];

GyroData.chr_data[5] = gyro_data[2];

GyroData.chr_data[6] = gyro_data[5];

GyroData.chr_data[7] = gyro_data[4];

return GyroData.chr_data + 2;

}

void gyroGetXYZ(unsigned char *data) {

gyroReadXYZ();

gyroDumpData(data);

}

/*-----------------------------------------------------------------------------

* ----------------------------------------------------------------------------

* The functions below are intended for internal use, i.e., private methods.

* Users are recommended to use functions defined above.

* ----------------------------------------------------------------------------

-----------------------------------------------------------------------------*/

/******************************************************************************

* Interrupt handler for Gyroscope

* ****************************************************************************/

void __attribute__((interrupt, no_auto_psv)) _INT1Interrupt(void) {

gyroHandleISR();

_INT1IF = 0; // Clear the interrupt flag

}

/*****************************************************************************

* Function Name : gyroHandleISR

* Description : Need to implement this function if interrupt is used.

* Parameters : None

* Return Value : None

*****************************************************************************/

static void gyroHandleISR(void) {

return;

}

/*****************************************************************************

* Function Name : gyroWrite

* Description : Write a data to a register

* Parameters : regaddr - address of register

* data - value to be written to the register

* Return Value : None

*****************************************************************************/

static void gyroWrite(unsigned char regaddr, unsigned char data ){

gyroStartTx();

gyroSendByte(GYRO_ADDR_WR);

gyroSendByte(regaddr);

gyroSendByte(data);

gyroEndTx();

}

/*****************************************************************************

* Function Name : gyroReadString

* Description : It reads predetermined data string length from the I2C bus.

* Parameters : length is the string length to read

* data is the storage for received gyro data

* data_wait is the timeout value

* Return Value : Number of bytes read before timeout.

*****************************************************************************/

static inline unsigned int gyroReadString(unsigned length, unsigned char * data,

unsigned int data_wait) {

return MastergetsI2C2(length, data, data_wait);

}

/*****************************************************************************

* Function Name : gyroSendByte

* Description : Send a byte to gyroscope

* Parameters : byte - a byte to send

* Return Value : None

*****************************************************************************/

static inline void gyroSendByte( unsigned char byte ) {

MasterWriteI2C2(byte);

while(I2C2STATbits.TRSTAT);

while(I2C2STATbits.ACKSTAT);

}

/*****************************************************************************

* Function Name : gyroReceiveByte

* Description : Receive a byte from gyroscope

* Parameters : None

* Return Value : None

*****************************************************************************/

static inline unsigned char gyroReceiveByte(void) {

return MasterReadI2C2();

}

/*****************************************************************************

* Function Name : gyroSendNACK

* Description : Send NACK to gyroscope

* Parameters : None

* Return Value : None

*****************************************************************************/

static inline void gyroSendNACK(void){

NotAckI2C2();

while(I2C2CONbits.ACKEN);

}

/*****************************************************************************

* Function Name : gyroStartTx

* Description : Start I2C transmission

* Parameters : None

* Return Value : None

*****************************************************************************/

static inline void gyroStartTx(void){

StartI2C2();

while(I2C2CONbits.SEN);

}

/*****************************************************************************

* Function Name : gyroEndTx

* Description : End I2C transmission

* Parameters : None

* Return Value : None

*****************************************************************************/

static inline void gyroEndTx(void){

StopI2C2();

while(I2C2CONbits.PEN);

}

/*****************************************************************************

* Function Name : gyroSetupPeripheral

* Description : Setup I2C for gyroscope

* Parameters : None

* Return Value : None

*****************************************************************************/

static inline void gyroSetupPeripheral(void) {

unsigned int I2C2CONvalue, I2C2BRGvalue;

I2C2CONvalue = I2C2_ON & I2C2_IDLE_CON & I2C2_CLK_HLD &

I2C2_IPMI_DIS & I2C2_7BIT_ADD & I2C2_SLW_DIS &

I2C2_SM_DIS & I2C2_GCALL_DIS & I2C2_STR_DIS &

I2C2_NACK & I2C2_ACK_DIS & I2C2_RCV_DIS &

I2C2_STOP_DIS & I2C2_RESTART_DIS & I2C2_START_DIS;

// BRG = Fcy(1/Fscl - 1/10000000)-1, Fscl = 400KHz

I2C2BRGvalue = 95;

OpenI2C2(I2C2CONvalue, I2C2BRGvalue);

IdleI2C2();

}

// Dead zone a value

static int applyDeadZone(int val) {

if(val < dead_zone && val > -dead_zone) { val = 0; }

return val;

}