Hello everyone! i could nt find a solution:
Problem description:
I am trying to use the LSM6DSV16X sensor on a Raspberry Pi 4B over I²C. My goal is to retrieve quaternion data using the Sensor Fusion Low Power (SFLP) engine. Here’s what I’ve tried so far:
Initialization steps:
I reset the sensor and set up the basic configuration.
I’ve enabled accelerometer and gyroscope with ±4g and ±2000 dps ranges.
The ODR for both sensors is set to 120 Hz.
I activated the SFLP engine and configured it to output quaternion data to the FIFO.
The FIFO is set to Stream Mode, and I’ve adjusted the FIFO batch data rates for accelerometer, gyroscope, and SFLP to 120 Hz.
Expected behavior:
The sensor should output quaternion data (tag 0x13) in the FIFO, alongside other data if configured.
What actually happens:
After setting everything up, I don’t see valid quaternion data in the FIFO.
Sometimes, the FIFO level stays at zero or shows unexpected tags.
I’ve verified that the sensor is accessible via I²C, and I am able to read the WHO_AM_I register correctly.
Request for help:
I’d like to confirm if my register setup and the sequence of operations are correct. If anyone has successfully used the LSM6DSV16X’s SFLP engine on the Raspberry Pi (or a similar setup), I’d appreciate guidance on the proper initialization steps, any timing considerations, and how to ensure the FIFO receives and outputs quaternion data reliably.
#include
#include <fcntl.h>
#include <unistd.h>
#include <linux/i2c-dev.h>
#include <sys/ioctl.h>
#include
#include
#define I2C_DEV “/dev/i2c-1”
#define LSM6DSV16X_ADDR 0x6B
bool write_reg(int fd, uint8_t reg, uint8_t value) {
uint8_t buffer[2] = {reg, value};
return write(fd, buffer, 2) == 2;
}
bool read_reg_multi(int fd, uint8_t reg, uint8_t *buf, size_t len) {
if (write(fd, ®, 1) != 1) return false;
return read(fd, buf, len) == (int)len;
}
int main() {
int fd = open(I2C_DEV, O_RDWR);
if (fd < 0 || ioctl(fd, I2C_SLAVE, LSM6DSV16X_ADDR) < 0) {
std::cerr << "Fehler beim Öffnen von " << I2C_DEV << “\n”;
return 1;
}
// WHO_AM_I prüfen
uint8_t whoami = 0;
read_reg_multi(fd, 0x0F, &whoami, 1);
if (whoami != 0x6C && whoami != 0x70) {
std::cerr << "Falscher Sensor! WHO_AM_I: 0x" << std::hex << (int)whoami << "\n";
return 1;
}
// --- Reset & Basis-Konfiguration ---
write_reg(fd, 0x12, 0x01); usleep(10000); // SW_RESET
write_reg(fd, 0x12, 0x44); // BDU + IF_INC
write_reg(fd, 0x17, 0x40); // ±4g, LPF2 = ODR/20
write_reg(fd, 0x18, 0x08); // LPF2_XL_EN
write_reg(fd, 0x16, 0x01); // LPF1_G_EN
write_reg(fd, 0x15, 0x61); // ±2000 dps, LPF1 BW
write_reg(fd, 0x10, 0x06); // ACC ODR = 120 Hz
write_reg(fd, 0x11, 0x06); // GYRO ODR = 120 Hz
// --- Embedded Function Config ---
write_reg(fd, 0x01, 0x80); // FUNC_CFG_ACCESS on
write_reg(fd, 0x05, 0x00); // MLC/FSM aus
write_reg(fd, 0x04, 0x02); // SFLP Game Rotation aktivieren
write_reg(fd, 0x44, 0x01); // SFLP Quaternion in FIFO
write_reg(fd, 0x5E, 0x06); // SFLP-Batch ODR = 120 Hz
write_reg(fd, 0x66, 0x08); // SFLP Init
write_reg(fd, 0x01, 0x00); // FUNC_CFG_ACCESS aus
// --- FIFO konfigurieren ---
write_reg(fd, 0x09, 0x00); // Kein ACC/GYRO direkt in FIFO
write_reg(fd, 0x0A, 0x06); // FIFO Stream Mode
std::cout << "Sensor konfiguriert. Warte auf SFLP...\n";
usleep(1000000); // SFLP braucht ca. 0.8s zum Kalibrieren
std::cout << "Lese Quaternionen aus FIFO...\n";
while (true) {
uint8_t status[2];
read_reg_multi(fd, 0x1E, status, 2); // FIFO_STATUS1/2
uint16_t fifo_level = ((status[1] & 0x0F) << 8) | status[0];
while (fifo_level >= 9) {
uint8_t tag;
if (read(fd, &tag, 1) != 1) break;
fifo_level--;
if ((tag & 0x1F) == 0x13) { // SFLP Quaternion Tag
uint8_t data[8];
if (read(fd, data, 8) != 8) break;
int16_t qw = (data[1] << 8) | data[0];
int16_t qx = (data[3] << 8) | data[2];
int16_t qy = (data[5] << 8) | data[4];
int16_t qz = (data[7] << 8) | data[6];
std::cout << "[Quaternion] qw=" << qw
<< " qx=" << qx
<< " qy=" << qy
<< " qz=" << qz << std::endl;
fifo_level -= 8;
} else if ((tag & 0x1F) == 0x02) { // Accelerometer Tag
uint8_t data[6];
if (read(fd, data, 6) != 6) break;
int16_t ax = (data[1] << 8) | data[0];
int16_t ay = (data[3] << 8) | data[2];
int16_t az = (data[5] << 8) | data[4];
std::cout << "[ACC] ax=" << ax
<< " ay=" << ay
<< " az=" << az << std::endl;
fifo_level -= 6;
} else if ((tag & 0x1F) == 0x01) { // Gyroscope Tag
uint8_t data[6];
if (read(fd, data, 6) != 6) break;
int16_t gx = (data[1] << 8) | data[0];
int16_t gy = (data[3] << 8) | data[2];
int16_t gz = (data[5] << 8) | data[4];
std::cout << "[Gyro] gx=" << gx
<< " gy=" << gy
<< " gz=" << gz << std::endl;
fifo_level -= 6;
} else {
uint8_t skip[6];
read(fd, skip, 6); // andere Tags überspringen
fifo_level -= 6;
std::cout << "[Übersprungen: Tag 0x" << std::hex << (int)tag << "]\n";
}
}
usleep(100000); // 100 ms Pause
}
close(fd);
return 0;
}