spa06.c

Go to the documentation of this file.

/*
 * Copyright (C) 2026 OpenUAS
 * Thanks to Florian Sansou florian.sansou@enac.fr for initial implementation
 *
 * This file is part of paparazzi.
 *
 * paparazzi is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2, or (at your option)
 * any later version.
 *
 * paparazzi is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with paparazzi; see the file COPYING.  If not, see
 * <http://www.gnu.org/licenses/>.
 */
 
#include "peripherals/spa06.h"
 
/* Oversampling selection: 64x pressure (high precision, ~104ms/conversion) with
 * 1x temperature (3.6ms; more adds nothing, it only feeds the compensation).
 * At the 4Hz rates written in spa06_config() this uses ~430ms/s of the sensor's
 * 1s/s conversion budget. raw_value_scale_factor() adapts automatically. */
#define SPL06_PRESSURE_OVERSAMPLING SPL06_OVERSAMPLING_64X_P
#define SPL06_TEMPERATURE_OVERSAMPLING SPL06_OVERSAMPLING_1X_T
 
/* Reliability limits */
#define SPA06_BROKEN_RETRY_US    5000000   
#define SPA06_MAX_ERROR_CNT      10        
#define SPA06_PRESSURE_MIN_PA    30000.0f  
#define SPA06_PRESSURE_MAX_PA    120000.0f 
 
/* Local functions: raw data extraction, calibration handling, compensation
 * and bus access helpers. See the definitions below for details. */
static void parse_sensor_data(struct spa06_t *spa, volatile uint8_t *data);
static void parse_calib_data(struct spa06_t *spa, volatile uint8_t *coef);
static void compensate_pressure(struct spa06_t *spa);
static bool spa06_config(struct spa06_t *spa);
static bool spa06_get_calib(struct spa06_t *spa);
static int32_t raw_value_scale_factor(uint8_t oversampling);
static void spa06_register_write(struct spa06_t *spa, uint8_t reg, uint8_t value);
static void spa06_register_read(struct spa06_t *spa, uint8_t reg, uint16_t size);
static int32_t getTwosComplement(uint32_t raw, uint8_t length);
 
void spa06_init(struct spa06_t *spa)
{
  spa->data_available = false;
  spa->initialized = false;
  spa->is_broken = false;
  spa->status = SPA06_STATUS_UNINIT;
  spa->device = SPA06_UNKNOWN;
  spa->reset = false;
  spa->init_error_cnt = 0;
  spa->config_idx = 0;
  spa->calib_idx = 0;
  spa->tmp_coef_srce = 0;
  spa->timer = 0;
 
  /* SPI setup */
  if (spa->bus == SPA06_SPI) {
    spa->spi.trans.cpol = SPICpolIdleHigh;
    spa->spi.trans.cpha = SPICphaEdge2;
    spa->spi.trans.dss = SPIDss8bit;
    spa->spi.trans.bitorder = SPIMSBFirst;
    spa->spi.trans.cdiv = SPIDiv16;
 
    spa->spi.trans.select = SPISelectUnselect;
    spa->spi.trans.slave_idx = spa->spi.slave_idx;
    spa->spi.trans.output_length = 0;
    spa->spi.trans.input_length = 0;
    spa->spi.trans.before_cb = NULL;
    spa->spi.trans.after_cb = NULL;
    spa->spi.trans.input_buf = spa->spi.rx_buf;
    spa->spi.trans.output_buf = spa->spi.tx_buf;
    spa->spi.trans.status = SPITransDone;
 
    // in SPI read, the first byte is garbage because writing the register address
    spa->rx_buffer = (volatile uint8_t *)&spa->spi.rx_buf[1];
    spa->tx_buffer = (volatile uint8_t *)spa->spi.tx_buf;
  }
  /* I2C setup */
  else {
 
    spa->i2c.trans.slave_addr = spa->i2c.slave_addr; // slave address
    spa->i2c.trans.status = I2CTransDone;  // set initial status: Done
 
    spa->rx_buffer = spa->i2c.trans.buf;
    spa->tx_buffer = spa->i2c.trans.buf;
 
  }
}
 
void spa06_periodic(struct spa06_t *spa)
{
  if (spa->is_broken) {
    // Back off, but periodically retry a full re-detection instead of giving up forever
    if ((uint32_t)(get_sys_time_usec() - spa->timer) < SPA06_BROKEN_RETRY_US) {
      return;
    }
    spa->is_broken = false;
    spa->init_error_cnt = 0;
    spa->status = SPA06_STATUS_UNINIT;
    spa->reset = false;
  }
 
  /* Idle */
  if ((spa->bus == SPA06_SPI && spa->spi.trans.status == SPITransDone) ||
      (spa->bus == SPA06_I2C && spa->i2c.trans.status == I2CTransDone)) {
 
    switch (spa->status) {
      case SPA06_STATUS_UNINIT:
        spa->data_available = false;
        spa->initialized = false;
        if (spa->reset == false) {
          spa->config_idx = 0;
          spa->calib_idx = 0;
          spa06_register_write(spa, SPL06_REG_RST, SPL06_RESET_BIT_SOFT_RST);
          spa->timer = get_sys_time_usec();
          spa->reset = true;
        }
        if (spa->reset == true) {
          // Unsigned arithmetic handles a wrap-around of the time counter
          uint32_t diff_val = get_sys_time_usec() - spa->timer;
          if (diff_val < 40000) {
            spa->status = SPA06_STATUS_UNINIT; //Stay in uninit state for 40ms after reset
            break;
          }
          spa->reset = false;
          spa->status = SPA06_STATUS_IDLE;
        }
        break;
 
      case SPA06_STATUS_IDLE:
        /* Request WHO_AM_I */
        spa06_register_read(spa, SPL06_REG_CHIP_ID, 1);
        break;
 
 
      case SPA06_STATUS_INIT_OK:
        spa06_register_read(spa, SPL06_REG_MODE_AND_STATUS, 1);
        break;
 
      case SPA06_STATUS_GET_COEF_SRCE:
        spa06_register_read(spa, SPL06_REG_COEF_SRCE, 1);
        break;
 
      case SPA06_STATUS_GET_CALIB:
        // request calibration data
        if (spa06_get_calib(spa)) {
          spa->status = SPA06_STATUS_CONFIGURE;
        }
        //process in spa06_event()
        break;
 
      case SPA06_STATUS_CONFIGURE:
        if (spa06_config(spa)) {
          spa->status = SPA06_STATUS_READ_STATUS_REG;
          spa->initialized = true;
          spa->init_error_cnt = 0;
        }
        break;
 
      case  SPA06_STATUS_READ_STATUS_REG:
        // READ THE STATUS BYTE
        spa06_register_read(spa, SPL06_REG_MODE_AND_STATUS, 1);
        break;
 
      case  SPA06_STATUS_READ_DATA_REGS:
        // READ ALL 6 DATA REGISTERS
        spa06_register_read(spa, SPL06_REG_PRESSURE_B2, SPL06_PRESSURE_LEN + SPL06_TEMPERATURE_LEN);
        break;
 
      default:
        break;
    }
  }
}
 
void spa06_event(struct spa06_t *spa)
{
  /* Successful transfer */
  if ((spa->bus == SPA06_SPI && spa->spi.trans.status == SPITransSuccess) ||
      (spa->bus == SPA06_I2C && spa->i2c.trans.status == I2CTransSuccess)) {
    switch (spa->status) {
      case SPA06_STATUS_UNINIT:
        spa->reset = true; // Reset command acknowledged, wait for the sensor to restart
        break;
 
      case SPA06_STATUS_IDLE: {
        /* WHO_AM_I */
        uint8_t chip_id = spa->rx_buffer[0];
        if (chip_id == SPA06_CHIP_ID) {
          spa->device = SPA06;
          spa->status = SPA06_STATUS_INIT_OK;
          spa->init_error_cnt = 0;
        } else if (chip_id == SPL06_CHIP_ID) {
          spa->device = SPL06;
          spa->status = SPA06_STATUS_INIT_OK;
          spa->init_error_cnt = 0;
        } else {
          // Invalid chip ID, sensor might be disconnected or corrupt
          spa->init_error_cnt++;
          if (spa->init_error_cnt >= SPA06_MAX_ERROR_CNT) {
            spa->is_broken = true;  // Back off, retried after SPA06_BROKEN_RETRY_US
            spa->timer = get_sys_time_usec();
          } else {
            spa->status = SPA06_STATUS_UNINIT;
            spa->reset = false;
          }
        }
        break;
      }
 
      case SPA06_STATUS_INIT_OK: {
        uint8_t status = spa->rx_buffer[0];
        if ((status & (SPL06_MEAS_CFG_COEFFS_RDY | SPL06_MEAS_CFG_SENSOR_RDY)) == (SPL06_MEAS_CFG_COEFFS_RDY |
            SPL06_MEAS_CFG_SENSOR_RDY)) {
          spa->status = SPA06_STATUS_GET_COEF_SRCE;
        }
        break;
      }
 
      case SPA06_STATUS_GET_COEF_SRCE:
        // The temperature measurement must use the same sensor as was used for
        // the factory calibration coefficients (TMP_COEF_SRCE, mirrored into TMP_CFG bit 7)
        spa->tmp_coef_srce = spa->rx_buffer[0] & SPL06_COEF_SRCE_BIT_TMP_COEF_SRCE;
        spa->status = SPA06_STATUS_GET_CALIB;
        break;
 
      case SPA06_STATUS_GET_CALIB:
        // compute calib
        parse_calib_data(spa, &spa->rx_buffer[0]);
        break;
 
      case SPA06_STATUS_CONFIGURE:
        /* Do nothing. We let spa06_periodic() detect that the transaction
           is I2CTransDone and submit the next config on the next tick */
        break;
 
      case SPA06_STATUS_READ_STATUS_REG:
        // check status byte
        if ((spa->rx_buffer[0] & (SPL06_MEAS_CFG_PRESSURE_RDY | SPL06_MEAS_CFG_TEMPERATURE_RDY)) ==
            (SPL06_MEAS_CFG_PRESSURE_RDY | SPL06_MEAS_CFG_TEMPERATURE_RDY)) {
          spa->status = SPA06_STATUS_READ_DATA_REGS;
        }
        break;
 
      case SPA06_STATUS_READ_DATA_REGS:
        // parse sensor data, compensate temperature first, then pressure
        parse_sensor_data(spa, &spa->rx_buffer[0]);
        compensate_pressure(spa);
        spa->init_error_cnt = 0; // successful transaction, reset the consecutive failure counter
        // Only publish values within the specified measurement range of the sensor,
        // anything outside means the data got corrupted on its way here
        if ((spa->pressure >= SPA06_PRESSURE_MIN_PA) && (spa->pressure <= SPA06_PRESSURE_MAX_PA)) {
          spa->data_available = true;
        }
        spa->status = SPA06_STATUS_READ_STATUS_REG;
        break;
 
      default:
        break;
    }
    if (spa->bus == SPA06_I2C) {
      spa->i2c.trans.status = I2CTransDone;
    } else {
      spa->spi.trans.status = SPITransDone;
    }
 
  } else if ((spa->bus == SPA06_SPI && spa->spi.trans.status == SPITransFailed) ||
             (spa->bus == SPA06_I2C && spa->i2c.trans.status == I2CTransFailed)) {
    spa->init_error_cnt++;
    if (!spa->initialized) {
      /* Failure during initialization: count and eventually back off */
      if (spa->init_error_cnt >= SPA06_MAX_ERROR_CNT) {
        spa->is_broken = true;  // Back off, retried after SPA06_BROKEN_RETRY_US
        spa->timer = get_sys_time_usec();
      }
      spa->status = SPA06_STATUS_UNINIT;
      spa->reset = false;  // Ensure reset sequence runs again
      // Hot-swap address (0xEC <-> 0xEE) on init failure to prevent deadlocking the I2C queue on wrong assignments
      if (spa->bus == SPA06_I2C && !spa->is_broken) {
        spa->i2c.slave_addr = (spa->i2c.slave_addr == SPA06_I2C_ADDR) ? SPA06_I2C_ADDR_ALT : SPA06_I2C_ADDR;
      }
    } else if (spa->init_error_cnt >= SPA06_MAX_ERROR_CNT) {
      /* Too many consecutive failures during measurements: full re-initialization */
      spa->initialized = false;
      spa->data_available = false;
      spa->init_error_cnt = 0;
      spa->status = SPA06_STATUS_UNINIT;
      spa->reset = false;
    }
    /* Otherwise a transient failure simply retries the current read on the next periodic tick */
 
    if (spa->bus == SPA06_I2C) {
      spa->i2c.trans.status = I2CTransDone;
    } else {
      spa->spi.trans.status = SPITransDone;
    }
  }
 
  return;
}
 
static void parse_sensor_data(struct spa06_t *spa, volatile uint8_t *data)
{
  spa->raw_pressure = getTwosComplement(((uint32_t)data[0] << 16) | ((uint32_t)data[1] << 8) | (uint32_t)data[2], 24);
  spa->raw_temperature = getTwosComplement(((uint32_t)data[3] << 16) | ((uint32_t)data[4] << 8) | (uint32_t)data[5], 24);
}
 
 
static void parse_calib_data(struct spa06_t *spa, volatile uint8_t *coef)
{
  switch (spa->calib_idx) {
    case 0:
      // 0x11 c0 [3:0] + 0x10 c0 [11:4]
      spa->calib.c0 = getTwosComplement(((uint32_t)coef[0] << 4) | (((uint32_t)coef[1] >> 4) & 0x0F), 12);
      // 0x11 c1 [11:8] + 0x12 c1 [7:0]
      spa->calib.c1 = getTwosComplement((((uint32_t)coef[1] & 0x0F) << 8) | (uint32_t)coef[2], 12);
 
      // 0x13 c00 [19:12] + 0x14 c00 [11:4] + 0x15 c00 [3:0]
      spa->calib.c00 = getTwosComplement(((uint32_t)coef[3] << 12) | ((uint32_t)coef[4] << 4) | (((
                                           uint32_t)coef[5] >> 4) & 0x0F), 20);
 
      // 0x15 c10 [19:16] + 0x16 c10 [15:8] + 0x17 c10 [7:0]
      spa->calib.c10 = getTwosComplement((((uint32_t)coef[5] & 0x0F) << 16) | ((uint32_t)coef[6] << 8) | (uint32_t)coef[7],
                                         20);
      spa->calib_idx++;
      break;
    case 1:
      // 0x18 c01 [15:8] + 0x19 c01 [7:0]
      spa->calib.c01 = getTwosComplement(((uint32_t)coef[0] << 8) | (uint32_t)coef[1], 16);
 
      // 0x1A c11 [15:8] + 0x1B c11 [7:0]
      spa->calib.c11 = getTwosComplement(((uint32_t)coef[2] << 8) | (uint32_t)coef[3], 16);
 
      // 0x1C c20 [15:8] + 0x1D c20 [7:0]
      spa->calib.c20 = getTwosComplement(((uint32_t)coef[4] << 8) | (uint32_t)coef[5], 16);
 
      // 0x1E c21 [15:8] + 0x1F c21 [7:0]
      spa->calib.c21 = getTwosComplement(((uint32_t)coef[6] << 8) | (uint32_t)coef[7], 16);
      spa->calib_idx++;
      break;
    case 2:
      // 0x20 c30 [15:8] + 0x21 c30 [7:0]
      spa->calib.c30 = getTwosComplement(((uint32_t)coef[0] << 8) | (uint32_t)coef[1], 16);
      if (spa->device == SPA06) {
        // 0x22 c31 [11:4] + 0x23 c31 [3:0]
        spa->calib.c31 = getTwosComplement(((uint32_t)coef[2] << 4) | (((uint32_t)coef[3] >> 4) & 0x0F), 12);
        // 0x23 c40 [11:8] + 0x24 c40 [7:0]
        spa->calib.c40 = getTwosComplement((((uint32_t)coef[3] & 0x0F) << 8) | (uint32_t)coef[4], 12);
      } else {
        // The SPL06 has no c31 and c40 coefficients
        spa->calib.c31 = 0;
        spa->calib.c40 = 0;
      }
      spa->calib_idx++;
      break;
    default:
      break;
  }
}
 
 
static void compensate_pressure(struct spa06_t *spa)
{
  // Calculate scaled measurement results.
  float Praw_sc = (float)spa->raw_pressure / raw_value_scale_factor(SPL06_PRESSURE_OVERSAMPLING);
  float Traw_sc = (float)spa->raw_temperature / raw_value_scale_factor(SPL06_TEMPERATURE_OVERSAMPLING);
 
  // Full SPA06 compensation polynomial (datasheet section 4.9.1); c31 and c40
  // are zero on the SPL06, so this reduces exactly to the SPL06 formula
  spa->pressure = spa->calib.c00 + Praw_sc * (spa->calib.c10 + Praw_sc * (spa->calib.c20 + Praw_sc *
                  (spa->calib.c30 + Praw_sc * spa->calib.c40))) + Traw_sc * spa->calib.c01 + Traw_sc * Praw_sc *
                  (spa->calib.c11 + Praw_sc * (spa->calib.c21 + Praw_sc * spa->calib.c31));
 
  // See section 4.9.2, How to Calculate Compensated Temperature Values, of datasheet
  spa->temperature = spa->calib.c0 * 0.5f + spa->calib.c1 * Traw_sc;
}
 
static bool spa06_config(struct spa06_t *spa)
{
  // Only one transaction can be made per call to the periodic function
  switch (spa->config_idx) {
    case 0:
      // PRS_CFG: pressure measurement rate (4 Hz) and oversampling
      spa06_register_write(spa, SPL06_REG_PRESSURE_CFG, (SPL06_PRES_RATE_4HZ | SPL06_PRESSURE_OVERSAMPLING));
      spa->config_idx++;
      break;
 
    case 1:
      // TMP_CFG: temperature measurement rate (4 Hz), oversampling and calibration temperature source
      spa06_register_write(spa, SPL06_REG_TEMPERATURE_CFG,
                           (SPL06_TEMP_RATE_4HZ | SPL06_TEMPERATURE_OVERSAMPLING | spa->tmp_coef_srce));
      spa->config_idx++;
      break;
 
    case 2: {
      uint8_t int_and_fifo_reg_value = 0;
      if (SPL06_TEMPERATURE_OVERSAMPLING > 3) { //SPL06_OVERSAMPLING_8X_T = 0x03
        int_and_fifo_reg_value |= SPL06_TEMPERATURE_RESULT_BIT_SHIFT;
      }
      if (SPL06_PRESSURE_OVERSAMPLING > 3) { // SPL06_OVERSAMPLING_8X_P = 0x03
        int_and_fifo_reg_value |= SPL06_PRESSURE_RESULT_BIT_SHIFT;
      }
      spa06_register_write(spa, SPL06_REG_INT_AND_FIFO_CFG, int_and_fifo_reg_value);
      spa->config_idx++;
      break;
    }
 
    case 3:
      spa06_register_write(spa, SPL06_REG_MODE_AND_STATUS, SPL06_MEAS_CON_PRE_TEM);
      spa->config_idx++;
      break;
 
    default:
      return true;
  }
  return false;
}
 
static bool spa06_get_calib(struct spa06_t *spa)
{
  switch (spa->calib_idx) {
    case 0:
      spa06_register_read(spa, SPL06_REG_CALIB_COEFFS_START, 8);
      break;
    case 1:
      spa06_register_read(spa, SPL06_REG_CALIB_COEFFS_START + 8, 8);
      break;
    case 2:
      // c30 (0x20-0x21); the SPA06 additionally has c31 and c40 (0x22-0x24).
      // Strictly read only the registers that exist on the detected device.
      spa06_register_read(spa, SPL06_REG_CALIB_COEFFS_START + 16, (spa->device == SPA06) ? 5 : 2);
      break;
    default:
      return true;
  }
  return false;
}
 
static int32_t raw_value_scale_factor(uint8_t oversampling)
{
  // From the datasheet page 13
  switch (oversampling) {
    case 0: return 524288;
    case 1: return 1572864;
    case 2: return 3670016;
    case 3: return 7864320;
    case 4: return 253952;
    case 5: return 516096;
    case 6: return 1040384;
    case 7: return 2088960;
    default: return -1; // invalid
  }
}
 
 
static void spa06_register_write(struct spa06_t *spa, uint8_t reg, uint8_t value)
{
 
  if (spa->bus == SPA06_SPI) {
    /* Never touch the buffers of a transaction that is still in flight */
    if (spa->spi.trans.status != SPITransDone) return;
    /* SPI transaction */
    spa->tx_buffer[0] = (reg & 0x7F); //write command (bit 7 = RW = '0')
    spa->tx_buffer[1] = value;
    spa->spi.trans.output_length = 2;
    spa->spi.trans.input_length = 0;
    spi_submit(spa->spi.p, &(spa->spi.trans));
  } else {
    if (spa->i2c.trans.status != I2CTransDone) return;
    /* I2C transaction */
    spa->tx_buffer[0] = reg;
    spa->tx_buffer[1] = value;
    i2c_transmit(spa->i2c.p, &(spa->i2c.trans), spa->i2c.slave_addr, 2);
  }
}
 
static void spa06_register_read(struct spa06_t *spa, uint8_t reg, uint16_t size)
{
 
 
  if (spa->bus == SPA06_SPI) {
    /* Never touch the buffers of a transaction that is still in flight */
    if (spa->spi.trans.status != SPITransDone) return;
    /* Guard against receive buffer overflow */
    if ((size + 1u) > sizeof(spa->spi.rx_buf)) return;
    /* SPI transaction */
    spa->tx_buffer[0] = reg | SPL06_READ_FLAG ;
    spa->spi.trans.output_length = 2;
    spa->spi.trans.input_length = size + 1; // already 1 is added for the transmission of the register to read
    spa->tx_buffer[1] = 0;
    spi_submit(spa->spi.p, &(spa->spi.trans));
  } else {
    if (spa->i2c.trans.status != I2CTransDone) return;
    /* Guard against receive buffer overflow */
    if (size > I2C_BUF_LEN) return;
    /* I2C transaction */
    spa->tx_buffer[0] = reg ;
    i2c_transceive(spa->i2c.p, &(spa->i2c.trans), spa->i2c.slave_addr, 1, size);
  }
}
 
static int32_t getTwosComplement(uint32_t raw, uint8_t length)
{
  if (raw & (1U << (length - 1))) {
    return ((int32_t)raw) - ((int32_t)1 << length);
  }
  return raw;
}