void MainUI::paintGL()
{
updateAccelerometer();
UpdateInputState(&input_state);
time_update();
UpdateRunLoop(&input_state);
}
void initializeAccelerometer(AccelerometerDefinition *accelerometer) {
accelerometer->xSum = 0;
accelerometer->ySum = 0;
accelerometer->zSum = 0;
accelerometer->xAvg = 0;
accelerometer->yAvg = 0;
accelerometer->zAvg = 0;
P1OUT &= ~(BIT0 + BIT1 + BIT2);
P1DIR &= ~(BIT0 + BIT1 + BIT2);
// From p558 of MSP User guide:
// "The DTC is enabled by setting the ADC10DTC1 register to a nonzero value."
// ADC10DTC1 = ~0;
ADC10CTL1 = INCH_2 + CONSEQ_3; // A2/A1/A0, repeat multi channel
ADC10CTL0 = ADC10SHT_2 + MSC + ADC10ON + ADC10IE;
ADC10AE0 = (BIT0 + BIT1 + BIT2); // P1.0,1, 2 Analog enable
ADC10DTC1 = 3; // number of conversions
int i;
for (i = 0; i < 8; i++)
updateAccelerometer(accelerometer);
}
void loopMain() {
refreshWatchdog();
unsigned long startMicros = micros();
updateAccelerometer();
// display lights
lights->reset();
controller->paint(*lights);
capOverallBrightness(*lights);
lights->show();
unsigned long endMicros = micros();
avgTime = (15*avgTime + endMicros - startMicros)/16;
int timeToDelay = (int)(10 - (endMicros - startMicros)/1000);
if (timeToDelay > 0)
delay(timeToDelay);
int blink = (millis() /100)%2;
digitalWrite(ONBOARD_LED_PIN, blink);
if (count++ >= 100) {
// Print head size for debugging.
#ifdef RN_PRINT_HEAP_SIZE
info->printf("Avg time = %5d, delay = %dms, heapSize = %d\n",
avgTime, timeToDelay, heapSize());
count = 0;
#endif /* RN_PRINT_HEAP_SIZE */
}
// Serial.println(millis()/10);
}
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);
}
