void Gyro_OFFSET(void)
{
uint16_t cnt_g=0;
int32_t tempgx=100,tempgy=100,tempgz=100;
int16_t gx_last=0,gy_last=0,gz_last=0;
sensor.gyro.quiet.x=0;
sensor.gyro.quiet.y=0;
sensor.gyro.quiet.z=0;
while(tempgx>=100 || tempgy>=100 || tempgz>=100) //此循环是确保四轴处于完全静止状态
{
tempgx=0;tempgy=0;tempgz=0;cnt_g=100;
while(cnt_g--)
{
MPU6050_Read();
sensor.gyro.origin.x = ((((int16_t)mpu6050_buffer[8]) << 8) | mpu6050_buffer[9]);
sensor.gyro.origin.y = ((((int16_t)mpu6050_buffer[10]) << 8)| mpu6050_buffer[11]);
sensor.gyro.origin.z = ((((int16_t)mpu6050_buffer[12]) << 8)| mpu6050_buffer[13]);
tempgx += FL_ABS(sensor.gyro.origin.x - gx_last);
tempgy += FL_ABS(sensor.gyro.origin.y - gy_last);
tempgz += FL_ABS(sensor.gyro.origin.z - gz_last);
gx_last = sensor.gyro.origin.x;
gy_last = sensor.gyro.origin.y;
gz_last = sensor.gyro.origin.z;
}
}
tempgx=0;tempgy=0;tempgz=0;cnt_g=2000;
while(cnt_g--) //此循环进行陀螺仪标定，前提是四轴已经处于完全静止状态
{
MPU6050_Read();
sensor.gyro.origin.x = ((((int16_t)mpu6050_buffer[8]) << 8) | mpu6050_buffer[9]);
sensor.gyro.origin.y = ((((int16_t)mpu6050_buffer[10]) << 8)| mpu6050_buffer[11]);
sensor.gyro.origin.z = ((((int16_t)mpu6050_buffer[12]) << 8)| mpu6050_buffer[13]);
tempgx += sensor.gyro.origin.x;
tempgy += sensor.gyro.origin.y;
tempgz += sensor.gyro.origin.z;
}
sensor.gyro.quiet.x = tempgx/2000;
sensor.gyro.quiet.y = tempgy/2000;
sensor.gyro.quiet.z = tempgz/2000;
}
bool Pos_Is_Value(vs16 *distance,vs16 *last_distance,int top_value)
{
//bool _result=false;
if( FL_ABS(*distance-*last_distance)>top_value )
{
*distance=*last_distance;
//_result=false;
}
// else
// {
// // _result=true;
// }
//
//*last_distance=*distance;
return true;
//return _result;
}
float sonar_filter(bool *_sonar_valid,Ultrasound* _ultra)
{
#define THRESHOLD 0.4//原始数据毛刺阈值
float high;
u8 i=0;
float sum=0;
static float last_ground_distance = 0;
static float last_filtered_high = 0;
static float sonar_high[3];
static u8 high_count=0;
if (*_sonar_valid == true)
{
//非正常跳变
if (FL_ABS(_ultra->KS103High - last_ground_distance)>THRESHOLD)//起始边界
{
last_ground_distance = _ultra->KS103High;
return 0;
}
//正常
else
{
// *_sonar_valid = true;
if(high_count<3)
{
sonar_high[high_count]= _ultra->KS103High;
high_count++;
}
else if(high_count>=3)
{
high_count=3;
sonar_high[2]= _ultra->KS103High;//之后从5插入
for(i=0;i<3;i++)
{
sum+= sonar_high[i];
}
_ultra->NowHigh =sum/3;
//移动数据
for(i=0;i<2;i++)
{
sonar_high[i]=sonar_high[i+1];
}
}
last_ground_distance = _ultra->KS103High;
}
}
//进行完上面的判断后
if (*_sonar_valid == true)
{
//if(Ultra.have_received==true)
{
high = LowPass(true, 0.3, 0.05, _ultra->NowHigh, last_filtered_high);
last_filtered_high = high;
}
}
return high;
}
