...
#define SAMPLE_RATE	40/200
...

void AdvancedAccumulatorFinal()
{
	unsigned int gyrovalue = 0;
	unsigned static int prevgyro;
	unsigned static char calibrated;
	unsigned static char samplecount;
	unsigned char gyrobase;	

	// Sample the gyro’s output
	gyrovalue = Get_Analog_Value(GYRO_INPUT);

	// Handle the oversampling here
	gyrovalue = gyrovalue * SAMPLE_RATE; // Be explicit!
	// Is it too high?
	if(gyrovalue > GYRO_MAX_HIGH || gyrovalue < GYRO_MAX_LOW)
	{
		// Set global Gyro overloaded flag so you know
		gyro_overloaded = 1;
		// I don't recommend handling anything else here, let
		// something else see the flag and deal with it.
	}
	

	// Has the gyro gone through it's power up calibration yet?
	if(!calibrated)
	{ // It hasn't been calibrated
		if(samplecount < CAL_SAMPLES)
		{ // Have we finished sampling?
			gyrobase += gyrovalue; // Accumulate samples
		}
		else 
		{ // Let's average it now
			// Divide samples to get average reading.
			gyrobase /= CAL_SAMPLES;
			// Tell the gyro it’s calibrated
			calibrated = 1;
		}
	}
	else // It has been calibrated.
	{ // Accumulate it

		// Now we want to convert the gyro output to signed.
		// We do this by subtracting gyrobase, making the 
		// reading center around 0.
		// Positive is one direction, negative another, then 
		// we just integrate it
		// Now we're also averaging the current sample with
		// the previous before normalizing it.
	
		// The following line is split over two for
		// readability
		heading += (signed long)(((gyrovalue + prevgyro) / 2)
	   						               – gyrobase);
	}
	// Last thing we do is store the gyrovalue for next time 
	prevgyro = gyrovalue;
}