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
}
