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