I try to write the sensor_fusion.code in ESP32. I get below compilation error;
Compilation error: 'LSM6DSV16X_SFLP_GYROSCOPE_BIAS_TAG' was not declared in this scope
#include <Arduino.h>
#include "lsm6dsv16x_reg.h"
#include "lsm6dsv16x_fsm_sflp_asc.h"
// #include "lsm6dsv16x_reg.c"
#include <Wire.h>
/** I2C Device Address 8 bit format if SA0=0 -> D5 if SA0=1 -> D7 **/
#define LSM6DSV16X_I2C_ADD_L 213
#define LSM6DSV16X_I2C_ADD_H 215
/* Private macro -------------------------------------------------------------*/
/*
* Select FIFO samples watermark, max value is 512
* in FIFO are stored acc, gyro and timestamp samples
*/
#define BOOT_TIME 10
#define FIFO_WATERMARK 32
/* Private variables ---------------------------------------------------------*/
static uint8_t whoamI;
/* Private variables ---------------------------------------------------------*/
static lsm6dsv16x_fifo_sflp_raw_t fifo_sflp;
/* Extern variables ----------------------------------------------------------*/
/* Private functions ---------------------------------------------------------*/
static int32_t platform_write(void *handle, uint8_t reg, const uint8_t *bufp,
uint16_t len);
static int32_t platform_read(void *handle, uint8_t reg, uint8_t *bufp,
uint16_t len);
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);
}
}
float npy_half_to_float(uint16_t h) {
union { float 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) {
float n = sqrtf(sumsq);
quat[0] /= n;
quat[1] /= n;
quat[2] /= n;
sumsq = 1.0f;
}
quat[3] = sqrtf(1.0f - sumsq);
}
void lsm6dsv16x_sensor_fusion() {
lsm6dsv16x_fifo_status_t fifo_status;
stmdev_ctx_t dev_ctx;
lsm6dsv16x_reset_t rst;
lsm6dsv16x_sflp_gbias_t gbias;
// Initialize sensor driver interface
dev_ctx.write_reg = platform_write;
dev_ctx.read_reg = platform_read;
// Initialize platform (if necessary)
// platform_init();
delay(BOOT_TIME); // Arduino delay function
// Check device ID
lsm6dsv16x_device_id_get(&dev_ctx, &whoamI);
if (whoamI != LSM6DSV16X_ID) {
while (1) {
Serial.println("Incorrect device ID");
delay(1000);
}
}
// ... [rest of your initialization code]
while (1) {
uint16_t num = 0;
lsm6dsv16x_fifo_status_get(&dev_ctx, &fifo_status);
if (fifo_status.fifo_th == 1) {
num = fifo_status.fifo_level;
Serial.print("-- FIFO num ");
Serial.println(num);
while (num--) {
lsm6dsv16x_fifo_out_raw_t f_data;
int16_t *axis;
float quat[4];
float gravity_mg[3];
float gbias_mdps[3];
// ... [rest of your data handling code]
switch (f_data.tag) {
case LSM6DSV16X_SFLP_GYROSCOPE_BIAS_TAG:
axis = (int16_t *)&f_data.data[0];
gbias_mdps[0] = lsm6dsv16x_from_fs125_to_mdps(axis[0]);
// ... [rest of the case]
Serial.print("GBIAS [mdps]: ");
Serial.print(gbias_mdps[0]);
Serial.print("\t");
// ... [rest of the printing]
break;
// ... [rest of the switch-case]
}
}
Serial.println("------");
}
}
}
void setup() {
// put your setup code here, to run once:
lsm6dsv16x_sensor_fusion(); // Call the sensor fusion function
}
void loop() {
// put your main code here, to run repeatedly:
}
int32_t platform_write(void *handle, uint8_t reg, const uint8_t *bufp, uint16_t len) {
Wire.beginTransmission(LSM6DSV16X_I2C_ADD_L); // Assume LSM6DSV16X_I2C_ADD_L is the I2C address
Wire.write(reg);
for(uint16_t i = 0; i < len; i++) {
Wire.write(bufp[i]);
}
return Wire.endTransmission();
}
int32_t platform_read(void *handle, uint8_t reg, uint8_t *bufp, uint16_t len) {
Wire.beginTransmission(LSM6DSV16X_I2C_ADD_L); // Assume LSM6DSV16X_I2C_ADD_L is the I2C address
Wire.write(reg);
Wire.endTransmission(false); // Use a repeated start, so we don't release the bus
uint16_t bytesRead = Wire.requestFrom(LSM6DSV16X_I2C_ADD_L, len); // Request bytes from slave device
for(uint16_t i = 0; i < bytesRead; i++) {
bufp[i] = Wire.read();
}
return bytesRead == len ? 0 : -1; // Return 0 if success, else -1
}
