Ms5611 fixes

This fixes several problem with the ms5611 driver:
* race condition when running the PIOS_MS5611_Test()
* uninitialized memory usage in PIOS_MS5611_Test()

Also i removed unused variables and cleanup a bit.

Todo:
* Let the init code run some hours to verfiy there is no more init failure
* Run this using fpu exception catching code to verify there are no more NaNs