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