[Chris Hibner]

Dr. Brooks' marklars are wise and true

One of the best thing that you can do to help reduce the drift is use a very large average during disabled mode to get the sensor bias, and don't throw out resolution when you're done with the average.

I think I posted a similar example in a different thread within the last couple months, but here is an example of what you want to do.

- assume you want to use a 32-bit variable for your integral.

- Let's assume we want to use a 64 sample average to get the sensor bias. Use the following code to calculate your bias (do this code ONLY in disabled mode).

        
tempAngRate = Get_Analog_Value(angRate);
angRateBias = (angRateBias - angRateArray[angRateBiasCounter]) + tempAngRate;
angRateArray[angRateBiasCounter] = tempAngRate;
angRateBiasCounter++;
if (angRateBiasCounter > 63)
{
    angRateBiasCounter = 0;
}

- Once disable mode is over, the above angularRateBias value contains the bias of the sensor. Just note that it is a 64 sample SUM, but you can think of it as the average * 64. At this point you DO NOT want to divide by 64, since you would then be throwing out the "decimal portion". Just multiply you A/D sample by 64 to bring it up to the same resolution of the bias value. See the following code:

angRateValue = Get_Analog_Value(angRate);
robotHeading += (s32)angRateValue*(s32)64 - angRateBias

Now you have a nice integral with 6 bits worth of fractional-resolution. If you want your fractional portion to be 7 bits, use 128 samples in the average, etc.