void Gyro_get_g(GYRO_XYZ *XYZ)
{
//Gyro_read(OUT_X_L_A,&XYZ,6);
uint8_t Temp=0;
do
{
Gyro_read(L3G_STATUS_REG,&Temp,1);
}while((Temp&0x8)!=0x8); //New data ready
Gyro_read(L3G_OUT_X_L,&XYZ->X_L,1);
Gyro_read(L3G_OUT_X_H,&XYZ->X_H,1);
Gyro_read(L3G_OUT_Y_L,&XYZ->Y_L,1);
Gyro_read(L3G_OUT_Y_H,&XYZ->Y_H,1);
Gyro_read(L3G_OUT_Z_L,&XYZ->Z_L,1);
Gyro_read(L3G_OUT_Z_H,&XYZ->Z_H,1);
}
void Gyro_init()
{
MY_GYRO_CONF_STRUCT Gyro_set;
Gyro_set.CTRL_1=0x0F; //Frequency 760 Hz + Cut off=100 +Enable axis
Gyro_set.CTRL_2=0x09; // HP filter
Gyro_set.CTRL_3=0; //no interrupts
Gyro_set.CTRL_4=FS_2000DPS; //+- 250 dps
Gyro_set.CTRL_5=HP_ENABLE;
uint8_t info;
delay_ms(10);
Gyro_send(L3G_CTRL_REG1,&Gyro_set.CTRL_1,1);
Gyro_send(L3G_CTRL_REG2,&Gyro_set.CTRL_2,1);
Gyro_send(L3G_CTRL_REG3,&Gyro_set.CTRL_3,1);
Gyro_send(L3G_CTRL_REG4,&Gyro_set.CTRL_4,1);
Gyro_send(L3G_CTRL_REG5,&Gyro_set.CTRL_5,1);
Gyro_read(L3G_WHO_AM_I,&info,1);
delay_ms(20);
}
void Imu_calculate()
{
static float gyro_z_tmp[4];
static int gyro_z_counter = 0;
Gyro_read();
//Acc_read();
//z轴四次平均
gyro_z_tmp[gyro_z_counter] = Gyro_data.z;
gyro_z_counter++;
if (gyro_z_counter >= 4)
gyro_z_counter = 0;
Gyro_data.z = 0;
for (int i = 0; i < 4; i++)
Gyro_data.z += gyro_z_tmp[gyro_z_counter];
Gyro_data.z = Gyro_data.z / 4;
///////////////////////////////////////////////////////////
//上坡
Gyro_info.Gyro_Sum = 0;
Gyro_info.Gyroscope_Fifo[Gyro_info.counter] = Gyro_data.y;
Gyro_info.counter++;
if (Gyro_info.counter == GYRO_LENGTH)
Gyro_info.counter = 0;
for (int i = 0; i < GYRO_LENGTH; i++) //70长度
Gyro_info.Gyro_Sum += Gyro_info.Gyroscope_Fifo[i];
Gyro_info.Gyro_Sum -= 65; //0偏
if (Gyro_info.Need_Delay_Counter > 0)
Gyro_info.Need_Delay_Counter--;
if ( ( Gyro_info.Gyro_Sum > Gyro_info.RampThresholdValue ) && //第一次上坡
( Gyro_info.Ramp_Over_0_1st == 0 ) &&
( Gyro_info.Ramp_Less_0 == 0) &&
( Gyro_info.Ramp_Over_0_2nd == 0 ) &&
( Gyro_info.Need_Delay_Counter == 0 ) )
{
Gyro_info.RampUpDown = 1;
Gyro_info.Ramp_Over_0_1st = 1;
Gyro_info.Ramp_Less_0 = 0;
Gyro_info.Ramp_Over_0_2nd = 0;
Car_state.pre = Car_state.now; //上坡
Car_state.now = Ramp_Up;
}
if ( ( Gyro_info.Gyro_Sum < -(Gyro_info.RampThresholdValue + 500)) &&
( Gyro_info.Ramp_Over_0_1st == 1) &&
( Gyro_info.Ramp_Less_0 == 0) &&
( Gyro_info.Ramp_Over_0_2nd == 0) )
{
Gyro_info.RampUpDown = 0;
Gyro_info.Ramp_Over_0_1st = 1;
Gyro_info.Ramp_Less_0 = 1;
Gyro_info.Ramp_Over_0_2nd = 0;
Car_state.pre = Car_state.now; //下坡
Car_state.now = Ramp_Down;
}
if ( ( Gyro_info.Gyro_Sum > Gyro_info.RampThresholdValue) &&
( Gyro_info.Ramp_Over_0_1st == 1) &&
( Gyro_info.Ramp_Less_0 == 1) &&
(Gyro_info.Ramp_Over_0_2nd == 0))
{
Gyro_info.RampUpDown = 0;
Gyro_info.Ramp_Over_0_1st = 0;
Gyro_info.Ramp_Less_0 = 0;
Gyro_info.Ramp_Over_0_2nd = 0;
Gyro_info.Need_Delay_Counter = 200;
}
if (( Gyro_info.Ramp_Over_0_1st == 0) &&
( Gyro_info.Ramp_Less_0 == 0) &&
( Gyro_info.Ramp_Over_0_2nd == 0) )
{
Gyro_info.RampUpDown = 0;
}
}