AP_Baro: SPx06: check type with switch

ArduPilot · Sep 3, 2024 · c20c1f8 · c20c1f8
1 parent c4dcbba
commit c20c1f8
Show file tree

Hide file tree

Showing 2 changed files with 30 additions and 20 deletions.
diff --git 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
@@ -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<AP_HAL::Device> 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