diff --git a/libraries/AP_Baro/AP_Baro_SPx06.cpp b/libraries/AP_Baro/AP_Baro_SPx06.cpp index dd21d3ecb68d5a..bf2f6745321a05 100644 --- a/libraries/AP_Baro/AP_Baro_SPx06.cpp +++ b/libraries/AP_Baro/AP_Baro_SPx06.cpp @@ -115,40 +115,43 @@ bool AP_Baro_SPx06::_init() // Sometimes SPx06 has init problems, that's due to failure of reading using SPI for the first time. The SPx06 is a dual // protocol sensor(I2C and SPI), sometimes it takes one SPI operation to convert it to SPI mode after it starts up. - bool is_SPL06 = false; - bool is_SPA06 = false; for (uint8_t i=0; i<5; i++) { if (_dev->read_registers(SPx06_REG_CHIP_ID, &whoami, 1)) { - switch(whoami){ + switch(whoami) { case SPL06_CHIP_ID: - _spx06_type = BARO_SPL06; - is_SPL06=true; + type = Type::SPL06; break; case SPA06_CHIP_ID: - _spx06_type = BARO_SPA06; - is_SPA06=true; + type = Type::SPA06; break; default: + type = Type::UNKNOWN; break; } } - if(is_SPL06 || is_SPA06) + if (type != Type::UNKNOWN) break; } - if(!is_SPL06 && !is_SPA06) { + if (type == Type::UNKNOWN) { return false; } // read the calibration data uint8_t SPx06_CALIB_COEFFS_LEN; - if(_spx06_type == BARO_SPL06) + switch(type) { + case Type::SPL06: SPx06_CALIB_COEFFS_LEN = SPL06_REG_CALIB_COEFFS_END - SPx06_REG_CALIB_COEFFS_START + 1; - - if(_spx06_type == BARO_SPA06) + break; + case Type::SPA06: SPx06_CALIB_COEFFS_LEN = SPA06_REG_CALIB_COEFFS_END - SPx06_REG_CALIB_COEFFS_START + 1; + break; + default: + SPx06_CALIB_COEFFS_LEN = 18; + break; + } uint8_t buf[SPx06_CALIB_COEFFS_LEN]; _dev->read_registers(SPx06_REG_CALIB_COEFFS_START, buf, sizeof(buf)); @@ -162,12 +165,11 @@ bool AP_Baro_SPx06::_init() _c20 = ((uint16_t)buf[12] << 8) | (uint16_t)buf[13]; _c21 = ((uint16_t)buf[14] << 8) | (uint16_t)buf[15]; _c30 = ((uint16_t)buf[16] << 8) | (uint16_t)buf[17]; - if(_spx06_type == BARO_SPA06) { + if(type == Type::SPA06) { _c31 = (buf[18] & 0x80 ? 0xF000 : 0) | ((uint16_t)buf[18] << 4) | (((uint16_t)buf[19] & 0xF0) >> 4); _c40 = ((buf[19] & 0x8 ? 0xF000 : 0) | ((uint16_t)buf[19] & 0x0F) << 8) | (uint16_t)buf[20]; } - // setup temperature and pressure measurements _dev->setup_checked_registers(3, 20); @@ -293,12 +295,19 @@ void AP_Baro_SPx06::_update_pressure(int32_t press_raw) float pressure_cal; float press_temp_comp; - if(_spx06_type == BARO_SPL06) { + switch(type) { + case Type::SPL06: pressure_cal = (float)_c00 + press_raw_sc * ((float)_c10 + press_raw_sc * ((float)_c20 + press_raw_sc * _c30)); press_temp_comp= _temp_raw * ((float)_c01 + press_raw_sc * ((float)_c11 + press_raw_sc * _c21)); - }else { + break; + case Type::SPA06: pressure_cal = (float)_c00 + press_raw_sc * ((float)_c10 + press_raw_sc * ((float)_c20 + press_raw_sc * ((float)_c30 + press_raw_sc * _c40))); press_temp_comp= _temp_raw * ((float)_c01 + press_raw_sc * ((float)_c11 + press_raw_sc * ((float)_c21) + press_raw_sc * _c31)); + break; + default: + pressure_cal = 0; + press_temp_comp= 0; + break; } const float press_comp = pressure_cal + press_temp_comp; diff --git a/libraries/AP_Baro/AP_Baro_SPx06.h b/libraries/AP_Baro/AP_Baro_SPx06.h index 18d3e6c52986a4..37ee8199665598 100644 --- a/libraries/AP_Baro/AP_Baro_SPx06.h +++ b/libraries/AP_Baro/AP_Baro_SPx06.h @@ -18,9 +18,10 @@ class AP_Baro_SPx06 : public AP_Baro_Backend { public: - enum SPx06 { - BARO_SPL06 = 0, - BARO_SPA06 = 1, + enum class Type { + UNKNOWN, + SPL06, + SPA06, }; AP_Baro_SPx06(AP_Baro &baro, AP_HAL::OwnPtr dev); @@ -51,7 +52,7 @@ class AP_Baro_SPx06 : public AP_Baro_Backend int32_t _c00, _c10; int16_t _c0, _c1, _c01, _c11, _c20, _c21, _c30, _c31, _c40; - enum SPx06 _spx06_type; + Type type; }; #endif // AP_BARO_SPx06_ENABLED