I was investigating the reasons why WMC heading hold mode (without MAG) not as good as commercial heli gyros, and looks like I found the reason. In the computeIMU() procedure we are dividing the actual Gyro output by 8. This is equivalent of dropping 3 LSB. After that the resolution of the ITG3200, for example, would be around 2 deg/sec, everything below that we just cannot "see". So my idea is really simple:
1) In the computeIMU() divide values by 2 (we still need some space in int16_t)
Code: Select all
void computeIMU () {
uint8_t axis;
static int16_t gyroADCprevious[3] = {0,0,0};
static int16_t gyroADCp[3] = {0,0,0};
int16_t gyroADCinter[3];
static int16_t lastAccADC[3] = {0,0,0};
static int16_t similarNumberAccData[3];
static int16_t gyroDeviation[3];
static uint32_t timeInterleave;
static int16_t gyroYawSmooth = 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 (nunchukPresent) {
annexCode();
while((micros()-timeInterleave)<INTERLEAVING_DELAY) ; //interleaving delay between 2 consecutive reads
timeInterleave=micros();
updateIMU(0);
getEstimatedAttitude(); // computation time must last less than one interleaving delay
while((micros()-timeInterleave)<INTERLEAVING_DELAY) ; //interleaving delay between 2 consecutive reads
timeInterleave=micros();
while(updateIMU(0) != 1) ; // 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
gyroData[axis] = (gyroADC[axis]*3+gyroADCprevious[axis]+4)/4/2; // /4 is to average 4 values ; /2 is to reserve 1 bit
gyroADCprevious[axis] = gyroADC[axis];
}
} else {
#if defined(I2C_ACC) || defined(ADCACC)
updateIMU(1); //with I2C or ADC ACC
getEstimatedAttitude();
#else
updateIMU(0); //without ACC
#endif
for (axis = 0; axis < 3; axis++)
gyroADCp[axis] = gyroADC[axis];
timeInterleave=micros();
annexCode();
while((micros()-timeInterleave)<650) ; //empirical, interleaving delay between 2 consecutive reads
updateIMU(0); //without ACC
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)/3/2; // /3 is to average 3 values ; /2 is to reserve 1 bit
gyroADCprevious[axis] = gyroADCinter[axis]/2;
#if not defined (I2C_ACC) && not defined (ADCACC)
accADC[axis]=0;
#endif
}
}
#if defined(TRI)
gyroData[YAW] = (gyroYawSmooth*2+gyroData[YAW]+1)/3;
gyroYawSmooth = gyroData[YAW];
#endif
}
2) Adjust PID controllers to divide gyro readings by 4 AFTER multiplications. This way we would still have gyro values properly scaled, but we preserve 2 bits of resolution:
Code: Select all
//**** PITCH & ROLL & YAW PID ****
for(axis=0;axis<3;axis++) {
if (accMode == 1 && axis<2 ) { //LEVEL MODE
errorAngle = rcCommand[axis]/2 - angle[axis]/2;
PTerm = (errorAngle)*PLEVEL8/50 - gyroData[axis]*dynP8[axis]/10/4;
errorAngleI[axis] += errorAngle;
errorAngleI[axis] = constrain(errorAngleI[axis],-5000,+5000); //WindUp
ITerm = errorAngleI[axis] *ILEVEL8/2000;
} else { //ACRO MODE or YAW axis
error = rcCommand[axis]*10*4/P8[axis] - gyroData[axis];
PTerm = rcCommand[axis]-gyroData[axis]*dynP8[axis]/10/4;
errorGyroI[axis] += error;
errorGyroI[axis] = constrain(errorGyroI[axis],-8000,+8000); //WindUp
if (abs(gyroData[axis])>320) errorGyroI[axis] = 0;
ITerm = errorGyroI[axis]*I8[axis]/1000/4;
}
delta = gyroData[axis] - lastGyro[axis];
DTerm = (delta1[axis]+delta2[axis]+delta+1)*dynD8[axis]/3/4;
delta2[axis] = delta1[axis];
delta1[axis] = delta;
lastGyro[axis] = gyroData[axis];
axisPID[axis] = PTerm + ITerm - DTerm;
}
The flight tests was quite impressive. In the room, I was able to hover couple of minutes without rudder corrections. Additionally, I have impression, that everything is much more smooth and precise.
Notes:
1) The Gyro values in the GUI would be over the range and limited by 500.
2) Most probably Gyro readings would not be 0 any more
3) It is not necessary to re-adjust PIDs
regards,
ziss_dm