ra8_lsm6dso.c

Go to the documentation of this file.

 
#include "ra8_lsm6dso.h"
 
#include <stdint.h>
 
#include "ra8_attributes.h"
#include "ra8_check.h"
#include "ra8_err.h"
#include "ra8_log.h"
 
/* =============================================================================
 * File-local constants and helpers
 * =============================================================================
 */

static const char* const s_lsm6dso_tag = "lsm6dso";

typedef enum : uint32_t {
  k_lsm6dso_xyz_burst_bytes   = 6U, 
  k_lsm6dso_temp_burst_bytes  = 2U, 
  k_lsm6dso_fifo_status_bytes = 2U, 
} ra8_lsm6dso_burst_bytes_t;

typedef enum : uint8_t {
  k_lsm6dso_idx_low  = 0U, 
  k_lsm6dso_idx_high = 1U, 
} ra8_lsm6dso_endian_idx_t;

typedef enum : uint8_t {
  k_lsm6dso_xyz_off_x_l = 0U, 
  k_lsm6dso_xyz_off_x_h = 1U, 
  k_lsm6dso_xyz_off_y_l = 2U, 
  k_lsm6dso_xyz_off_y_h = 3U, 
  k_lsm6dso_xyz_off_z_l = 4U, 
  k_lsm6dso_xyz_off_z_h = 5U, 
} ra8_lsm6dso_xyz_off_t;

typedef enum : uint8_t {
  k_lsm6dso_shift_odr    = 4U, 
  k_lsm6dso_shift_fs_xl  = 2U, 
  k_lsm6dso_shift_fs_g   = 2U, 
  k_lsm6dso_shift_fs_125 = 1U, 
} ra8_lsm6dso_shift_t;
 
typedef enum : uint8_t {
  k_lsm6dso_mask_odr       = 0xF0U, 
  k_lsm6dso_mask_fs_xl     = 0x0CU, 
  k_lsm6dso_mask_fs_g      = 0x0CU, 
  k_lsm6dso_mask_fs_125    = 0x02U, 
  k_lsm6dso_mask_fs_g_full = 0x0EU, 
  k_lsm6dso_mask_nibble    = 0x0FU, 
  k_lsm6dso_mask_byte      = 0xFFU, 
} ra8_lsm6dso_mask_t;

typedef enum : int32_t {
  k_lsm6dso_temp_offset_centi_c = 2500, 
  k_lsm6dso_temp_scale_num      = 100,  
  k_lsm6dso_temp_scale_den      = 256,  
} ra8_lsm6dso_temp_const_t;

typedef enum : uint8_t {
  k_lsm6dso_xl_fs_max = (uint8_t)k_lsm6dso_g_fs_2000dps, 
  k_lsm6dso_xl_fs_cap = (uint8_t)k_lsm6dso_xl_fs_8g,     
  k_lsm6dso_g_fs_cap  = (uint8_t)k_lsm6dso_g_fs_2000dps, 
  k_lsm6dso_odr_cap   = (uint8_t)k_lsm6dso_odr_6660hz,   
} ra8_lsm6dso_cap_t;
 
/* =============================================================================
 * Internal: build the byte that programs CTRL2_G's FS field
 * =============================================================================
 */

RA8_INTERNAL
static uint8_t internal_lsm6dso_g_fs_bits(ra8_lsm6dso_g_fs_t fs)
{
  /* DS12140 Table 47: FS_125 takes priority over FS_G[1:0]. */
  if (fs == k_lsm6dso_g_fs_125dps) {
    return (uint8_t)((uint8_t)1U << (uint8_t)k_lsm6dso_shift_fs_125);
  }
  /* Numerically, _250dps == 1, _500dps == 2, _1000dps == 3, _2000dps == 4.
   * Map back to the FS_G[1:0] bit field by subtracting one. */
  const uint8_t fs_g_field = (uint8_t)((uint8_t)fs - (uint8_t)k_lsm6dso_g_fs_250dps);
  return (uint8_t)((uint8_t)(fs_g_field & 0x03U) << (uint8_t)k_lsm6dso_shift_fs_g);
}

RA8_INTERNAL
static ra8_err_t internal_lsm6dso_write_byte(ra8_lsm6dso_t* dev, uint8_t reg, uint8_t val)
{
  return dev->bus.write_regs(dev->bus.ctx, reg, &val, 1U);
}

RA8_INTERNAL
static ra8_err_t internal_lsm6dso_read_byte(ra8_lsm6dso_t* dev, uint8_t reg, uint8_t* out)
{
  return dev->bus.read_regs(dev->bus.ctx, reg, out, 1U);
}

RA8_INTERNAL
static ra8_err_t internal_lsm6dso_rmw_odr(ra8_lsm6dso_t* dev, uint8_t reg, uint8_t odr_bits)
{
  uint8_t         now = 0U;
  const ra8_err_t r   = internal_lsm6dso_read_byte(dev, reg, &now);
  if (r != k_ra8_ok) {
    return r;
  }
  const uint8_t merged = (uint8_t)((now & (uint8_t)~k_lsm6dso_mask_odr) | odr_bits);
  return internal_lsm6dso_write_byte(dev, reg, merged);
}
 
/* =============================================================================
 * Lifecycle
 * =============================================================================
 */
 
ra8_err_t ra8_lsm6dso_init(ra8_lsm6dso_t* out_dev, const ra8_lsm6dso_bus_t* bus)
{
  RA8_CHECK_NULL_PTR(out_dev, s_lsm6dso_tag, "init: out_dev");
  RA8_CHECK_NULL_PTR(bus, s_lsm6dso_tag, "init: bus");
  RA8_CHECK_NULL_PTR(bus->read_regs, s_lsm6dso_tag, "init: bus.read_regs");
  RA8_CHECK_NULL_PTR(bus->write_regs, s_lsm6dso_tag, "init: bus.write_regs");
 
  out_dev->bus           = *bus;
  out_dev->accel_fs_code = k_lsm6dso_xl_fs_2g;    /* DS12140 sec 9.12 reset default. */
  out_dev->gyro_fs_code  = k_lsm6dso_g_fs_250dps; /* DS12140 sec 9.13 reset default. */
  out_dev->odr_code      = k_lsm6dso_odr_off;     /* DS12140 sec 9.12 reset default. */
  out_dev->initialized   = true;
  return k_ra8_ok;
}
 
/* =============================================================================
 * Identification
 * =============================================================================
 */
 
ra8_err_t ra8_lsm6dso_who_am_i(ra8_lsm6dso_t* dev, uint8_t* out_id)
{
  RA8_CHECK_NULL_PTR(dev, s_lsm6dso_tag, "who_am_i: dev");
  RA8_CHECK_NULL_PTR(out_id, s_lsm6dso_tag, "who_am_i: out_id");
  RA8_VALIDATE_INIT(dev->initialized, s_lsm6dso_tag, "who_am_i: not initialized");
 
  /* DS12140 sec 9.11 WHO_AM_I (0Fh). */
  return internal_lsm6dso_read_byte(dev, (uint8_t)k_lsm6dso_reg_who_am_i, out_id);
}
 
/* =============================================================================
 * Configuration
 * =============================================================================
 */
 
ra8_err_t ra8_lsm6dso_set_accel_range(ra8_lsm6dso_t* dev, ra8_lsm6dso_xl_fs_t fs)
{
  RA8_CHECK_NULL_PTR(dev, s_lsm6dso_tag, "set_accel_range: dev");
  RA8_VALIDATE_INIT(dev->initialized, s_lsm6dso_tag, "set_accel_range: not initialized");
  RA8_CHECK_RANGE_TAG((uint8_t)fs,
                      0U,
                      (uint8_t)k_lsm6dso_xl_fs_cap,
                      k_ra8_err_invalid_arg,
                      s_lsm6dso_tag);
 
  /* DS12140 sec 9.12 CTRL1_XL: read-modify-write FS_XL[3:2]. */
  uint8_t         current = 0U;
  const ra8_err_t r = internal_lsm6dso_read_byte(dev, (uint8_t)k_lsm6dso_reg_ctrl1_xl, &current);
  if (r != k_ra8_ok) {
    return r;
  }
  const uint8_t merged =
    (uint8_t)((current & (uint8_t)~k_lsm6dso_mask_fs_xl) |
              (uint8_t)(((uint8_t)fs & 0x03U) << (uint8_t)k_lsm6dso_shift_fs_xl));
  const ra8_err_t w = internal_lsm6dso_write_byte(dev, (uint8_t)k_lsm6dso_reg_ctrl1_xl, merged);
  if (w != k_ra8_ok) {
    return w;
  }
  dev->accel_fs_code = fs;
  return k_ra8_ok;
}
 
ra8_err_t ra8_lsm6dso_set_gyro_range(ra8_lsm6dso_t* dev, ra8_lsm6dso_g_fs_t fs)
{
  RA8_CHECK_NULL_PTR(dev, s_lsm6dso_tag, "set_gyro_range: dev");
  RA8_VALIDATE_INIT(dev->initialized, s_lsm6dso_tag, "set_gyro_range: not initialized");
  RA8_CHECK_RANGE_TAG((uint8_t)fs,
                      0U,
                      (uint8_t)k_lsm6dso_g_fs_cap,
                      k_ra8_err_invalid_arg,
                      s_lsm6dso_tag);
 
  /* DS12140 sec 9.13 CTRL2_G: read-modify-write FS_G[3:2] + FS_125[1]. */
  uint8_t         current = 0U;
  const ra8_err_t r = internal_lsm6dso_read_byte(dev, (uint8_t)k_lsm6dso_reg_ctrl2_g, &current);
  if (r != k_ra8_ok) {
    return r;
  }
  const uint8_t   fs_bits = internal_lsm6dso_g_fs_bits(fs);
  const uint8_t   merged  = (uint8_t)((current & (uint8_t)~k_lsm6dso_mask_fs_g_full) | fs_bits);
  const ra8_err_t w = internal_lsm6dso_write_byte(dev, (uint8_t)k_lsm6dso_reg_ctrl2_g, merged);
  if (w != k_ra8_ok) {
    return w;
  }
  dev->gyro_fs_code = fs;
  return k_ra8_ok;
}
 
ra8_err_t ra8_lsm6dso_set_odr(ra8_lsm6dso_t* dev, ra8_lsm6dso_odr_t odr)
{
  RA8_CHECK_NULL_PTR(dev, s_lsm6dso_tag, "set_odr: dev");
  RA8_VALIDATE_INIT(dev->initialized, s_lsm6dso_tag, "set_odr: not initialized");
  RA8_CHECK_RANGE_TAG((uint8_t)odr,
                      0U,
                      (uint8_t)k_lsm6dso_odr_cap,
                      k_ra8_err_invalid_arg,
                      s_lsm6dso_tag);
 
  /* DS12140 sec 9.12 / 9.13: ODR field is bits [7:4] of CTRL1_XL and CTRL2_G. */
  const uint8_t odr_bits =
    (uint8_t)(((uint8_t)odr & (uint8_t)k_lsm6dso_mask_nibble) << (uint8_t)k_lsm6dso_shift_odr);
 
  const ra8_err_t rxl = internal_lsm6dso_rmw_odr(dev, (uint8_t)k_lsm6dso_reg_ctrl1_xl, odr_bits);
  if (rxl != k_ra8_ok) {
    return rxl;
  }
  const ra8_err_t rg = internal_lsm6dso_rmw_odr(dev, (uint8_t)k_lsm6dso_reg_ctrl2_g, odr_bits);
  if (rg != k_ra8_ok) {
    return rg;
  }
 
  dev->odr_code = odr;
  return k_ra8_ok;
}
 
/* =============================================================================
 * Sample-read helpers
 * =============================================================================
 */

RA8_INTERNAL
static ra8_err_t internal_lsm6dso_read_xyz(ra8_lsm6dso_t* dev, uint8_t reg, ra8_lsm6dso_xyz_t* out)
{
  uint8_t         bytes[6] = {};
  const ra8_err_t r =
    dev->bus.read_regs(dev->bus.ctx, reg, bytes, (uint32_t)k_lsm6dso_xyz_burst_bytes);
  if (r != k_ra8_ok) {
    return r;
  }
  /* LSM6DSO XYZ samples are little-endian two's complement
   * (DS12140 sec 9.29 .. 9.40). */
  out->x = (int16_t)(((uint16_t)bytes[k_lsm6dso_xyz_off_x_h] << 8) |
                     (uint16_t)bytes[k_lsm6dso_xyz_off_x_l]);
  out->y = (int16_t)(((uint16_t)bytes[k_lsm6dso_xyz_off_y_h] << 8) |
                     (uint16_t)bytes[k_lsm6dso_xyz_off_y_l]);
  out->z = (int16_t)(((uint16_t)bytes[k_lsm6dso_xyz_off_z_h] << 8) |
                     (uint16_t)bytes[k_lsm6dso_xyz_off_z_l]);
  return k_ra8_ok;
}
 
ra8_err_t ra8_lsm6dso_read_accel(ra8_lsm6dso_t* dev, ra8_lsm6dso_xyz_t* out)
{
  RA8_CHECK_NULL_PTR(dev, s_lsm6dso_tag, "read_accel: dev");
  RA8_CHECK_NULL_PTR(out, s_lsm6dso_tag, "read_accel: out");
  RA8_VALIDATE_INIT(dev->initialized, s_lsm6dso_tag, "read_accel: not initialized");
  /* DS12140 sec 9.35 OUTX_L_A (28h) -- auto-increment burst over 6 bytes. */
  return internal_lsm6dso_read_xyz(dev, (uint8_t)k_lsm6dso_reg_outx_l_a, out);
}
 
ra8_err_t ra8_lsm6dso_read_gyro(ra8_lsm6dso_t* dev, ra8_lsm6dso_xyz_t* out)
{
  RA8_CHECK_NULL_PTR(dev, s_lsm6dso_tag, "read_gyro: dev");
  RA8_CHECK_NULL_PTR(out, s_lsm6dso_tag, "read_gyro: out");
  RA8_VALIDATE_INIT(dev->initialized, s_lsm6dso_tag, "read_gyro: not initialized");
  /* DS12140 sec 9.29 OUTX_L_G (22h) -- auto-increment burst over 6 bytes. */
  return internal_lsm6dso_read_xyz(dev, (uint8_t)k_lsm6dso_reg_outx_l_g, out);
}
 
ra8_err_t ra8_lsm6dso_read_temp(ra8_lsm6dso_t* dev, int32_t* out_centi_c)
{
  RA8_CHECK_NULL_PTR(dev, s_lsm6dso_tag, "read_temp: dev");
  RA8_CHECK_NULL_PTR(out_centi_c, s_lsm6dso_tag, "read_temp: out_centi_c");
  RA8_VALIDATE_INIT(dev->initialized, s_lsm6dso_tag, "read_temp: not initialized");
 
  /* DS12140 sec 9.27 OUT_TEMP_L (20h) + sec 9.28 OUT_TEMP_H (21h). */
  uint8_t         bytes[2] = {};
  const ra8_err_t r        = dev->bus.read_regs(dev->bus.ctx,
                                                (uint8_t)k_lsm6dso_reg_out_temp_l,
                                                bytes,
                                                (uint32_t)k_lsm6dso_temp_burst_bytes);
  if (r != k_ra8_ok) {
    return r;
  }
  const int16_t raw =
    (int16_t)(((uint16_t)bytes[k_lsm6dso_idx_high] << 8) | (uint16_t)bytes[k_lsm6dso_idx_low]);
  /* DS12140 sec 4.3 "Temperature sensor characteristics":
   *   T[degC] = raw / 256 + 25
   * Scale into centi-deg C to keep integer arithmetic: */
  *out_centi_c = (int32_t)((int32_t)raw * (int32_t)k_lsm6dso_temp_scale_num /
                           (int32_t)k_lsm6dso_temp_scale_den) +
                 (int32_t)k_lsm6dso_temp_offset_centi_c;
  return k_ra8_ok;
}

RA8_INTERNAL
static ra8_err_t internal_lsm6dso_read_fifo_depth(ra8_lsm6dso_t* dev, uint32_t* out_n)
{
  uint8_t         status[2] = {};
  const ra8_err_t rs        = dev->bus.read_regs(dev->bus.ctx,
                                                 (uint8_t)k_lsm6dso_reg_fifo_status1,
                                                 status,
                                                 (uint32_t)k_lsm6dso_fifo_status_bytes);
  if (rs != k_ra8_ok) {
    return rs;
  }
  *out_n = (uint32_t)status[k_lsm6dso_idx_low] |
           ((uint32_t)(status[k_lsm6dso_idx_high] & (uint8_t)k_lsm6dso_mask_nibble) << 8U);
  return k_ra8_ok;
}

RA8_INTERNAL
static ra8_err_t internal_lsm6dso_burst_fifo(ra8_lsm6dso_t* dev, uint8_t* out_buf, uint32_t n_words)
{
  const uint32_t total_bytes = n_words * (uint32_t)k_lsm6dso_fifo_bytes_word;
  return dev->bus.read_regs(dev->bus.ctx,
                            (uint8_t)k_lsm6dso_reg_fifo_data_out,
                            out_buf,
                            total_bytes);
}

RA8_INTERNAL
static ra8_err_t internal_lsm6dso_fifo_check_args(const ra8_lsm6dso_t* dev,
                                                  const uint8_t*       out_buf,
                                                  uint32_t             max_words,
                                                  const uint32_t*      out_words)
{
  RA8_CHECK_NULL_PTR(dev, s_lsm6dso_tag, "read_fifo: dev");
  RA8_CHECK_NULL_PTR(out_buf, s_lsm6dso_tag, "read_fifo: out_buf");
  RA8_CHECK_NULL_PTR(out_words, s_lsm6dso_tag, "read_fifo: out_words");
  RA8_VALIDATE_INIT(dev->initialized, s_lsm6dso_tag, "read_fifo: not initialized");
  if (max_words == 0U) {
    ra8_log_error(s_lsm6dso_tag, "read_fifo: max_words is zero");
    return k_ra8_err_invalid_arg;
  }
  return k_ra8_ok;
}
 
ra8_err_t ra8_lsm6dso_read_xl_gyro_fifo(ra8_lsm6dso_t* dev,
                                        uint8_t*       out_buf,
                                        uint32_t       max_words,
                                        uint32_t*      out_words)
{
  /* Set the output count early so callers can rely on it on failure. */
  if (out_words != nullptr) {
    *out_words = 0U;
  }
  const ra8_err_t chk = internal_lsm6dso_fifo_check_args(dev, out_buf, max_words, out_words);
  if (chk != k_ra8_ok) {
    return chk;
  }
 
  uint32_t        live = 0U;
  const ra8_err_t rs   = internal_lsm6dso_read_fifo_depth(dev, &live);
  if (rs != k_ra8_ok) {
    return rs;
  }
  const uint32_t to_read = (live > max_words) ? max_words : live;
  if (to_read == 0U) {
    return k_ra8_ok;
  }
  const ra8_err_t rd = internal_lsm6dso_burst_fifo(dev, out_buf, to_read);
  if (rd != k_ra8_ok) {
    return rd;
  }
  *out_words = to_read;
  return k_ra8_ok;
}