From fea7040affaed604bef44b2cd81aad181f253d73 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 9 Nov 2016 20:42:29 +1100 Subject: [PATCH] AP_Baro: don't panic when we can't find MS5611 this makes booting a PH2 without its sensor hat on much easier --- libraries/AP_Baro/AP_Baro_MS5611.cpp | 18 +++++++++++------- 1 file changed, 11 insertions(+), 7 deletions(-) diff --git a/libraries/AP_Baro/AP_Baro_MS5611.cpp b/libraries/AP_Baro/AP_Baro_MS5611.cpp index c0f672e38c..4cd81c67a1 100644 --- a/libraries/AP_Baro/AP_Baro_MS5611.cpp +++ b/libraries/AP_Baro/AP_Baro_MS5611.cpp @@ -15,6 +15,7 @@ #include "AP_Baro_MS5611.h" #include +#include #include @@ -61,23 +62,24 @@ AP_Baro_MS56XX::AP_Baro_MS56XX(AP_Baro &baro, AP_HAL::OwnPtr dev void AP_Baro_MS56XX::_init() { if (!_dev) { - AP_HAL::panic("AP_Baro_MS56XX: failed to use device"); + printf("MS5611: no device available"); + return; } - _instance = _frontend.register_sensor(); - if (!_dev->get_semaphore()->take(10)) { AP_HAL::panic("PANIC: AP_Baro_MS56XX: failed to take serial semaphore for init"); } - _dev->transfer(&CMD_MS56XX_RESET, 1, nullptr, 0); - hal.scheduler->delay(4); - uint16_t prom[8]; if (!_read_prom(prom)) { - AP_HAL::panic("Can't read PROM"); + printf("MS5611: Can't read PROM on bus %d", _dev->get_bus_id()); + _dev->get_semaphore()->give(); + return; } + _dev->transfer(&CMD_MS56XX_RESET, 1, nullptr, 0); + hal.scheduler->delay(4); + // Save factory calibration coefficients _cal_reg.c1 = prom[1]; _cal_reg.c2 = prom[2]; @@ -92,6 +94,8 @@ void AP_Baro_MS56XX::_init() memset(&_accum, 0, sizeof(_accum)); + _instance = _frontend.register_sensor(); + _dev->get_semaphore()->give(); /* Request 100Hz update */