#include "gsensor_6050.h"

void gSensor_Delay(void)

{

asm("nop");

}

void g_sda_output(void)

{

GPIO_Init(GPIOC,c_Gsensor_IO_SDA,GPIO_MODE_OUT_PP_HIGH_FAST);

}

void g_sda_input(void)

{

GPIO_Init(GPIOC,c_Gsensor_IO_SDA,GPIO_MODE_IN_FL_NO_IT);

}

void g_i2c_start(void)

{

g_sda_output();

Handler_G_SDA_H;

Handler_G_SCL_H;

gSensor_Delay();

Handler_G_SDA_L;

gSensor_Delay();

Handler_G_SCL_L;

}

void g_i2c_stop(void)

{

g_sda_output();

Handler_G_SCL_L;

Handler_G_SDA_L;

gSensor_Delay();

Handler_G_SCL_H;

Handler_G_SDA_H;

gSensor_Delay();

}

unsigned char g_i2c_wait_ack(void)

{

unsigned char err_timer_cnt;

g_sda_input();

gSensor_Delay();

Handler_G_SCL_H;

gSensor_Delay();

while(Handler_R_G_SDA)

{

err_timer_cnt++;

if(err_timer_cnt > 100)

{

g_i2c_stop();

return 1;

}

}

Handler_G_SCL_L;

return 0;

}

void g_i2c_ack(void)

{

g_sda_output();

Handler_G_SDA_L;

gSensor_Delay();

Handler_G_SCL_H;

gSensor_Delay();

Handler_G_SCL_L;

}

void g_i2c_not_ack(void)

{

g_sda_output();

Handler_G_SDA_H;

gSensor_Delay();

Handler_G_SCL_H;

gSensor_Delay();

Handler_G_SCL_L;

}

void g_i2c_sendbyte(unsigned char data_byte)

{

unsigned char i;

g_sda_output();

for(i = 0; i < 8;i++)

{

Handler_G_SCL_L;

if(data_byte & 0x80)

{

Handler_G_SDA_H;

}

else

{

Handler_G_SDA_L;

}

data_byte<<=1;

Handler_G_SCL_H;

gSensor_Delay();

}

Handler_G_SCL_L;

}

unsigned char g_i2c_readbyte(unsigned char ack_type)

{

unsigned char i,rec_byte;

rec_byte = 0;

g_sda_input();

Handler_G_SCL_L;

for( i = 0; i < 8;i++)

{

gSensor_Delay();

Handler_G_SCL_H;

rec_byte<<=1;

if(Handler_R_G_SDA)

{

rec_byte |= 0x01;

}

Handler_G_SCL_L;

}

if(!ack_type)

{

g_i2c_not_ack();

}

else

{

g_i2c_ack();

}

return rec_byte;

}

void gsensor_wirte(unsigned char reg_address,unsigned char data)

{

while(1)

{

g_i2c_start();

g_i2c_sendbyte(0xd0);

if(g_i2c_wait_ack()) continue;

g_i2c_sendbyte(reg_address);

if(g_i2c_wait_ack()) continue;

g_i2c_sendbyte(data);

if(g_i2c_wait_ack()) continue;

g_i2c_stop();

return;

}

}

unsigned char gsensor_read(unsigned char reg_address)

{

unsigned char rec_byte = 0;

unsigned char err_timer_cnt = 0;

while(1)

{

g_i2c_start();

g_i2c_sendbyte(0xd0);

if(g_i2c_wait_ack())

{

err_timer_cnt++;

if(err_timer_cnt > 100)

{

return 0;

}

else

{

continue;

}

}

g_i2c_sendbyte(reg_address);

if(g_i2c_wait_ack())

{

err_timer_cnt++;

if(err_timer_cnt > 100)

{

return 0;

}

else

{

continue;

}

}

g_i2c_start();

g_i2c_sendbyte(0xd1);

if(g_i2c_wait_ack())

{

err_timer_cnt++;

if(err_timer_cnt > 100)

{

return 0;

}

else

{

continue;

}

}

rec_byte = g_i2c_readbyte(0);

g_i2c_stop();

return rec_byte;

}

}

void g_mpu6050_init(void)

{

gsensor_wirte(c_g_reg_pwr_mgr_1,0);//clear sleep

delayMs(200);

gsensor_wirte(c_g_reg_smplrt,0x7);

gsensor_wirte(c_g_reg_config,0x6);

gsensor_wirte(c_g_reg_gyro_config,0x18);//1000.sec

gsensor_wirte(c_g_reg_accel_config,0x08);

}