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(rollAndPitchTrims_t *accelerometerTrims, uint8_t mixerConfiguration)
{
static int16_t gyroYawSmooth = 0;
gyroGetADC();
if (sensors(SENSOR_ACC)) {
updateAccelerationReadings(accelerometerTrims);
getEstimatedAttitude();
} else {
accADC[X] = 0;
accADC[Y] = 0;
accADC[Z] = 0;
}
gyroData[FD_ROLL] = gyroADC[FD_ROLL];
gyroData[FD_PITCH] = gyroADC[FD_PITCH];
if (mixerConfiguration == MULTITYPE_TRI) {
gyroData[FD_YAW] = (gyroYawSmooth * 2 + gyroADC[FD_YAW]) / 3;
gyroYawSmooth = gyroData[FD_YAW];
} else {
gyroData[FD_YAW] = gyroADC[FD_YAW];
}
}
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
}
void Display_balance(void)
{
int16_t x_pos, y_pos;
int8_t roll_axis, pitch_axis;
while(BUTTON1 != 0)
{
ReadAcc();
// Refresh accSmooth values
// Note that because it takes 4.096ms to refresh the whole GLCD this loop cannot run
// faster than 244Hz, but that's close enough to the actual loop time so that the
// actual Acc LPF effect is closely mirrored on the balance meter.
getEstimatedAttitude();
// HORIZONTAL: Pitch = X, Roll = Y
// UPSIDEDOWN: Pitch = X, Roll = Y
// AFT: Pitch = X, Roll = Y
// VERTICAL: Pitch = Y, Roll = X
// SIDEWAYS: Pitch = Y, Roll = X
if ((Config.Orientation == VERTICAL) || (Config.Orientation == SIDEWAYS))
{
roll_axis = PITCH;
pitch_axis = ROLL;
}
else
{
roll_axis = ROLL;
pitch_axis = PITCH;
}
// We need to reverse the polarity reversal so that the meter is once again
// related to the KK2.0, not the model.
// For some reason, pitch has to be reversed on he KK2.1
#ifdef KK21
x_pos = ((int8_t)pgm_read_byte(&Acc_Pol[Config.Orientation][pitch_axis]) * -accSmooth[pitch_axis]) + 32;
#else
x_pos = ((int8_t)pgm_read_byte(&Acc_Pol[Config.Orientation][pitch_axis]) * accSmooth[pitch_axis]) + 32;
#endif
y_pos = ((int8_t)pgm_read_byte(&Acc_Pol[Config.Orientation][roll_axis]) * accSmooth[roll_axis]) + 64;
if (x_pos < 0) x_pos = 0;
if (x_pos > 64) x_pos = 64;
if (y_pos < 0) y_pos = 0;
if (y_pos > 128) y_pos = 128;
// Print bottom markers
LCD_Display_Text(12, (prog_uchar*)Wingdings, 2, 55); // Left
// Draw balance meter
drawrect(buffer, 0, 0, 128, 64, 1); // Border
drawrect(buffer, 54, 22, 21, 21, 1); // Target
drawline(buffer, 64, 8, 64, 56, 1); // Crosshairs
drawline(buffer, 32, 32, 96, 32, 1);
fillcircle(buffer, y_pos, x_pos, 8, 1); // Bubble
// Refresh GLCD
write_buffer(buffer,1);
clear_buffer(buffer);
}
}