示例#1
0
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);



}
示例#2
0
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);
}
示例#3
0
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;
  }

}