FIFO game rotation lsm6dsv32x

code.c

#include "lsm6dsv32x_reg.h"
#include <stdint.h>
#include <stdio.h>
#include <string.h>

/* Private macro -------------------------------------------------------------*/
/*
 * Select FIFO samples watermark, max value is 512
 * in FIFO are stored acc, gyro and timestamp samples
 */

// NOTE(eli): higher watermarks here cause queuing which results in batches of
//            messages appearing over usb. The end result is 'janky' behavior
//            but that is mainly due to the fact that we don't have timestamps
//            associated with the events.
#define FIFO_WATERMARK 1

/* Private variables ---------------------------------------------------------*/
static lsm6dsv32x_fifo_sflp_raw_t fifo_sflp;

/*
 * Original conversion routines taken from: https://github.com/numpy/numpy
 *
 * uint32_t npy_halfbits_to_floatbits(uint16_t h);
 * float_t npy_half_to_float(uint16_t h);
 *
 * Released under BSD-3-Clause License
 */
static uint32_t
npy_halfbits_to_floatbits(uint16_t h) {
  uint16_t h_exp, h_sig;
  uint32_t f_sgn, f_exp, f_sig;

  h_exp = (h & 0x7c00u);
  f_sgn = ((uint32_t)h & 0x8000u) << 16;
  switch (h_exp) {
    case 0x0000u: /* 0 or subnormal */
      h_sig = (h & 0x03ffu);
      /* Signed zero */
      if (h_sig == 0) {
        return f_sgn;
      }
      /* Subnormal */
      h_sig <<= 1;
      while ((h_sig & 0x0400u) == 0) {
        h_sig <<= 1;
        h_exp++;
      }
      f_exp = ((uint32_t)(127 - 15 - h_exp)) << 23;
      f_sig = ((uint32_t)(h_sig & 0x03ffu)) << 13;
      return f_sgn + f_exp + f_sig;
    case 0x7c00u: /* inf or NaN */
      /* All-ones exponent and a copy of the significand */
      return f_sgn + 0x7f800000u + (((uint32_t)(h & 0x03ffu)) << 13);
    default: /* normalized */
      /* Just need to adjust the exponent and shift */
      return f_sgn + (((uint32_t)(h & 0x7fffu) + 0x1c000u) << 13);
  }
}

static float_t
npy_half_to_float(uint16_t h) {
  union {
    float_t ret;
    uint32_t retbits;
  } conv;
  conv.retbits = npy_halfbits_to_floatbits(h);
  return conv.ret;
}

static void
sflp2q(float quat[4], uint16_t sflp[3]) {
  float sumsq = 0;

  quat[0] = npy_half_to_float(sflp[0]);
  quat[1] = npy_half_to_float(sflp[1]);
  quat[2] = npy_half_to_float(sflp[2]);

  for (uint8_t i = 0; i < 3; i++)
    sumsq += quat[i] * quat[i];

  if (sumsq > 1.0f)
    sumsq = 1.0f;

  quat[3] = sqrtf(1.0f - sumsq);
}

bool
lsm6dsv32x_setup(stmdev_ctx_t *dev_ctx) {
  lsm6dsv32x_reset_t rst;
  lsm6dsv32x_sflp_gbias_t gbias;

  uint8_t whoamI;
  /* Check device ID */
  lsm6dsv32x_device_id_get(dev_ctx, &whoamI);

  if (whoamI != LSM6DSV32X_ID) {
    printf("device mismatch --- 0x%02X\n", whoamI);
    return false;
  }

  /* Restore default configuration */
  lsm6dsv32x_reset_set(dev_ctx, LSM6DSV32X_RESTORE_CTRL_REGS);
  do {
    lsm6dsv32x_reset_get(dev_ctx, &rst);
  } while (rst != LSM6DSV32X_READY);

  /* Enable Block Data Update */
  lsm6dsv32x_block_data_update_set(dev_ctx, PROPERTY_ENABLE);
  /* Set full scale */
  lsm6dsv32x_xl_full_scale_set(dev_ctx, LSM6DSV32X_4g);
  lsm6dsv32x_gy_full_scale_set(dev_ctx, LSM6DSV32X_2000dps);

  /*
   * Set FIFO watermark (number of unread sensor data TAG + 6 bytes
   * stored in FIFO) to FIFO_WATERMARK samples
   */
  lsm6dsv32x_fifo_watermark_set(dev_ctx, FIFO_WATERMARK);
  /* Set FIFO batch of sflp data */
  fifo_sflp.game_rotation = 1;
  fifo_sflp.gravity = 0;
  fifo_sflp.gbias = 0;
  lsm6dsv32x_fifo_sflp_batch_set(dev_ctx, fifo_sflp);

  /* Set FIFO mode to Stream mode (aka Continuous Mode) */
  lsm6dsv32x_fifo_mode_set(dev_ctx, LSM6DSV32X_STREAM_MODE);

  /* Set Output Data Rate */
  lsm6dsv32x_xl_data_rate_set(dev_ctx, LSM6DSV32X_ODR_AT_60Hz);
  lsm6dsv32x_gy_data_rate_set(dev_ctx, LSM6DSV32X_ODR_AT_60Hz);
  lsm6dsv32x_sflp_data_rate_set(dev_ctx, LSM6DSV32X_SFLP_60Hz);

  lsm6dsv32x_sflp_game_rotation_set(dev_ctx, PROPERTY_ENABLE);

  /*
   * here application may initialize offset with latest values
   * calculated from previous run and saved to non volatile memory.
   */
  gbias.gbias_x = 0.0f;
  gbias.gbias_y = 0.0f;
  gbias.gbias_z = 0.0f;
  lsm6dsv32x_sflp_game_gbias_set(dev_ctx, &gbias);
  return true;
}

/* Main Example --------------------------------------------------------------*/
void
lsm6dsv32x_tick(stmdev_ctx_t *dev_ctx,
                int imuIndex,
                uint8_t busIndex,
                uint8_t deviceIndex,
                uint16_t *compressedQuatOut) {
  lsm6dsv32x_fifo_status_t fifo_status;

  float quat[4];
  uint16_t num = 0;
  lsm6dsv32x_fifo_out_raw_t f_data;

  lsm6dsv32x_fifo_status_get(dev_ctx, &fifo_status);
  if (fifo_status.fifo_th == 1) {
    num = fifo_status.fifo_level;
    // printf("%i fifo level %u\n", imuIndex, num);
    int limit = 1;
    while (num--) {
      lsm6dsv32x_fifo_out_raw_get(dev_ctx, &f_data);
      memcpy(compressedQuatOut, f_data.data, 6);
      if (limit-- > 0 && f_data.tag == LSM6DSV32X_SFLP_GAME_ROTATION_VECTOR_TAG) {
        sflp2q(quat, (uint16_t *)&f_data.data[0]);
        // printf("{\"id\":\"%u:%u\",\"Q\":[%f,%f,%f,%f]}\n",
        //        busIndex,
        //        deviceIndex,
        //        quat[0],
        //        quat[1],
        //        quat[2],
        //        quat[3]);
      }
    }
  }
}