feat(boards/px4_fmu-v6c): add Pixhawk6C support for LSM6DSV16X by zebulon-86 · Pull Request #26951 · PX4/PX4-Autopilot

zebulon-86 · 2026-04-02T04:06:52Z

Summary

This PR adds Pixhawk6C board integration for the ST LSM6DSV16X IMU.

This PR depends on #26950. Since the driver PR has not been merged yet,
the driver commits are included here as the dependency. Relative to
#26950, this PR only adds the Pixhawk6C board integration.

Details

On Pixhawk6C, the LSM6DSV16X is enabled on SPI1 and shares CS PC13 and
DRDY PE6 with the ICM42688P.

Board startup probes the ICM42688P first and then the LSM6DSV16X, so
only the IMU populated on the board is started.

-R 26 (ROTATION_PITCH_180_YAW_90) is used to match the sensor
orientation on the Pixhawk6C PCB.

This PR only contains the board-specific integration changes. The
generic LSM6DSV16X driver is provided separately in #26950.

Testing

Tested on Pixhawk6C with bring-up, ground testing, and flight validation.

Flight Review logs:

Initial flight validation (252.9 s): Flight Review log 1
Post-rebase re-test flight (312 s): Flight Review log 2

Post-rebase flight result:

312 s (~5.2 min) flight duration
LSM6DSV selected and operating normally on the primary path
no new issues introduced by the rebase
no EKF time_slip on the primary instance
no accel or gyro clipping
stable IMU timing (delta_velocity_dt std ~= 2.8 us)
both the primary LSM6DSV path and the backup BMI088 path remained healthy throughout the flight
LSM6DSV temperature remained stable, 35.85 C max

Copilot

Pull request overview

This PR adds the ST LSM6DSV16X IMU driver and integrates it on the Pixhawk6C (FMUv6C) board by registering it on SPI1 (sharing CS/DRDY with ICM42688P) and starting it from the board sensor init script with the required rotation.

Changes:

Add new SPI FIFO-based IMU driver: lsm6dsv (register definitions, driver implementation, module entrypoint, build + Kconfig).
Add new IMU devtype ID (DRV_IMU_DEVTYPE_ST_LSM6DSV) and enable the driver in the Pixhawk6C default config.
Register the device on Pixhawk6C SPI1 and start it from rc.board_sensors (alongside ICM42688P probe-based startup).

Reviewed changes

Copilot reviewed 10 out of 10 changed files in this pull request and generated 5 comments.

Show a summary per file

File	Description
src/drivers/imu/st/lsm6dsv/ST_LSM6DSV_Registers.hpp	New register/bit definitions and FIFO constants for LSM6DSV16X
src/drivers/imu/st/lsm6dsv/LSM6DSV.hpp	New SPI driver class definition, configuration, and state machine declarations
src/drivers/imu/st/lsm6dsv/LSM6DSV.cpp	New driver implementation: reset/configure, DRDY handling, FIFO readout/parsing, publishing
src/drivers/imu/st/lsm6dsv/lsm6dsv_main.cpp	New module CLI entrypoint for starting/stopping/status
src/drivers/imu/st/lsm6dsv/Kconfig	Kconfig option to enable the new driver
src/drivers/imu/st/lsm6dsv/CMakeLists.txt	Build definition for the new driver module
src/drivers/drv_sensor.h	Add a new IMU devtype constant for LSM6DSV
boards/px4/fmu-v6c/src/spi.cpp	Register the LSM6DSV device on SPI1 using the shared CS/DRDY pins
boards/px4/fmu-v6c/init/rc.board_sensors	Start `lsm6dsv` on Pixhawk6C (probe-based alongside ICM42688P) with the specified rotation
boards/px4/fmu-v6c/default.px4board	Enable `CONFIG_DRIVERS_IMU_ST_LSM6DSV` for Pixhawk6C builds

Copilot · 2026-04-02T05:03:02Z

src/drivers/imu/st/lsm6dsv/ST_LSM6DSV_Registers.hpp

+ */
+
+#pragma once
+


size_t is used in this header (FIFO::WORD_SIZE/MAX_DRAIN_SAMPLES/DEPTH), but only <cstdint> is included. Please include <cstddef> (as other register headers do) to ensure this compiles on all toolchains.

Suggested change

#include <cstddef>

Fixed in a follow-up commit.

Copilot · 2026-04-02T05:03:03Z

src/drivers/imu/st/lsm6dsv/LSM6DSV.cpp

+				uint8_t STATUS1{0};
+				uint8_t STATUS2{0};
+			} fifo_status{};
+			transfer((uint8_t *)&fifo_status, (uint8_t *)&fifo_status, sizeof(fifo_status));


The FIFO status read via transfer() ignores the return value. If the SPI exchange fails, STATUS1/STATUS2 stay at their default and the driver will treat it as an empty FIFO (and increment failure_count) without recording a bad transfer. Please check the return code here, perf_count(_bad_transfer_perf), and bail out of this cycle early on error.

Suggested change

transfer((uint8_t *)&fifo_status, (uint8_t *)&fifo_status, sizeof(fifo_status));

const int ret = transfer((uint8_t *)&fifo_status, (uint8_t *)&fifo_status, sizeof(fifo_status));

if (ret != OK) {

perf_count(_bad_transfer_perf);

_failure_count++;

if (_failure_count > 10) {

Reset();

}

break;

}

Fixed in a follow-up commit.

Copilot · 2026-04-02T05:03:03Z

src/drivers/imu/st/lsm6dsv/LSM6DSV.cpp

+				uint16_t samples = fifo_status1;
+
+				if (fifo_status2 & static_cast<uint8_t>(FIFO_STATUS2_BIT::DIFF_FIFO_8)) {
+					samples |= (1u << 8);
+				}
+
+				if (samples == 0) {
+					perf_count(_fifo_empty_perf);
+
+				} else if (samples > FIFO_MAX_SAMPLES) {
+					// not technically an overflow, but more samples than we expected or can publish
+					FIFOReset();
+					perf_count(_fifo_overflow_perf);
+
+				} else {
+
+					// tolerate minor jitter, leave sample to next iteration if behind by only 1
+					if (samples == static_cast<uint16_t>(_fifo_gyro_samples) + 1) {
+						timestamp_sample -= static_cast<int>(FIFO_SAMPLE_DT);
+						samples--;
+					}
+
+					if (FIFORead(timestamp_sample, samples)) {


FIFO_STATUS1/2 DIFF field is documented here as an unread word count, but the code compares it directly to FIFO_MAX_SAMPLES (32) and also applies jitter logic against _fifo_gyro_samples (computed in gyro-samples per cycle). With accel+gyro enabled you configure the watermark as samples*2, so DIFF will typically be ~2× _fifo_gyro_samples and can exceed FIFO_MAX_SAMPLES, causing repeated FIFO resets. Please make the units consistent (e.g. track fifo_words separately, derive expected words as _fifo_gyro_samples*2, and bound/compare using the same unit before calling FIFORead()).

Suggested change

uint16_t samples = fifo_status1;

if (fifo_status2 & static_cast<uint8_t>(FIFO_STATUS2_BIT::DIFF_FIFO_8)) {

samples |= (1u << 8);

}

if (samples == 0) {

perf_count(_fifo_empty_perf);

} else if (samples > FIFO_MAX_SAMPLES) {

// not technically an overflow, but more samples than we expected or can publish

FIFOReset();

perf_count(_fifo_overflow_perf);

} else {

// tolerate minor jitter, leave sample to next iteration if behind by only 1

if (samples == static_cast<uint16_t>(_fifo_gyro_samples) + 1) {

timestamp_sample -= static_cast<int>(FIFO_SAMPLE_DT);

samples--;

}

if (FIFORead(timestamp_sample, samples)) {

uint16_t fifo_words = fifo_status1;

if (fifo_status2 & static_cast<uint8_t>(FIFO_STATUS2_BIT::DIFF_FIFO_8)) {

fifo_words |= (1u << 8);

}

if (fifo_words == 0) {

perf_count(_fifo_empty_perf);

} else if (fifo_words > static_cast<uint16_t>(FIFO_MAX_SAMPLES * 2)) {

// not technically an overflow, but more data than we expected or can publish

FIFOReset();

perf_count(_fifo_overflow_perf);

} else {

// Expected FIFO words for the configured number of gyro samples per cycle

const uint16_t expected_fifo_words = static_cast<uint16_t>(_fifo_gyro_samples * 2);

// tolerate minor jitter: leave one sample (2 words) to next iteration if behind by only 1 sample

if (fifo_words == static_cast<uint16_t>(expected_fifo_words + 2)) {

timestamp_sample -= static_cast<int>(FIFO_SAMPLE_DT);

fifo_words -= 2;

}

// convert FIFO words to gyro samples (2 words per accel+gyro sample period)

const uint16_t gyro_samples = fifo_words / 2;

if (gyro_samples == 0) {

perf_count(_fifo_empty_perf);

} else if (FIFORead(timestamp_sample, gyro_samples)) {

Confirmed and fixed in a follow-up commit.

The datasheet reports DIFF_FIFO in FIFO words, while _fifo_gyro_samples and FIFO_MAX_SAMPLES are in sample periods. The fix keeps fifo_words for FIFORead() and uses sample_periods = fifo_words / 2 when comparing against those limits.

Copilot · 2026-04-02T05:03:03Z

src/drivers/imu/st/lsm6dsv/LSM6DSV.cpp

+	// Read FIFO word by word: each word is 7 bytes (tag + 6 data)
+	for (uint16_t i = 0; i < samples; i++) {
+		// Read tag + data in one transfer (1 cmd byte + 7 data bytes)
+		struct FIFOWordTransfer {
+			uint8_t cmd{static_cast<uint8_t>(Register::FIFO_DATA_OUT_TAG) | DIR_READ};
+			uint8_t TAG{0};
+			uint8_t DATA_X_L{0};
+			uint8_t DATA_X_H{0};
+			uint8_t DATA_Y_L{0};
+			uint8_t DATA_Y_H{0};
+			uint8_t DATA_Z_L{0};
+			uint8_t DATA_Z_H{0};
+		} buffer{};
+
+		if (transfer((uint8_t *)&buffer, (uint8_t *)&buffer, sizeof(buffer)) != PX4_OK) {
+			perf_count(_bad_transfer_perf);
+			continue;
+		}
+
+		// Decode tag from upper 5 bits
+		const uint8_t tag_id = buffer.TAG >> 3;
+
+		const int16_t data_x = combine(buffer.DATA_X_H, buffer.DATA_X_L);
+		const int16_t data_y = combine(buffer.DATA_Y_H, buffer.DATA_Y_L);
+		const int16_t data_z = combine(buffer.DATA_Z_H, buffer.DATA_Z_L);
+
+		if (tag_id == static_cast<uint8_t>(FifoTag::GYRO_NC)) {
+			if (gyro.samples < (sizeof(gyro.x) / sizeof(gyro.x[0]))) {
+				gyro.x[gyro.samples] = data_x;
+				gyro.y[gyro.samples] = data_y;
+				gyro.z[gyro.samples] = data_z;
+				gyro.samples++;
+			}
+
+		} else if (tag_id == static_cast<uint8_t>(FifoTag::ACCEL_NC)) {
+			if (accel.samples < (sizeof(accel.x) / sizeof(accel.x[0]))) {
+				accel.x[accel.samples] = data_x;
+				accel.y[accel.samples] = data_y;
+				accel.z[accel.samples] = data_z;
+				accel.samples++;
+			}
+
+		} else if (tag_id == static_cast<uint8_t>(FifoTag::TEMPERATURE)) {
+			const int16_t temp_raw = combine(buffer.DATA_X_H, buffer.DATA_X_L);
+			const float temperature = (temp_raw / 256.0f) + 25.0f;
+
+			if (PX4_ISFINITE(temperature)) {
+				_px4_accel.set_temperature(temperature);
+				_px4_gyro.set_temperature(temperature);
+			}
+		}
+
+		// Other tags (TIMESTAMP, etc.) are silently ignored


FIFO draining currently performs one SPI transfer per FIFO word (up to 32 transfers per cycle). Since FIFO words are contiguous and IF_INC is enabled, this can be significantly more expensive than a single burst transfer of words * 7 bytes. Consider reading multiple FIFO words in one transaction into a buffer and parsing locally to reduce SPI overhead and scheduler load.

Suggested change

// Read FIFO word by word: each word is 7 bytes (tag + 6 data)

for (uint16_t i = 0; i < samples; i++) {

// Read tag + data in one transfer (1 cmd byte + 7 data bytes)

struct FIFOWordTransfer {

uint8_t cmd{static_cast<uint8_t>(Register::FIFO_DATA_OUT_TAG) | DIR_READ};

uint8_t TAG{0};

uint8_t DATA_X_L{0};

uint8_t DATA_X_H{0};

uint8_t DATA_Y_L{0};

uint8_t DATA_Y_H{0};

uint8_t DATA_Z_L{0};

uint8_t DATA_Z_H{0};

} buffer{};

if (transfer((uint8_t *)&buffer, (uint8_t *)&buffer, sizeof(buffer)) != PX4_OK) {

perf_count(_bad_transfer_perf);

continue;

}

// Decode tag from upper 5 bits

const uint8_t tag_id = buffer.TAG >> 3;

const int16_t data_x = combine(buffer.DATA_X_H, buffer.DATA_X_L);

const int16_t data_y = combine(buffer.DATA_Y_H, buffer.DATA_Y_L);

const int16_t data_z = combine(buffer.DATA_Z_H, buffer.DATA_Z_L);

if (tag_id == static_cast<uint8_t>(FifoTag::GYRO_NC)) {

if (gyro.samples < (sizeof(gyro.x) / sizeof(gyro.x[0]))) {

gyro.x[gyro.samples] = data_x;

gyro.y[gyro.samples] = data_y;

gyro.z[gyro.samples] = data_z;

gyro.samples++;

}

} else if (tag_id == static_cast<uint8_t>(FifoTag::ACCEL_NC)) {

if (accel.samples < (sizeof(accel.x) / sizeof(accel.x[0]))) {

accel.x[accel.samples] = data_x;

accel.y[accel.samples] = data_y;

accel.z[accel.samples] = data_z;

accel.samples++;

}

} else if (tag_id == static_cast<uint8_t>(FifoTag::TEMPERATURE)) {

const int16_t temp_raw = combine(buffer.DATA_X_H, buffer.DATA_X_L);

const float temperature = (temp_raw / 256.0f) + 25.0f;

if (PX4_ISFINITE(temperature)) {

_px4_accel.set_temperature(temperature);

_px4_gyro.set_temperature(temperature);

}

}

// Other tags (TIMESTAMP, etc.) are silently ignored

// Read FIFO in a single burst: each word is 7 bytes (tag + 6 data)

const uint16_t max_fifo_words = 32;

uint16_t samples_to_read = samples;

if (samples_to_read > max_fifo_words) {

samples_to_read = max_fifo_words;

}

struct FIFOBurstTransfer {

uint8_t cmd{static_cast<uint8_t>(Register::FIFO_DATA_OUT_TAG) | DIR_READ};

uint8_t data[7 * max_fifo_words]{};

} buffer{};

const size_t transfer_size = 1 + (samples_to_read * 7);

if (transfer(reinterpret_cast<uint8_t *>(&buffer), reinterpret_cast<uint8_t *>(&buffer), transfer_size) != PX4_OK) {

perf_count(_bad_transfer_perf);

} else {

for (uint16_t i = 0; i < samples_to_read; i++) {

const uint8_t *word = &buffer.data[i * 7];

// Decode tag from upper 5 bits

const uint8_t tag_id = word[0] >> 3;

const int16_t data_x = combine(word[2], word[1]);

const int16_t data_y = combine(word[4], word[3]);

const int16_t data_z = combine(word[6], word[5]);

if (tag_id == static_cast<uint8_t>(FifoTag::GYRO_NC)) {

if (gyro.samples < (sizeof(gyro.x) / sizeof(gyro.x[0]))) {

gyro.x[gyro.samples] = data_x;

gyro.y[gyro.samples] = data_y;

gyro.z[gyro.samples] = data_z;

gyro.samples++;

}

} else if (tag_id == static_cast<uint8_t>(FifoTag::ACCEL_NC)) {

if (accel.samples < (sizeof(accel.x) / sizeof(accel.x[0]))) {

accel.x[accel.samples] = data_x;

accel.y[accel.samples] = data_y;

accel.z[accel.samples] = data_z;

accel.samples++;

}

} else if (tag_id == static_cast<uint8_t>(FifoTag::TEMPERATURE)) {

const int16_t temp_raw = combine(word[2], word[1]);

const float temperature = (temp_raw / 256.0f) + 25.0f;

if (PX4_ISFINITE(temperature)) {

_px4_accel.set_temperature(temperature);

_px4_gyro.set_temperature(temperature);

}

}

// Other tags (TIMESTAMP, etc.) are silently ignored

}

I checked this again against the datasheet, and no code change is needed here.

A burst read is not valid for this FIFO interface. Starting at FIFO_DATA_OUT_TAG (0x78), IF_INC advances linearly through 0x7E for a single FIFO word and then continues to 0x7F and beyond rather than wrapping back to 0x78 for the next dequeued word. So only the first 7-byte FIFO word would be valid in a single bulk transfer, and the subsequent bytes would not correspond to FIFO data. For this device, reading one FIFO word per transaction is intentional and matches the hardware behavior.

Copilot · 2026-04-02T05:03:03Z

src/drivers/drv_sensor.h

 #define DRV_TEMP_DEVTYPE_MCP9808	0xEE
 #define DRV_TEMP_DEVTYPE_TMP102		0xF0

+#define DRV_IMU_DEVTYPE_ST_LSM6DSV   0xF1


This new devtype define doesn't match the surrounding formatting (tabs/alignment) used for other DRV_*_DEVTYPE_* constants in this file. Please align it to keep the header consistent (e.g., use tabs like adjacent lines).

Suggested change

#define DRV_IMU_DEVTYPE_ST_LSM6DSV 0xF1

#define DRV_IMU_DEVTYPE_ST_LSM6DSV 0xF1

Fixed in a follow-up commit.

Add a new SPI driver for the ST LSM6DSV16X 6-axis IMU with FIFO-based accel, gyro, and temperature acquisition. The driver supports continuous FIFO mode with tagged sample parsing, DRDY interrupt triggering via FIFO threshold, and dynamic watermark configuration based on the selected sample rate. It also falls back to scheduled polling when a GPIO interrupt is not available. To improve robustness, the driver periodically validates critical register settings and limits FIFO draining per cycle. Tested on Pixhawk6C with ground testing and flight validation.

Enable the ST LSM6DSV16X as an alternative probe-based IMU on Pixhawk6C. The sensor is configured on SPI1 with chip-select PC13 and shares the same CS and DRDY lines as the ICM42688P across all supported hardware revisions. Board startup first probes the ICM42688P and then the LSM6DSV16X, so only the IMU populated on a given board is started. This change enables the LSM6DSV16X driver in the board configuration, registers the device type in `spi.cpp`, and adds board sensor probing for the new IMU. Use rotation `-R 26` (`ROTATION_PITCH_180_YAW_90`) to match the sensor orientation on the Pixhawk6C PCB.

- add missing #include <cstddef> for size_t - check FIFO status transfer() return value - fix FIFO word count to sample period conversion (DIFF_FIFO reports words, not sample periods; each sample period is 2 words) - fix drv_sensor.h macro alignment (spaces to tab)

zebulon-86 mentioned this pull request Apr 2, 2026

feat(drivers): add ST LSM6DSV16X driver #26950

Closed

mrpollo requested a review from Copilot April 2, 2026 04:55

Copilot started reviewing on behalf of mrpollo April 2, 2026 04:56 View session

Copilot AI reviewed Apr 2, 2026

View reviewed changes

zebulon-86 added 3 commits April 2, 2026 18:13

zebulon-86 force-pushed the lsm6dsv-fmu-v6c-board branch from 32bc3ed to f77cad1 Compare April 2, 2026 10:17

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

feat(boards/px4_fmu-v6c): add Pixhawk6C support for LSM6DSV16X#26951

feat(boards/px4_fmu-v6c): add Pixhawk6C support for LSM6DSV16X#26951
zebulon-86 wants to merge 3 commits intoPX4:mainfrom
zebulon-86:lsm6dsv-fmu-v6c-board

zebulon-86 commented Apr 2, 2026 •

edited

Loading

Uh oh!

Copilot AI left a comment

Uh oh!

Copilot AI Apr 2, 2026

Uh oh!

zebulon-86 Apr 2, 2026

Uh oh!

Copilot AI Apr 2, 2026

Uh oh!

zebulon-86 Apr 2, 2026

Uh oh!

Copilot AI Apr 2, 2026

Uh oh!

zebulon-86 Apr 2, 2026

Uh oh!

Copilot AI Apr 2, 2026

Uh oh!

zebulon-86 Apr 2, 2026

Uh oh!

Copilot AI Apr 2, 2026

Uh oh!

zebulon-86 Apr 2, 2026

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

2 participants

-			transfer((uint8_t *)&fifo_status, (uint8_t *)&fifo_status, sizeof(fifo_status));
+			const int ret = transfer((uint8_t *)&fifo_status, (uint8_t *)&fifo_status, sizeof(fifo_status));
+			if (ret != OK) {
+				perf_count(_bad_transfer_perf);
+				_failure_count++;
+				if (_failure_count > 10) {
+					Reset();
+				}
+				break;
+			}

	#define DRV_IMU_DEVTYPE_ST_LSM6DSV 0xF1
	#define DRV_IMU_DEVTYPE_ST_LSM6DSV 0xF1

		*/

		#pragma once

Conversation

zebulon-86 commented Apr 2, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Summary

Details

Testing

Uh oh!

Copilot AI left a comment

Choose a reason for hiding this comment

Pull request overview

Reviewed changes

Uh oh!

Copilot AI Apr 2, 2026

Choose a reason for hiding this comment

Uh oh!

zebulon-86 Apr 2, 2026

Choose a reason for hiding this comment

Uh oh!

Copilot AI Apr 2, 2026

Choose a reason for hiding this comment

Uh oh!

zebulon-86 Apr 2, 2026

Choose a reason for hiding this comment

Uh oh!

Copilot AI Apr 2, 2026

Choose a reason for hiding this comment

Uh oh!

zebulon-86 Apr 2, 2026

Choose a reason for hiding this comment

Uh oh!

Copilot AI Apr 2, 2026

Choose a reason for hiding this comment

Uh oh!

zebulon-86 Apr 2, 2026

Choose a reason for hiding this comment

Uh oh!

Copilot AI Apr 2, 2026

Choose a reason for hiding this comment

Uh oh!

zebulon-86 Apr 2, 2026

Choose a reason for hiding this comment

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

2 participants

zebulon-86 commented Apr 2, 2026 •

edited

Loading