AP_Baro: add SPA06 to SPx06 driver

radiolinkW · Aug 29, 2024 · 330993f · 330993f
1 parent d005836
commit 330993f
Show file tree

Hide file tree

Showing 2 changed files with 50 additions and 10 deletions.
diff --git a/libraries/AP_Baro/AP_Baro_SPx06.cpp b/libraries/AP_Baro/AP_Baro_SPx06.cpp
@@ -23,6 +23,7 @@
 extern const AP_HAL::HAL &hal;
 
 #define SPL06_CHIP_ID                          0x10
+#define SPA06_CHIP_ID                          0x11
 
 #define SPx06_REG_PRESSURE_B2                  0x00    // Pressure MSB Register
 #define SPx06_REG_PRESSURE_B1                  0x01    // Pressure middle byte Register
@@ -44,8 +45,8 @@ extern const AP_HAL::HAL &hal;
 #define SPx06_REG_CHIP_ID                      0x0D    // Chip ID Register
 #define SPx06_REG_CALIB_COEFFS_START           0x10
 #define SPL06_REG_CALIB_COEFFS_END             0x21
+#define SPA06_REG_CALIB_COEFFS_END             0x24
 
-#define SPL06_CALIB_COEFFS_LEN                 (SPL06_REG_CALIB_COEFFS_END - SPx06_REG_CALIB_COEFFS_START + 1)
 
 // TEMPERATURE_CFG_REG
 #define SPx06_TEMP_USE_EXT_SENSOR              (1<<7)
@@ -109,21 +110,41 @@ 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)  &&
-            whoami == SPL06_CHIP_ID) {
-            is_SPL06=true;
-            break;
+        if (_dev->read_registers(SPx06_REG_CHIP_ID, &whoami, 1)) {
+        	switch(whoami){
+			case SPL06_CHIP_ID:
+				_spx06_type = BARO_SPL06;
+				is_SPL06=true;
+				break;
+			case SPA06_CHIP_ID:
+				_spx06_type = BARO_SPA06;
+				is_SPA06=true;
+				break;
+			default:
+				break;
+			}
         }
+
+		if(is_SPL06 || is_SPA06)
+			break;
     }
 
-    if(!is_SPL06) {
+    if(!is_SPL06 && !is_SPA06) {
         return false;
     }
 
     // read the calibration data
-    uint8_t buf[SPL06_CALIB_COEFFS_LEN];
+    uint8_t SPx06_CALIB_COEFFS_LEN;
+	if(_spx06_type == BARO_SPL06)
+		SPx06_CALIB_COEFFS_LEN = SPL06_REG_CALIB_COEFFS_END - SPx06_REG_CALIB_COEFFS_START + 1;
+
+	if(_spx06_type == BARO_SPA06)
+		SPx06_CALIB_COEFFS_LEN = SPA06_REG_CALIB_COEFFS_END - SPx06_REG_CALIB_COEFFS_START + 1;
+
+    uint8_t buf[SPx06_CALIB_COEFFS_LEN];
     _dev->read_registers(SPx06_REG_CALIB_COEFFS_START, buf, sizeof(buf));
 
     _c0 = (buf[0] & 0x80 ? 0xF000 : 0) | ((uint16_t)buf[0] << 4) | (((uint16_t)buf[1] & 0xF0) >> 4);
@@ -135,6 +156,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) {
+		_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);
@@ -236,8 +262,16 @@ void AP_Baro_SPx06::_update_temperature(int32_t temp_raw)
 void AP_Baro_SPx06::_update_pressure(int32_t press_raw)
 {
     const float press_raw_sc = (float)press_raw / raw_value_scale_factor(SPx06_PRESSURE_OVERSAMPLING);
-    const float pressure_cal = (float)_c00 + press_raw_sc * ((float)_c10 + press_raw_sc * ((float)_c20 + press_raw_sc * _c30));
-    const float press_temp_comp = _temp_raw * ((float)_c01 + press_raw_sc * ((float)_c11 + press_raw_sc * _c21));
+    float pressure_cal;
+    float press_temp_comp;
+
+    if(_spx06_type == BARO_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 {
+		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));
+	}
 
     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,6 +18,10 @@
 class AP_Baro_SPx06 : public AP_Baro_Backend
 {
 public:
+	enum SPx06 {
+			BARO_SPL06 = 0,
+			BARO_SPA06 = 1,
+	};
     AP_Baro_SPx06(AP_Baro &baro, AP_HAL::OwnPtr<AP_HAL::Device> dev);
 
     /* AP_Baro public interface: */
@@ -45,7 +49,9 @@ class AP_Baro_SPx06 : public AP_Baro_Backend
 
     // Internal calibration registers
     int32_t _c00, _c10;
-    int16_t _c0, _c1, _c01, _c11, _c20, _c21, _c30;
+    int16_t _c0, _c1, _c01, _c11, _c20, _c21, _c30, _c31, _c40;
+
+    enum SPx06 _spx06_type;
 };
 
 #endif  // AP_BARO_SPx06_ENABLED