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