void computeIMU(void) { static float LastGyroSmooth[3] = { 0.0f, 0.0f, 0.0f }; static int16_t triywavg[4]; static uint8_t triywavgpIDX = 0; static uint32_t prevT; uint32_t curT; uint8_t axis, i; float flttmp; if (MpuSpecial) GETMPU6050(); else { gyro.temperature(&telemTemperature1); // Read out gyro temperature Gyro_getADC(); // Also feeds gyroData if (sensors(SENSOR_ACC)) ACC_getADC(); } curT = micros(); ACCDeltaTimeINS = (float)(curT - prevT) * 0.000001f; // ACCDeltaTimeINS is in seconds now ACCDeltaTimeINS = constrain(ACCDeltaTimeINS, 0.0001f, 0.5f); // Constrain to range 0,1ms - 500ms prevT = curT; if(cfg.acc_calibrated) getEstimatedAttitude(); // acc_calibrated just can turn true if acc present. if(cfg.mixerConfiguration == MULTITYPE_TRI && cfg.gy_smyw) // Moving average for yaw in tri mode { triywavg[triywavgpIDX] = (int16_t)gyroData[YAW]; triywavgpIDX++; if (triywavgpIDX == 4) triywavgpIDX = 0; flttmp = 0; for (i = 0; i < 4; i++) flttmp += triywavg[i]; gyroData[YAW] = flttmp * 0.25f; } if (GyroSmoothing) { for (axis = 0; axis < 3; axis++) { if (SmoothingFactor[axis] > 1) // Circumvent useless action { flttmp = (float)SmoothingFactor[axis]; gyroData[axis] = ((LastGyroSmooth[axis] * (flttmp - 1.0f)) + gyroData[axis]) / flttmp; LastGyroSmooth[axis] = gyroData[axis]; } } } }
void computeIMU(void) { static int16_t gyroYawSmooth = 0; Gyro_getADC(); if (sensors(SENSOR_ACC)) { ACC_getADC(); getEstimatedAttitude(); } else { accADC[X] = 0; accADC[Y] = 0; accADC[Z] = 0; } if (mcfg.mixerConfiguration == MULTITYPE_TRI) { gyroData[YAW] = (gyroYawSmooth * 2 + gyroADC[YAW]) / 3; gyroYawSmooth = gyroData[YAW]; } else { gyroData[YAW] = gyroADC[YAW]; } gyroData[ROLL] = gyroADC[ROLL]; gyroData[PITCH] = gyroADC[PITCH]; }
void computeIMU () { uint8_t axis; static int16_t gyroADCprevious[3] = {0,0,0}; int16_t gyroADCp[3]; int16_t gyroADCinter[3]; static uint32_t timeInterleave = 0; //we separate the 2 situations because reading gyro values with a gyro only setup can be acchieved at a higher rate //gyro+nunchuk: we must wait for a quite high delay betwwen 2 reads to get both WM+ and Nunchuk data. It works with 3ms //gyro only: the delay to read 2 consecutive values can be reduced to only 0.65ms #if defined(NUNCHUCK) annexCode(); while((micros()-timeInterleave)<INTERLEAVING_DELAY) ; //interleaving delay between 2 consecutive reads timeInterleave=micros(); ACC_getADC(); getEstimatedAttitude(); // computation time must last less than one interleaving delay while((micros()-timeInterleave)<INTERLEAVING_DELAY) ; //interleaving delay between 2 consecutive reads timeInterleave=micros(); f.NUNCHUKDATA = 1; while(f.NUNCHUKDATA) ACC_getADC(); // For this interleaving reading, we must have a gyro update at this point (less delay) for (axis = 0; axis < 3; axis++) { // empirical, we take a weighted value of the current and the previous values // /4 is to average 4 values, note: overflow is not possible for WMP gyro here gyroData[axis] = (gyroADC[axis]*3+gyroADCprevious[axis])/4; gyroADCprevious[axis] = gyroADC[axis]; } #else #if ACC ACC_getADC(); getEstimatedAttitude(); #endif #if GYRO Gyro_getADC(); #endif for (axis = 0; axis < 3; axis++) gyroADCp[axis] = gyroADC[axis]; timeInterleave=micros(); annexCode(); if ((micros()-timeInterleave)>650) { annex650_overrun_count++; } else { while((micros()-timeInterleave)<650) ; //empirical, interleaving delay between 2 consecutive reads } #if GYRO Gyro_getADC(); #endif for (axis = 0; axis < 3; axis++) { gyroADCinter[axis] = gyroADC[axis]+gyroADCp[axis]; // empirical, we take a weighted value of the current and the previous values gyroData[axis] = (gyroADCinter[axis]+gyroADCprevious[axis])/3; gyroADCprevious[axis] = gyroADCinter[axis]/2; if (!ACC) accADC[axis]=0; } #endif #if defined(GYRO_SMOOTHING) static int16_t gyroSmooth[3] = {0,0,0}; for (axis = 0; axis < 3; axis++) { gyroData[axis] = (int16_t) ( ( (int32_t)((int32_t)gyroSmooth[axis] * (conf.Smoothing[axis]-1) )+gyroData[axis]+1 ) / conf.Smoothing[axis]); gyroSmooth[axis] = gyroData[axis]; } #elif defined(TRI) static int16_t gyroYawSmooth = 0; gyroData[YAW] = (gyroYawSmooth*2+gyroData[YAW])/3; gyroYawSmooth = gyroData[YAW]; #endif }