diff --git a/src/drivers/barometer/ms5611/CMakeLists.txt b/src/drivers/barometer/ms5611/CMakeLists.txt index f67fb270d8..6faa5b7a46 100644 --- a/src/drivers/barometer/ms5611/CMakeLists.txt +++ b/src/drivers/barometer/ms5611/CMakeLists.txt @@ -35,7 +35,6 @@ px4_add_module( MODULE drivers__barometer__ms5611 MAIN ms5611 COMPILE_FLAGS - -Wno-cast-align # TODO: fix and enable SRCS ms5611_spi.cpp ms5611_i2c.cpp diff --git a/src/drivers/barometer/ms5611/ms5611_main.cpp b/src/drivers/barometer/ms5611/ms5611_main.cpp index ee40612f46..1f19f6bfba 100644 --- a/src/drivers/barometer/ms5611/ms5611_main.cpp +++ b/src/drivers/barometer/ms5611/ms5611_main.cpp @@ -107,7 +107,7 @@ extern "C" int ms5611_main(int argc, char *argv[]) #else BusCLIArguments cli {false, true}; #endif - cli.default_spi_frequency = 20 * 1000 * 1000; + cli.default_spi_frequency = 16 * 1000 * 1000; uint16_t dev_type_driver = DRV_BARO_DEVTYPE_MS5611; while ((ch = cli.getOpt(argc, argv, "T:")) != EOF) { diff --git a/src/drivers/barometer/ms5611/ms5611_spi.cpp b/src/drivers/barometer/ms5611/ms5611_spi.cpp index c0109fa1a3..0125715afb 100644 --- a/src/drivers/barometer/ms5611/ms5611_spi.cpp +++ b/src/drivers/barometer/ms5611/ms5611_spi.cpp @@ -115,27 +115,32 @@ MS5611_SPI::init() if (ret != OK) { PX4_DEBUG("SPI init failed"); - goto out; + return PX4_ERROR; } - /* send reset command */ - ret = _reset(); + // reset and read PROM (try up to 3 times) + for (int i = 0; i < 3; i++) { + /* send reset command */ + ret = _reset(); - if (ret != OK) { - PX4_DEBUG("reset failed"); - goto out; + if (ret != OK) { + PX4_DEBUG("reset failed"); + continue; + } + + /* read PROM */ + ret = _read_prom(); + + if (ret == OK) { + return PX4_OK; + + } else { + PX4_DEBUG("prom readout failed"); + continue; + } } - /* read PROM */ - ret = _read_prom(); - - if (ret != OK) { - PX4_DEBUG("prom readout failed"); - goto out; - } - -out: - return ret; + return PX4_ERROR; } int