Distance timing out

[seanwitte]

Here you go. This based on an algorithm I found on Microchip's web site. It is supposedly optimized for the PIC series chips and will always loop 9 times.

//return the square root of an unsigned 16 bit integer
//based on sample from microchip.com
unsigned int Sqrt16(unsigned int val)
{
	unsigned int mask = 0x100;
	unsigned int result = 0x80;
	
	while (mask > 0)
	{
		if ((result * result) > val)
		{
			result &= ~mask;
		}
		mask >>= 1;
		result |= mask;
	}
	return result;
}

<edit>
Sorry, I wasn't paying attention. You need a 32-bit version:

//Return the square root of value x where x is an integer. This
//is based on the fast square root algorithm from microchip.com. 
unsigned long sqrt32(unsigned long x)
{	
	unsigned long mask = 0x1000;
	unsigned long result = 0x800;

	while (mask > 0)
	{
		if ((result * result) > x)
		{
			result &= (~mask);
		}
		mask >>= 1;
		result |= mask;
	}
	return result;
}

</edit>