iPhone 3G and GPS

I love my iPhone, but there is one thing I'm struggling with: GPS
I think it doesn't work since I haven't been able to get an accurate position in Maps. The triangulation based on cells work, but that's only accurate up to about 1700m and doesn't really match the current position.
I've checked the boards and see that it may be firmware related. However, I'd like to be sure. Is there a diagnostics tool available to check if the gps hardware is actually working?
Maybe I should download the SDK and write a tool myself (I bet there are some calls to check if there is an active GPS module)
In any case, I'm slightly worried it may be the hardware. I don't want to return the iPhone and have to wait for week to get it back.
-- Sent from my iPhone
views