void rotateDegrees(int degrees) { initTimer(); degrees_rotated = 0; // Reset degrees rotated so we make sure not to rotate to much degrees = degrees * 100; // To compensate for the value returned by updateGyroData being 100 times bigger than it should while (abs(degrees_rotated) < degrees) { // Rotate until we reach the requested amount of degrees rotated updateGyroData(); } send_REQ(); // Alert our huvudenhet that we're done and should stop spinning SPI_Send(0x01); SPI_Send(0x00); }
static void sample() { AccSampleSlideBuf* slideBuf = (AccSampleSlideBuf*)(globalAttitudeSensor.accSlideBuf) ; SysTime startTime , endTime ; getSysTime(&startTime); // 对加速度计采样,并低通滤波+滑动平均滤波 updateAccelerometer() ; globalAttitudeSensor.accSamplesCount ++ ; int curPos = slideBuf->curPos ; int nextPos = curPos + 1 ; if (nextPos==SLIDE_BUF_LEN) nextPos = 0 ; int oldX = slideBuf->x[nextPos] ; int oldY = slideBuf->y[nextPos] ; int oldZ = slideBuf->z[nextPos] ; int ratio = LOW_PASS_RATIO ; //低通差分公式的比率因子的倒数, int ratio2 = ratio - 1 ; // 低通滤波 slideBuf->x[nextPos] = ( slideBuf->x[curPos]*ratio2 + 1000*globalAttitudeSensor.ax ) / ratio ; slideBuf->y[nextPos] = ( slideBuf->y[curPos]*ratio2 + 1000*globalAttitudeSensor.ay ) / ratio ; slideBuf->z[nextPos] = ( slideBuf->z[curPos]*ratio2 + 1000*globalAttitudeSensor.az ) / ratio ; slideBuf->curPos = nextPos ; // 更新各轴的sum和滑动平均值和跟随预测值 int32_t deltaX = slideBuf->x[nextPos] - oldX ; int32_t deltaY = slideBuf->y[nextPos] - oldY ; int32_t deltaZ = slideBuf->z[nextPos] - oldZ ; slideBuf->sumX += deltaX ; slideBuf->sumY += deltaY ; slideBuf->sumZ += deltaZ ; slideBuf->aveX = (slideBuf->sumX) / (SLIDE_BUF_LEN) ; slideBuf->aveY = (slideBuf->sumY) / (SLIDE_BUF_LEN) ; slideBuf->aveZ = (slideBuf->sumZ) / (SLIDE_BUF_LEN) ; slideBuf->followPredictX = slideBuf->aveX + deltaX ; slideBuf->followPredictY = slideBuf->aveY + deltaY ; slideBuf->followPredictZ = slideBuf->aveZ + deltaZ ; // 对陀螺仪采样 updateGyroData(); if ( ( globalAttitudeSensor.accSamplesCount % globalAttitudeSensor.M_sample_Period )==0 ) { float Ax, Ay, Az ; float Mx, My, Mz ; float x, y, z ; updateMagnetometer(); // 磁场低通滤波 float ratio = MagFilterRatio ; x = globalAttitudeSensor.filtered_mx - globalAttitudeSensor.mx ; y = globalAttitudeSensor.filtered_my - globalAttitudeSensor.my ; z = globalAttitudeSensor.filtered_mz - globalAttitudeSensor.mz ; float d = sqrt(x*x+y*y+z*z); if ( d < 0.04f*500 ) ratio = 0.01f ; else if ( d < 0.08f*500 ) ratio = 0.02f ; else if ( d < 0.2f*500 ) ratio = 0.1f ; else ratio = 0.25f ; globalAttitudeSensor.filtered_mx = globalAttitudeSensor.filtered_mx * (1.0f-ratio) + globalAttitudeSensor.mx * ratio ; globalAttitudeSensor.filtered_my = globalAttitudeSensor.filtered_my * (1.0f-ratio) + globalAttitudeSensor.my * ratio ; globalAttitudeSensor.filtered_mz = globalAttitudeSensor.filtered_mz * (1.0f-ratio) + globalAttitudeSensor.mz * ratio ; // 磁场水平分量(再滤波) Ax = getAccXStable(&globalAttitudeSensor) ; Ay = getAccYStable(&globalAttitudeSensor) ; Az = getAccZStable(&globalAttitudeSensor) ; Mx = globalAttitudeSensor.filtered_mx ; My = globalAttitudeSensor.filtered_my ; Mz = globalAttitudeSensor.filtered_mz ; x = Ay*Mz - My*Az ; y = Az*Mx - Mz*Ax ; z = Ax*My - Mx*Ay ; Mx = -x ; My = -y ; Mz = -z ; x = Ay*Mz - My*Az ; y = Az*Mx - Mz*Ax ; z = Ax*My - Mx*Ay ; float tmp = 1.0f / sqrt(x*x + y*y + z*z) ; x *= tmp ; y *= tmp ; z *= tmp ; ratio = 0.05 ; globalAttitudeSensor.filtered_Hmx = x*ratio + globalAttitudeSensor.filtered_Hmx*(1-ratio) ; globalAttitudeSensor.filtered_Hmy = y*ratio + globalAttitudeSensor.filtered_Hmy*(1-ratio) ; globalAttitudeSensor.filtered_Hmz = z*ratio + globalAttitudeSensor.filtered_Hmz*(1-ratio) ; } getSysTime(&endTime); sampleTime = getDeltaTime(&endTime,&startTime); }