#include "ICM20600.h"
#include "ICM20600_i2c.h"
#include "math.h"
#include "var.h"

float twoKp = 0.4, twoKi = 0.001, invSampleFreq = 0.005; 
float q0 = 1, q1 = 0, q2 = 0, q3 = 0;          
float integralFBx = 0, integralFBy = 0, integralFBz = 0; 

ICM20600_Data_typedef ICM20600Sensor;

Bike_Attitude_typedef Bike_Attitude = {FALSE,FALSE,0,0,0,0};

TrueOrFalse_Flag_Struct_t ICM20600_OK_Flag = FALSE;

uint8_t ICM20600_getDeviceID( void )
{
    uint8_t data=0;
		Sensors_I2C_ReadRegister(ICM20600_I2C_ADDR2,ICM20600_PWR_MGMT_1,1,&data);
	  if(data != 0)
		{
		  ICM20600_OK_Flag = TRUE;
		}
    return data;
}

void ICM20600_reset( void )
{
    uint8_t data=0;
	
		Sensors_I2C_ReadRegister(ICM20600_I2C_ADDR2, ICM20600_USER_CTRL, 1, &data);
    data &= 0xfe;  // ICM20600_USER_CTRL[0] 11111110
    data |= ICM20600_RESET_BIT;
    Sensors_I2C_WriteRegister(ICM20600_I2C_ADDR2, ICM20600_USER_CTRL, 1, &data);
}

void ICM20600_setPowerMode(icm20600_power_type_t mode)
{
    uint8_t data_pwr1;
    uint8_t data_pwr2 = 0x00;
    uint8_t data_gyro_lp;
    Sensors_I2C_ReadRegister(ICM20600_I2C_ADDR2, ICM20600_PWR_MGMT_1, 1, &data_pwr1);
    data_pwr1 &= 0x8f;                  // 0b10001111
    Sensors_I2C_ReadRegister(ICM20600_I2C_ADDR2, ICM20600_GYRO_LP_MODE_CFG, 1, &data_gyro_lp);

    // When set to 1 low-power gyroscope mode is enabled. Default setting is 0
    data_gyro_lp &= 0x7f;               // 0b01111111
    switch(mode)
    {
        case ICM_SLEEP_MODE:
            data_pwr1 |= 0x40;          // set 0b01000000
        break;

        case ICM_STANDYBY_MODE:
            data_pwr1 |= 0x10;          // set 0b00010000
            data_pwr2 = 0x38;           // 0x00111000 disable acc
        break;

        case ICM_ACC_LOW_POWER:
            data_pwr1 |= 0x20;          // set bit5 0b00100000
            data_pwr2 = 0x07;           //0x00000111 disable gyro
        break;

        case ICM_ACC_LOW_NOISE:
            data_pwr1 |= 0x00;     
            data_pwr2 = 0x07;           //0x00000111 disable gyro
        break;

        case ICM_GYRO_LOW_POWER:
            data_pwr1 |= 0x00;          // dont set bit5 0b00000000
            data_pwr2 = 0x38;           // 0x00111000 disable acc
            data_gyro_lp |= 0x80;
        break;

        case ICM_GYRO_LOW_NOISE:
            data_pwr1 |= 0x00;  
            data_pwr2 = 0x38;           // 0x00111000 disable acc
        break;

        case ICM_6AXIS_LOW_POWER:
            data_pwr1 |= 0x00;          // dont set bit5 0b00000000
            data_gyro_lp |= 0x80;
        break;

        case ICM_6AXIS_LOW_NOISE:
            data_pwr1 |= 0x00;  
        break;

        default:
        break;
    }
    Sensors_I2C_WriteRegister(ICM20600_I2C_ADDR2, ICM20600_PWR_MGMT_1, 1, &data_pwr1);
    Sensors_I2C_WriteRegister(ICM20600_I2C_ADDR2, ICM20600_PWR_MGMT_2, 1, &data_pwr2);
    Sensors_I2C_WriteRegister(ICM20600_I2C_ADDR2, ICM20600_GYRO_LP_MODE_CFG, 1, &data_gyro_lp);
}

void ICM20600_setGyroScaleRange(gyro_scale_type_t range)
{
    uint8_t data = 0;
    Sensors_I2C_ReadRegister(ICM20600_I2C_ADDR2, ICM20600_GYRO_CONFIG, 1, &data);
    data &= 0xe7; // 0b11100111

    switch(range)
    {
        case RANGE_250_DPS:
            data |= 0x00;   // 0bxxx00xxx
        break;
        
        case RANGE_500_DPS:
            data |= 0x08;   // 0bxxx00xxx
        break;

        case RANGE_1K_DPS:
            data |= 0x10;   // 0bxxx10xxx
        break;

        case RANGE_2K_DPS:
            data |= 0x18;   // 0bxxx11xxx
        break;

        default:
        break;
    }
    Sensors_I2C_WriteRegister(ICM20600_I2C_ADDR2, ICM20600_GYRO_CONFIG, 1, &data);
}

// for low power mode only
void ICM20600_setGyroAverageSample(gyro_averaging_sample_type_t sample)
{
    uint8_t data = 0;
    Sensors_I2C_ReadRegister(ICM20600_I2C_ADDR2, ICM20600_GYRO_LP_MODE_CFG, 1, &data);

    data &= 0x8f;           // 0b10001111 
    switch(sample)
    {
        case GYRO_AVERAGE_1:
            data |= 0x00; // 0bx000xxxx
        break;

        case GYRO_AVERAGE_2:
            data |= 0x10; // 0bx001xxxx
        break;

        case GYRO_AVERAGE_4:
            data |= 0x20; // 0bx010xxxx
        break;

        case GYRO_AVERAGE_8:
            data |= 0x30; // 0bx011xxxx
        break;

        case GYRO_AVERAGE_16:
            data |= 0x40; // 0bx100xxxx
        break;

        case GYRO_AVERAGE_32:
            data |= 0x50; // 0bx101xxxx
        break;

        case GYRO_AVERAGE_64:
            data |= 0x60;
        break;

        case GYRO_AVERAGE_128:
            data |= 0x70;
        break;

        default:
        break;
    }
    Sensors_I2C_WriteRegister(ICM20600_I2C_ADDR2, ICM20600_GYRO_LP_MODE_CFG, 1, &data);
}

void ICM20600_setGyroOutputDataRate(gyro_lownoise_odr_type_t odr)
{
    uint8_t data;
    Sensors_I2C_ReadRegister(ICM20600_I2C_ADDR2, ICM20600_CONFIG, 1, &data);
    data &= 0xf8;  // DLPF_CFG[2:0] 0b11111000

    switch(odr)
    {
        case GYRO_RATE_8K_BW_3281:
            data |= 0x07;
        break;
        case GYRO_RATE_8K_BW_250:
            data |= 0x00;
        break;
        case GYRO_RATE_1K_BW_176:
            data |= 0x01;
        break;
        case GYRO_RATE_1K_BW_92:
            data |= 0x02;
        break;
        case GYRO_RATE_1K_BW_41:
            data |= 0x03;
        break;
        case GYRO_RATE_1K_BW_20:
            data |= 0x04;
        break;
        case GYRO_RATE_1K_BW_10:
            data |= 0x05;
        break;
        case GYRO_RATE_1K_BW_5:
            data |= 0x06;
        break;
    }
    Sensors_I2C_WriteRegister(ICM20600_I2C_ADDR2, ICM20600_CONFIG, 1, &data);
}

void ICM20600_setAccScaleRange(acc_scale_type_t range)
{
    uint8_t data;
    Sensors_I2C_ReadRegister(ICM20600_I2C_ADDR2, ICM20600_ACCEL_CONFIG, 1, &data);
    data &= 0xe7; // 0b 1110 0111

    switch(range)
    {
        case RANGE_2G:
            data |= 0x00;   // 0bxxx00xxx
        break;
        
        case RANGE_4G:
            data |= 0x08;   // 0bxxx01xxx
        break;

        case RANGE_8G:
            data |= 0x10;   // 0bxxx10xxx
        break;

        case RANGE_16G:
            data |= 0x18;   // 0bxxx11xxx
        break;

        default:
        break;
    }

    Sensors_I2C_WriteRegister(ICM20600_I2C_ADDR2, ICM20600_ACCEL_CONFIG, 1, &data);
}


// for low power mode only
void ICM20600_setAccAverageSample(acc_averaging_sample_type_t sample)
{
    uint8_t data = 0;
    Sensors_I2C_ReadRegister(ICM20600_I2C_ADDR2, ICM20600_ACCEL_CONFIG2, 1, &data);

    data &= 0xcf; // & 0b11001111 
    switch(sample)
    {
        case ACC_AVERAGE_4:
            data |= 0x00; // 0bxx00xxxx
        break;

        case ACC_AVERAGE_8:
            data |= 0x10; // 0bxx01xxxx
        break;

        case ACC_AVERAGE_16:
            data |= 0x20; // 0bxx10xxxx
        break;

        case ACC_AVERAGE_32:
            data |= 0x30; // 0bxx11xxxx
        break;

        default:
        break;
    }

    Sensors_I2C_WriteRegister(ICM20600_I2C_ADDR2, ICM20600_ACCEL_CONFIG2, 1,&data);
}


void ICM20600_setAccOutputDataRate(acc_lownoise_odr_type_t odr)
{
    uint8_t data;
    Sensors_I2C_ReadRegister(ICM20600_I2C_ADDR2, ICM20600_ACCEL_CONFIG2, 1, &data);
    data &= 0xf0;  // 0b11110000

    switch(odr)
    {
        case ACC_RATE_4K_BW_1046:
            data |= 0x08;
        break;

        case ACC_RATE_1K_BW_420:
            data |= 0x07;
        break;

        case ACC_RATE_1K_BW_218:
            data |= 0x01;
        break;

        case ACC_RATE_1K_BW_99:
            data |= 0x02;
        break;

        case ACC_RATE_1K_BW_44:
            data |= 0x03;
        break;

        case ACC_RATE_1K_BW_21:
            data |= 0x04;
        break;

        case ACC_RATE_1K_BW_10:
            data |= 0x05;
        break;

        case ACC_RATE_1K_BW_5:
            data |= 0x06;
        break;

        default:
        break;
    }
    Sensors_I2C_WriteRegister(ICM20600_I2C_ADDR2, ICM20600_ACCEL_CONFIG2, 1, &data);
}

void ICM20600_initialize( void )
{
	  uint8_t data=0x00;
	
	  //I2C初始化
	  I2C_Bus_Init();
	  
	  //读取ID，检测是否正常
	  ICM20600_getDeviceID();
	  if(ICM20600_OK_Flag == FALSE)
		{
		  return;
		}
	
    // configuration
    Sensors_I2C_WriteRegister(ICM20600_I2C_ADDR2, ICM20600_CONFIG, 1, &data);
    // disable fifo
    Sensors_I2C_WriteRegister(ICM20600_I2C_ADDR2, ICM20600_FIFO_EN, 1, &data);

    // set default power mode
    ICM20600_setPowerMode(ICM_6AXIS_LOW_POWER);

    // gyro config
    ICM20600_setGyroScaleRange(RANGE_250_DPS);
    ICM20600_setGyroOutputDataRate(GYRO_RATE_1K_BW_5);
    ICM20600_setGyroAverageSample(GYRO_AVERAGE_1);

    // accel config
    ICM20600_setAccScaleRange(RANGE_2G);
    ICM20600_setAccOutputDataRate(ACC_RATE_1K_BW_5);
    ICM20600_setAccAverageSample(ACC_AVERAGE_4);
}

void ICM20600_coefficientinitialize( gyro_scale_type_t gyro_scale, acc_scale_type_t acc_scale, ICM20600_Data_typedef *ICM20600Sensor_temp )
{
	ICM20600Sensor_temp->GyroX.offset	= 0;
	ICM20600Sensor_temp->GyroY.offset	= 0;
	ICM20600Sensor_temp->GyroZ.offset	= 0;
	ICM20600Sensor_temp->AcceX.offset	= 0;
	ICM20600Sensor_temp->AcceY.offset	= 0;
	ICM20600Sensor_temp->AcceZ.offset	= 0;	
	switch(gyro_scale)
  {
    case RANGE_250_DPS:
				 ICM20600Sensor_temp->GyroX.coefficient	= 131.07;
				 ICM20600Sensor_temp->GyroY.coefficient	= 131.07;
				 ICM20600Sensor_temp->GyroZ.coefficient	= 131.07;
    break;
        
    case RANGE_500_DPS:
				 ICM20600Sensor_temp->GyroX.coefficient	= 65.535;
				 ICM20600Sensor_temp->GyroY.coefficient	= 65.535;
				 ICM20600Sensor_temp->GyroZ.coefficient	= 65.535;
    break;

    case RANGE_1K_DPS:
				 ICM20600Sensor_temp->GyroX.coefficient	= 32.7675;
				 ICM20600Sensor_temp->GyroY.coefficient	= 32.7675;
				 ICM20600Sensor_temp->GyroZ.coefficient	= 32.7675;
    break;

    case RANGE_2K_DPS:
				 ICM20600Sensor_temp->GyroX.coefficient	= 16.38375;
				 ICM20600Sensor_temp->GyroY.coefficient	= 16.38375;
				 ICM20600Sensor_temp->GyroZ.coefficient	= 16.38375;
    break;

    default:
    break;
  }
	
	switch(acc_scale)
  {
    case RANGE_2G:
				 ICM20600Sensor_temp->AcceX.coefficient	= 16383.75;
				 ICM20600Sensor_temp->AcceY.coefficient	= 16383.75;
				 ICM20600Sensor_temp->AcceZ.coefficient	= 16383.75;
    break;
        
    case RANGE_4G:
				 ICM20600Sensor_temp->AcceX.coefficient	= 8191.875;
				 ICM20600Sensor_temp->AcceY.coefficient	= 8191.875;
				 ICM20600Sensor_temp->AcceZ.coefficient	= 8191.875;
    break;

    case RANGE_8G:
				 ICM20600Sensor_temp->AcceX.coefficient	= 4095.9375;
				 ICM20600Sensor_temp->AcceY.coefficient	= 4095.9375;
				 ICM20600Sensor_temp->AcceZ.coefficient	= 4095.9375;
    break;

    case RANGE_16G:
				 ICM20600Sensor_temp->AcceX.coefficient	= 2047.96875;
				 ICM20600Sensor_temp->AcceY.coefficient	= 2047.96875;
				 ICM20600Sensor_temp->AcceZ.coefficient	= 2047.96875;
    break;

    default:
    break;
  }
}


void ICM20600_getSensordata( ICM20600_Data_typedef *ICM20600Sensor_temp )
{
	uint16_t temp1=0,temp2=0,temp3=0,temp4=0,temp5=0,temp6=0,temp7=0;
	Soft_DMP_I2C_Read(ICM20600_I2C_ADDR2,ICM20600_ACCEL_XOUT_H,14,ICM20600Sensor_temp->DataBuffer);
	temp1	= ((ICM20600Sensor_temp->DataBuffer[0])<<8) + ICM20600Sensor_temp->DataBuffer[1];
  ICM20600Sensor_temp->AcceX.data_raw = temp1;
	ICM20600Sensor_temp->AcceX.data_offset = ICM20600Sensor_temp->AcceX.data_raw - ICM20600Sensor_temp->AcceX.offset;
	ICM20600Sensor_temp->AcceX.data			=	(float)ICM20600Sensor_temp->AcceX.data_offset / ICM20600Sensor_temp->AcceX.coefficient;
	
	temp2	= ((ICM20600Sensor_temp->DataBuffer[2])<<8) + ICM20600Sensor_temp->DataBuffer[3];
  ICM20600Sensor_temp->AcceY.data_raw = temp2;
	ICM20600Sensor_temp->AcceY.data_offset = ICM20600Sensor_temp->AcceY.data_raw - ICM20600Sensor_temp->AcceY.offset;
	ICM20600Sensor_temp->AcceY.data			=	(float)ICM20600Sensor_temp->AcceY.data_offset / ICM20600Sensor_temp->AcceY.coefficient;	
	
	temp3	= ((ICM20600Sensor_temp->DataBuffer[4])<<8) + ICM20600Sensor_temp->DataBuffer[5];
	ICM20600Sensor_temp->AcceZ.data_raw = temp3;
	ICM20600Sensor_temp->AcceZ.data_offset = ICM20600Sensor_temp->AcceZ.data_raw - ICM20600Sensor_temp->AcceZ.offset;	
	ICM20600Sensor_temp->AcceZ.data			=	(float)(ICM20600Sensor_temp->AcceZ.data_raw - ICM20600Sensor_temp->AcceZ.offset) / ICM20600Sensor_temp->AcceZ.coefficient;
	
	temp4	= ((ICM20600Sensor_temp->DataBuffer[6])<<8) + ICM20600Sensor_temp->DataBuffer[7];
	ICM20600Sensor_temp->Temp.data_raw = temp4;
	ICM20600Sensor_temp->Temp.data			=	((float)ICM20600Sensor_temp->Temp.data_raw / 326.8) + 25;
	
	temp5	= ((ICM20600Sensor_temp->DataBuffer[8])<<8) + ICM20600Sensor_temp->DataBuffer[9];
	ICM20600Sensor_temp->GyroX.data_raw = temp5;
	ICM20600Sensor_temp->GyroX.data_offset = ICM20600Sensor_temp->GyroX.data_raw - ICM20600Sensor_temp->GyroX.offset;		
	ICM20600Sensor_temp->GyroX.data			=	(float)ICM20600Sensor_temp->GyroX.data_offset / ICM20600Sensor_temp->GyroX.coefficient;
	
	temp6	= ((ICM20600Sensor_temp->DataBuffer[10])<<8) + ICM20600Sensor_temp->DataBuffer[11];
	ICM20600Sensor_temp->GyroY.data_raw = temp6;
	ICM20600Sensor_temp->GyroY.data_offset = ICM20600Sensor_temp->GyroY.data_raw - ICM20600Sensor_temp->GyroY.offset;		
	ICM20600Sensor_temp->GyroY.data			=	(float)ICM20600Sensor_temp->GyroY.data_offset / ICM20600Sensor_temp->GyroY.coefficient;
	
	temp7	= ((ICM20600Sensor_temp->DataBuffer[12])<<8) + ICM20600Sensor_temp->DataBuffer[13];	
	ICM20600Sensor_temp->GyroZ.data_raw = temp7;
	ICM20600Sensor_temp->GyroZ.data_offset = ICM20600Sensor_temp->GyroZ.data_raw - ICM20600Sensor_temp->GyroZ.offset;		
	ICM20600Sensor_temp->GyroZ.data			=	(float)ICM20600Sensor_temp->GyroZ.data_offset / ICM20600Sensor_temp->GyroZ.coefficient;
}

void ICM20600_computePitchAndRollAngle( ICM20600_Data_typedef *ICM20600Sensor_temp )
{
	float PitchTemp=0.0,RollTemp=0.0;
	PitchTemp	= asin(ICM20600Sensor_temp->AcceX.data);
	RollTemp	= atan(0.0 - ICM20600Sensor_temp->AcceZ.data / ICM20600Sensor_temp->AcceY.data);
	ICM20600Sensor_temp->PitchAngle = PitchTemp * 57.3;
	ICM20600Sensor_temp->RollAngle 	= RollTemp  * 57.3;
}      
  
//-------------------------------------------------------------------------------------------
// Fast inverse square-root
// See: http://en.wikipedia.org/wiki/Fast_inverse_square_root
float invSqrt(float x)
{
	float halfx = 0.5f * x;
	float y = x;
	long i = *(long*)&y;
	i = 0x5f3759df - (i>>1);
	y = *(float*)&i;
	y = y * (1.5f - (halfx * y * y));
	y = y * (1.5f - (halfx * y * y));
	return y;
}

//-------------------------------------------------------------------------------------------
void ComputeAngles(void)
{
	ICM20600Sensor.RollAngle = atan2f(q0*q1 + q2*q3, 0.5f - q1*q1 - q2*q2) * 57.29578f;
	ICM20600Sensor.PitchAngle = -asinf(-2.0f * (q1*q3 - q0*q2)) * 57.29578f;
//	yaw = atan2f(q1*q2 + q0*q3, 0.5f - q2*q2 - q3*q3);
	ICM20600Sensor.Pitch = (int16_t)(ICM20600Sensor.PitchAngle * 10);
	ICM20600Sensor.Roll  = (int16_t)(ICM20600Sensor.RollAngle  * 10);
}

void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az)
{
	float recipNorm;
	float halfvx, halfvy, halfvz;
	float halfex, halfey, halfez;
	float qa, qb, qc;

	// Convert gyroscope degrees/sec to radians/sec
	gx *= 0.0174533f;
	gy *= 0.0174533f;
	gz *= 0.0174533f;

	// Compute feedback only if accelerometer measurement valid
	// (avoids NaN in accelerometer normalisation)
	if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) 
	{
		// Normalise accelerometer measurement
		recipNorm = invSqrt(ax * ax + ay * ay + az * az);
		ax *= recipNorm;
		ay *= recipNorm;
		az *= recipNorm;

		// Estimated direction of gravity
		halfvx = q1 * q3 - q0 * q2;
		halfvy = q0 * q1 + q2 * q3;
		halfvz = q0 * q0 - 0.5f + q3 * q3;

		// Error is sum of cross product between estimated
		// and measured direction of gravity
		halfex = (ay * halfvz - az * halfvy);
		halfey = (az * halfvx - ax * halfvz);
		halfez = (ax * halfvy - ay * halfvx);

		// Compute and apply integral feedback if enabled
		if(twoKi > 0.0f) 
		{
			// integral error scaled by Ki
			integralFBx += twoKi * halfex * invSampleFreq;
			integralFBy += twoKi * halfey * invSampleFreq;
			integralFBz += twoKi * halfez * invSampleFreq;
			gx += integralFBx;	// apply integral feedback
			gy += integralFBy;
			gz += integralFBz;
		} 
		else 
		{
			integralFBx = 0.0f;	// prevent integral windup
			integralFBy = 0.0f;
			integralFBz = 0.0f;
		}

		// Apply proportional feedback
		gx += twoKp * halfex;
		gy += twoKp * halfey;
		gz += twoKp * halfez;
	}

	// Integrate rate of change of quaternion
	gx *= (0.5f * invSampleFreq);		// pre-multiply common factors
	gy *= (0.5f * invSampleFreq);
	gz *= (0.5f * invSampleFreq);
	qa = q0;
	qb = q1;
	qc = q2;
	q0 += (-qb * gx - qc * gy - q3 * gz);
	q1 += (qa * gx + qc * gz - q3 * gy);
	q2 += (qa * gy - qb * gz + q3 * gx);
	q3 += (qa * gz + qb * gy - qc * gx);

	// Normalise quaternion
	recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
	q0 *= recipNorm;
	q1 *= recipNorm;
	q2 *= recipNorm;
	q3 *= recipNorm;
}

void BikeAttitude_Process(void)
{
	if((ICM20600_OK_Flag == TRUE) && (MC_ConfigParam2.UseAttitudeAngle_Flag == MC_SUPPORT_ENABLE))
	{
		if(ICM20600Sensor.UpdateEn == ENABLE)/*计算一次5ms*/
		{
			ICM20600Sensor.UpdateEn = DISABLE;
			//计算姿态角度
			ICM20600Sensor.count++;
			if(ICM20600Sensor.count > 199)
			{
				ICM20600Sensor.count = 0;
			}				
			ICM20600_getSensordata(&ICM20600Sensor);
			IMUupdate(ICM20600Sensor.GyroX.data, -ICM20600Sensor.GyroZ.data, ICM20600Sensor.GyroY.data,
								ICM20600Sensor.AcceX.data, -ICM20600Sensor.AcceZ.data, ICM20600Sensor.AcceY.data);
			ComputeAngles();
			MC_AttitudeAngle.Angle_Pitch_Absolute = ICM20600Sensor.Pitch;
			MC_AttitudeAngle.Angle_Pitch_Relative = MC_AttitudeAngle.Angle_Pitch_Absolute - MC_ConfigParam2.ZeroAngle_Pitch;
			
			MC_AttitudeAngle.Angle_Roll_Absolute  = ICM20600Sensor.Roll;
			MC_AttitudeAngle.Angle_Roll_Relative  = MC_AttitudeAngle.Angle_Roll_Absolute - MC_ConfigParam2.ZeroAngle_Roll;
			//判断是否处于爬坡状态
			if(MC_AttitudeAngle.Angle_Pitch_Relative > 40)
			{
				Bike_Attitude.UpWardSlope_True_Timecount++;
				Bike_Attitude.UpWardSlope_False_Timecount=0;
				if(Bike_Attitude.UpWardSlope_True_Timecount > 100)
				{
					Bike_Attitude.UpWardSlope_True_Timecount=0;
					Bike_Attitude.UpWardSlope_flag = TRUE;
				}
			}
			else
			{	
				Bike_Attitude.UpWardSlope_True_Timecount=0;
				Bike_Attitude.UpWardSlope_False_Timecount++;
				if(Bike_Attitude.UpWardSlope_False_Timecount > 100)
				{
					Bike_Attitude.UpWardSlope_False_Timecount=0;
					Bike_Attitude.UpWardSlope_flag = FALSE;
				}
			}
			//判断车辆是否倒下
			if((MC_AttitudeAngle.Angle_Roll_Relative > 450) || (MC_AttitudeAngle.Angle_Roll_Relative < -450))
			{
				Bike_Attitude.FellDown_True_Timecount++;
				Bike_Attitude.FellDown_False_Timecount=0;
				if(Bike_Attitude.FellDown_True_Timecount > 100)
				{
					Bike_Attitude.FellDown_True_Timecount=0;
					Bike_Attitude.FellDown_flag = TRUE;
				}
			}
			else
			{	
				Bike_Attitude.FellDown_True_Timecount=0;
				Bike_Attitude.FellDown_False_Timecount++;
				if(Bike_Attitude.FellDown_False_Timecount > 100)
				{
					Bike_Attitude.FellDown_False_Timecount=0;
					Bike_Attitude.FellDown_flag = FALSE;
				}
			}
		}
	}
	else
	{
	  Bike_Attitude.UpWardSlope_flag = FALSE;
		Bike_Attitude.FellDown_flag = FALSE;
	}
}