drivers/ins: Add driver for InertialLabs INS

Signed-off-by: Valentin Bugrov <vladbvnsk@gmail.com>

src/drivers/drv_sensor.h

@@ -250,6 +250,8 @@
 #define DRV_BARO_DEVTYPE_AUAV		0xE7
 #define DRV_BARO_DEVTYPE_SPA06          0xE8
 
+#define DRV_INS_DEVTYPE_ILABS           0xE9
+
 #define DRV_DEVTYPE_UNUSED		0xff
 
 #endif /* _DRV_SENSOR_H */

src/drivers/ins/Kconfig

@@ -3,6 +3,7 @@ menu "Inertial Navigation Systems (INS)"
         bool "All INS sensors"
         default n
         select DRIVERS_INS_VECTORNAV
+        select DRIVERS_INS_ILABS
         ---help---
             Enable default set of INS sensors
     rsource "*/Kconfig"

src/drivers/ins/ilabs/CMakeLists.txt

@@ -0,0 +1,49 @@
+############################################################################
+#
+#   Copyright (c) 2025 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+add_subdirectory(libilabs)
+
+px4_add_module(
+	MODULE drivers__ins__ilabs
+	MAIN ilabs
+	INCLUDES
+		libilabs/include
+	COMPILE_FLAGS
+	SRCS
+		ILabs.cpp
+		ILabs.h
+	MODULE_CONFIG
+		module.yaml
+	DEPENDS
+		libilabs
+	)

src/drivers/ins/ilabs/ILabs.cpp

@@ -0,0 +1,525 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2025 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include "ILabs.h"
+
+#include <matrix/Euler.hpp>
+#include <matrix/Quaternion.hpp>
+#include <px4_platform_common/getopt.h>
+#include <px4_platform_common/log.h>
+
+#ifndef MODULE_NAME
+#define MODULE_NAME "ilabs_ins_driver"  // NOLINT(cppcoreguidelines-macro-usage)
+#endif
+
+using namespace time_literals;
+
+// GPS epoch: 1980-01-06 00:00:00 UTC
+constexpr uint64_t GPS_EPOCH_SECS = 315964800ULL;
+
+constexpr uint8_t DECIMATION_VALUE = 20;
+
+uint64_t ToUtcMicroseconds(uint16_t gpsWeek, uint32_t msTow) {
+	const uint64_t gpsTimeSec = gpsWeek * 7ULL * 86400ULL + msTow / 1000ULL;
+	// 18 is a leap seconds correction
+	const uint64_t utcTimeSec = gpsTimeSec + GPS_EPOCH_SECS - 18;
+	const uint64_t timeUtcUsec = utcTimeSec * 1'000'000 + (msTow % 1000) * 1000ULL;
+	return timeUtcUsec;
+}
+
+enum ILabsMode {
+	RAW_SENSORS_DATA = 0,
+	FULL_INS         = 1,
+};
+
+ILabs::ILabs(const char *serialDeviceName)
+	: ModuleParams(nullptr),
+	  ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(serialDeviceName)),
+	  _attitude_pub((_param_ilabs_mode.get() == ILabsMode::RAW_SENSORS_DATA) ? ORB_ID(external_ins_attitude)
+																			 : ORB_ID(vehicle_attitude)),
+	  _local_position_pub((_param_ilabs_mode.get() == ILabsMode::RAW_SENSORS_DATA) ? ORB_ID(external_ins_local_position)
+																				   : ORB_ID(vehicle_local_position)),
+	  _global_position_pub((_param_ilabs_mode.get() == ILabsMode::RAW_SENSORS_DATA)
+							   ? ORB_ID(external_ins_global_position)
+							   : ORB_ID(vehicle_global_position)) {
+	// store port name
+	strncpy(_serialDeviceName, serialDeviceName, sizeof(_serialDeviceName) - 1);
+
+	// enforce null termination
+	_serialDeviceName[sizeof(_serialDeviceName) - 1] = '\0';
+
+	if (_param_ilabs_mode.get() == ILabsMode::FULL_INS) {
+		int32_t value = 0;
+		param_set(param_find("SENS_IMU_MODE"), &value);
+		param_set(param_find("SENS_MAG_MODE"), &value);
+	}
+
+	_device_id.devid_s.devtype  = DRV_INS_DEVTYPE_ILABS;
+	_device_id.devid_s.bus_type = device::Device::DeviceBusType_SERIAL;
+
+	_px4_accel.set_device_id(_device_id.devid);
+	_px4_gyro.set_device_id(_device_id.devid);
+	_px4_mag.set_device_id(_device_id.devid);
+
+	_attitude_pub.advertise();
+	_local_position_pub.advertise();
+	_global_position_pub.advertise();
+	_sensor_baro_pub.advertise();
+	_sensor_gps_pub.advertise();
+}
+
+ILabs::~ILabs() {
+	_sensor.deinit();
+
+	perf_free(_sample_perf);
+	perf_free(_comms_errors);
+
+	perf_free(_accel_pub_interval_perf);
+	perf_free(_gyro_pub_interval_perf);
+	perf_free(_mag_pub_interval_perf);
+	perf_free(_gnss_pub_interval_perf);
+	perf_free(_baro_pub_interval_perf);
+
+	perf_free(_attitude_pub_interval_perf);
+	perf_free(_local_position_pub_interval_perf);
+	perf_free(_global_position_pub_interval_perf);
+}
+
+int ILabs::task_spawn(int argc, char *argv[]) {
+	bool error_flag = false;
+
+	int         opt_index   = 1;
+	const char *opt_arg     = nullptr;
+	int         opt_val     = 0;
+	const char *device_name = nullptr;
+
+	while ((opt_val = px4_getopt(argc, argv, "d:", &opt_index, &opt_arg)) != EOF) {
+		switch (opt_val) {
+			case 'd':
+				device_name = opt_arg;
+				break;
+
+			case '?':
+				error_flag = true;
+				break;
+
+			default:
+				PX4_WARN("Unrecognized flag");
+				error_flag = true;
+				break;
+		}
+	}
+
+	if (error_flag) {
+		return -1;
+	}
+
+	if (device_name && (access(device_name, R_OK | W_OK) == 0)) {
+		ILabs *instance = new ILabs(device_name);
+
+		if (instance == nullptr) {
+			PX4_ERR("Alloc failed");
+			return PX4_ERROR;
+		}
+
+		_object.store(instance);
+		_task_id = task_id_is_work_queue;
+
+		instance->ScheduleNow();
+
+		return PX4_OK;
+	}
+
+	if (device_name) {
+		PX4_ERR("Invalid device (-d) %s", device_name);
+
+	} else {
+		PX4_INFO("Valid device required");
+	}
+
+	return PX4_ERROR;
+}
+
+int ILabs::custom_command(int argc, char *argv[]) {
+	return print_usage("unknown command");
+}
+
+int ILabs::print_usage(const char *reason) {
+	if (reason) {
+		PX4_WARN("%s\n", reason);
+	}
+
+	PRINT_MODULE_DESCRIPTION(
+		R"DESCR_STR(
+### Description
+
+Serial bus driver for the ILabs sensors.
+
+Most boards are configured to enable/start the driver on a specified UART using the SENS_ILABS_CFG.
+After that you can use the ILABS_MODE parameter to config outputs:
+
+- Only raw sensor output (the default).
+- Sensor output and INS data such as position and velocity estimates.
+
+Setup/usage information: https://docs.px4.io/main/en/sensor/ilabs.html
+
+### Examples
+
+Attempt to start driver on a specified serial device.
+$ ilabs start -d /dev/ttyS1
+Stop driver
+$ ilabs stop
+)DESCR_STR");
+
+	PRINT_MODULE_USAGE_NAME("ilabs", "driver");
+	PRINT_MODULE_USAGE_SUBCATEGORY("ins");
+	PRINT_MODULE_USAGE_COMMAND_DESCR("start", "Start driver");
+	PRINT_MODULE_USAGE_PARAM_STRING('d', nullptr, nullptr, "Serial device", false);
+	PRINT_MODULE_USAGE_COMMAND_DESCR("status", "Driver status");
+	PRINT_MODULE_USAGE_COMMAND_DESCR("stop", "Stop driver");
+	PRINT_MODULE_USAGE_COMMAND_DESCR("status", "Print driver status");
+
+	return PX4_OK;
+}
+
+int ILabs::print_status() {
+	if (_serialDeviceName[0] != '\0') {
+		PX4_INFO("UART device: %s", _serialDeviceName);
+	}
+
+	perf_print_counter(_sample_perf);
+	perf_print_counter(_comms_errors);
+
+	return 0;
+}
+
+void ILabs::Run() {
+	if (should_exit()) {
+		_sensor.deinit();
+		exit_and_cleanup();
+		return;
+	}
+
+	if (!_sensor.isInitialized()) {
+		const bool result = _sensor.init(_serialDeviceName, this, &ILabs::processDataProxy);
+
+		if (!result) {
+			PX4_ERR("Sensor initializing error");
+			ScheduleDelayed(1_s);
+			return;
+		}
+		_time_initialized.store(hrt_absolute_time());
+	}
+
+	// check for timeout
+	const hrt_abstime time_initialized         = _time_initialized.load();
+	const hrt_abstime time_last_valid_imu_data = _time_last_valid_imu_data.load();
+
+	if (_param_ilabs_mode.get() == ILabsMode::FULL_INS && time_last_valid_imu_data != 0 &&
+		hrt_elapsed_time(&time_last_valid_imu_data) < 3_s) {
+		// update sensor_selection if configured in INS mode
+		if ((_px4_accel.get_device_id() != 0) && (_px4_gyro.get_device_id() != 0)) {
+			sensor_selection_s sensor_selection{};
+			sensor_selection.accel_device_id = _px4_accel.get_device_id();
+			sensor_selection.gyro_device_id  = _px4_gyro.get_device_id();
+			sensor_selection.timestamp       = _time_initialized.load();
+			_sensor_selection_pub.publish(sensor_selection);
+		} else {
+			PX4_ERR("Sensor not initialized");
+		}
+	}
+
+	if (time_initialized != 0 && hrt_elapsed_time(&time_last_valid_imu_data) > 5_s &&
+		time_last_valid_imu_data != 0 && hrt_elapsed_time(&time_last_valid_imu_data) > 1_s) {
+		PX4_ERR("Timeout, reinitializing");
+		_sensor.deinit();
+	}
+
+	ScheduleDelayed(100_ms);
+}
+
+void ILabs::processData(InertialLabs::SensorsData *data) {
+	// PX4 by default uses FRD/NED frame position
+	// Inertial Labs INS by default uses in RFU/ENU frame position
+
+	if (!data) {
+		PX4_ERR("Invalid sensor data");
+		return;
+	}
+
+	const bool isFilterOk = (data->ins.unitStatus & InertialLabs::USW::INITIAL_ALIGNMENT_FAIL) == 0 &&
+				 data->ins.solutionStatus != InertialLabs::InsSolution::INVALID;
+	const bool isAccelOk = (data->ins.unitStatus & InertialLabs::USW::ACCEL_FAIL) == 0;
+	const bool isGyroOk  = (data->ins.unitStatus & InertialLabs::USW::GYRO_FAIL) == 0;
+	const bool isMagOk   = (data->ins.unitStatus & InertialLabs::USW::MAG_FAIL) == 0;
+
+	// true if received new GNSS position or velocity
+	const bool hasNewGpsData = (data->gps.newData & (InertialLabs::NewGpsData::NEW_GNSS_POSITION | InertialLabs::NewGpsData::NEW_GNSS_VELOCITY));
+	const bool hasEnoughSatellites = data->gps.usedSatCount > 3;
+
+	const bool isBaroOk = (data->ins.unitStatus2 & InertialLabs::USW2::ADU_BARO_FAIL) == 0;
+
+	// TODO wind data:
+	// const bool isDiffPressureOk = (data->ins.unitStatus2 & InertialLabs::USW2::ADU_DIFF_PRESS_FAIL) == 0;
+
+	const hrt_abstime time_now_us = hrt_absolute_time();
+	_time_last_valid_imu_data.store(time_now_us);
+
+	// update all temperatures
+	if (isFilterOk) {
+		_px4_accel.set_temperature(data->temperature);  // degC
+		_px4_gyro.set_temperature(data->temperature);   // degC
+		_px4_mag.set_temperature(data->temperature);    // degC
+	}
+
+	// publish accel
+	if (isFilterOk && isAccelOk) {
+		_px4_accel.update(time_now_us, data->accel(0), data->accel(1), data->accel(2));  // NED, in m/s^2
+		perf_count(_accel_pub_interval_perf);
+	}
+
+	// publish gyro
+	if (isFilterOk && isGyroOk) {
+		_px4_gyro.update(time_now_us, data->gyro(0), data->gyro(1), data->gyro(2));  // NED, in rad/s
+		perf_count(_gyro_pub_interval_perf);
+	}
+
+	// publish mag
+	if (isFilterOk && isMagOk) {
+		_px4_mag.update(time_now_us, data->mag(0), data->mag(1), data->mag(2));  // NED, in Gauss
+		perf_count(_mag_pub_interval_perf);
+	}
+
+	// publish baro
+	if (isFilterOk && isBaroOk) {
+		if (_average_sensors_data.count > DECIMATION_VALUE) {
+			sensor_baro_s sensor_baro{};
+			sensor_baro.timestamp = time_now_us;
+			sensor_baro.timestamp_sample = time_now_us;
+
+			sensor_baro.device_id   = _device_id.devid;
+			sensor_baro.pressure    = _average_sensors_data.pressure / static_cast<float>(_average_sensors_data.count);    // Pa
+			sensor_baro.temperature = _average_sensors_data.temperature / static_cast<float>(_average_sensors_data.count);  // degC
+
+			_sensor_baro_pub.publish(sensor_baro);
+			perf_count(_baro_pub_interval_perf);
+
+			_average_sensors_data.count = 0;
+			_average_sensors_data.pressure = 0.0f;
+			_average_sensors_data.temperature = 0.0f;
+		}
+
+		_average_sensors_data.pressure += data->pressure;  // Pa
+		_average_sensors_data.temperature += data->temperature;  // degC
+		++_average_sensors_data.count;
+	}
+
+	// publish attitude
+	if (isFilterOk) {
+		// TODO: Use quaternion message in UDD?
+		const matrix::Quatf quat{matrix::Eulerf(math::radians(data->ins.roll),
+							math::radians(data->ins.pitch),
+							math::radians(data->ins.yaw))};
+		vehicle_attitude_s  attitude{};
+		attitude.timestamp        = time_now_us;
+		attitude.timestamp_sample = time_now_us;
+
+		attitude.q[0] = quat(0);
+		attitude.q[1] = quat(1);
+		attitude.q[2] = quat(2);
+		attitude.q[3] = quat(3);
+
+		_attitude_pub.publish(attitude);
+		perf_count(_attitude_pub_interval_perf);
+	}
+
+	// publish local position
+	if (isFilterOk) {
+		vehicle_local_position_s local_position{};
+		local_position.timestamp        = time_now_us;
+		local_position.timestamp_sample = time_now_us;
+
+		local_position.xy_valid   = true;
+		local_position.z_valid    = true;
+		local_position.v_xy_valid = true;
+		local_position.v_z_valid  = true;
+
+		if (!_pos_ref.isInitialized()) {
+			_pos_ref.initReference(data->ins.latitude, data->ins.longitude, time_now_us);
+		}
+		const matrix::Vector2f pos_ned = _pos_ref.project(data->ins.latitude, data->ins.longitude);
+		local_position.x               = pos_ned(0);
+		local_position.y               = pos_ned(1);
+		local_position.z               = data->ins.altitude;
+
+		local_position.vx = data->ins.velocity(0);
+		local_position.vy = data->ins.velocity(1);
+		local_position.vz = data->ins.velocity(2);
+
+		local_position.ax = data->accel(0);
+		local_position.ay = data->accel(1);
+		local_position.az = data->accel(2);
+
+		local_position.heading = static_cast<float>(data->ext.headingData.heading) *
+								 static_cast<float>(M_DEG_TO_RAD) * 0.01f;  // rad
+		local_position.unaided_heading          = NAN;
+		local_position.heading_good_for_control = true;
+
+		local_position.xy_global     = true;
+		local_position.ref_timestamp = _pos_ref.getProjectionReferenceTimestamp();
+		local_position.ref_lat       = _pos_ref.getProjectionReferenceLat();
+		local_position.ref_lon       = _pos_ref.getProjectionReferenceLon();
+		local_position.z_global      = true;
+
+		local_position.dist_bottom_valid = false;
+
+		const float lat_err = static_cast<float>(data->ins.accuracy.lat) * 0.001f;
+		const float lon_err = static_cast<float>(data->ins.accuracy.lon) * 0.001f;
+		local_position.eph = sqrtf(lat_err * lat_err + lon_err * lon_err);
+		local_position.epv = static_cast<float>(data->ins.accuracy.alt) * 0.001f;
+
+		const float northVel_err = static_cast<float>(data->ins.accuracy.northVel) * 0.001f;
+		const float eastVel_err = static_cast<float>(data->ins.accuracy.eastVel) * 0.001f;
+		local_position.evh = sqrtf(northVel_err * northVel_err + eastVel_err * eastVel_err);
+
+		local_position.evv = static_cast<float>(data->ins.accuracy.verVel) * 0.001f;
+
+		local_position.dead_reckoning = false;
+
+		local_position.vxy_max     = INFINITY;
+		local_position.vz_max      = INFINITY;
+		local_position.hagl_min    = INFINITY;
+		local_position.hagl_max_z  = INFINITY;
+		local_position.hagl_max_xy = INFINITY;
+
+		_local_position_pub.publish(local_position);
+		perf_count(_local_position_pub_interval_perf);
+	}
+
+	// publish global_position
+	if (isFilterOk) {
+		vehicle_global_position_s global_position{};
+		global_position.timestamp        = time_now_us;
+		global_position.timestamp_sample = time_now_us;
+
+		global_position.lat_lon_valid = true;
+		global_position.alt_valid     = true;
+		global_position.lat           = data->ins.latitude;
+		global_position.lon           = data->ins.longitude;
+		global_position.alt           = data->ins.altitude;
+
+		const float lat_err = static_cast<float>(data->ins.accuracy.lat) * 0.001f;
+		const float lon_err = static_cast<float>(data->ins.accuracy.lon) * 0.001f;
+		global_position.eph           = sqrtf(lat_err * lat_err + lon_err * lon_err);
+		global_position.epv           = static_cast<float>(data->ins.accuracy.alt) * 0.001f;
+
+		global_position.dead_reckoning = false;
+
+		_global_position_pub.publish(global_position);
+		perf_count(_global_position_pub_interval_perf);
+	}
+
+	// publish GPS data
+	if (hasEnoughSatellites && isFilterOk && hasNewGpsData) {
+		sensor_gps_s sensor_gps{};
+		sensor_gps.timestamp        = time_now_us;
+		sensor_gps.timestamp_sample = time_now_us;
+
+		sensor_gps.device_id = _device_id.devid;
+
+		sensor_gps.latitude_deg   = data->gps.latitude;
+		sensor_gps.longitude_deg  = data->gps.longitude;
+		sensor_gps.altitude_ellipsoid_m = data->gps.altitude;
+		sensor_gps.altitude_msl_m = data->gps.altitude;
+
+		sensor_gps.fix_type = data->gps.fixType + 1;
+
+		const float lat_err = static_cast<float>(data->ins.accuracy.lat) * 0.001f;
+		const float lon_err = static_cast<float>(data->ins.accuracy.lon) * 0.001f;
+		sensor_gps.eph = sqrtf(lat_err * lat_err + lon_err * lon_err);
+		sensor_gps.epv = static_cast<float>(data->ins.accuracy.alt) * 0.001f;
+
+		sensor_gps.hdop = static_cast<float>(data->gps.dop.hdop) * 0.001f;
+		sensor_gps.vdop = static_cast<float>(data->gps.dop.vdop) * 0.001f;
+
+		sensor_gps.jamming_state = data->gps.jamStatus;
+		sensor_gps.jamming_indicator = (sensor_gps.jamming_state != InertialLabs::JammingStatus::UNKOWN_OR_DISABLED) &&
+					       (sensor_gps.jamming_state != InertialLabs::JammingStatus::OK);
+		sensor_gps.spoofing_state = data->gps.spoofingStatus;
+
+		sensor_gps.vel_m_s =
+			matrix::Vector3f(data->ins.velocity(0), data->ins.velocity(1), data->ins.velocity(2)).length();
+		sensor_gps.vel_n_m_s     = data->ins.velocity(0);
+		sensor_gps.vel_e_m_s     = data->ins.velocity(1);
+		sensor_gps.vel_d_m_s     = data->ins.velocity(2);
+		sensor_gps.vel_ned_valid = true;
+
+		sensor_gps.time_utc_usec = ToUtcMicroseconds(data->gps.gpsWeek, data->gps.msTow);
+		sensor_gps.timestamp_time_relative = static_cast<int32_t>(sensor_gps.time_utc_usec - time_now_us);
+
+		sensor_gps.satellites_used = data->gps.usedSatCount;
+
+		sensor_gps.heading = NAN;
+		sensor_gps.heading_offset = NAN;
+		sensor_gps.heading_accuracy = NAN;
+
+		// sensor_gps.s_variance_m_s = ...; // TODO: need 0x43 UDD Package?
+
+		_sensor_gps_pub.publish(sensor_gps);
+		perf_count(_gnss_pub_interval_perf);
+	}
+
+	if (_param_ilabs_mode.get() == ILabsMode::FULL_INS) {
+		estimator_status_s estimator_status{};
+		estimator_status.timestamp        = time_now_us;
+		estimator_status.timestamp_sample = time_now_us;
+
+		const float test_ratio = 0.1f;
+
+		estimator_status.hdg_test_ratio = test_ratio;
+		estimator_status.vel_test_ratio = test_ratio;
+		estimator_status.pos_test_ratio = test_ratio;
+		estimator_status.hgt_test_ratio = test_ratio;
+
+		estimator_status.accel_device_id = _px4_accel.get_device_id();
+		estimator_status.gyro_device_id  = _px4_gyro.get_device_id();
+		estimator_status.mag_device_id  = _device_id.devid;
+		estimator_status.baro_device_id  = _device_id.devid;
+
+		_estimator_status_pub.publish(estimator_status);
+	}
+}
+
+extern "C" __EXPORT int ilabs_main(int argc, char *argv[]) {
+	return ILabs::main(argc, argv);
+}

src/drivers/ins/ilabs/ILabs.h

@@ -0,0 +1,126 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2025 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ *
+ * Driver for the InertialLabs INS, AHRS
+ */
+
+#pragma once
+
+#include <stdio.h>
+
+#include <drivers/accelerometer/PX4Accelerometer.hpp>
+#include <drivers/device/Device.hpp>
+#include <drivers/gyroscope/PX4Gyroscope.hpp>
+#include <drivers/magnetometer/PX4Magnetometer.hpp>
+#include <perf/perf_counter.h>
+#include <px4_platform_common/module.h>
+#include <px4_platform_common/module_params.h>
+#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
+#include <uORB/Publication.hpp>
+#include <uORB/topics/estimator_status.h>
+#include <uORB/topics/sensor_baro.h>
+#include <uORB/topics/sensor_gps.h>
+#include <uORB/topics/sensor_selection.h>
+#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/vehicle_local_position.h>
+
+#include "sensor.h"
+
+class ILabs : public ModuleBase<ILabs>, public ModuleParams, public px4::ScheduledWorkItem {
+public:
+	ILabs(const char *port);
+	~ILabs() override;
+
+	/** @see ModuleBase */
+	static int task_spawn(int argc, char *argv[]);
+
+	/** @see ModuleBase */
+	static int custom_command(int argc, char *argv[]);
+
+	/** @see ModuleBase */
+	static int print_usage(const char *reason = nullptr);
+
+	/** @see ModuleBase::print_status() */
+	int print_status() override;
+
+private:
+	void        Run() override;
+	void        processData(InertialLabs::SensorsData *sensordata);
+	static void processDataProxy(void *context, InertialLabs::SensorsData *data) {
+		ILabs *self = static_cast<ILabs *>(context);
+		self->processData(data);
+	}
+
+	InertialLabs::Sensor _sensor{};
+
+	char _serialDeviceName[20]{};
+	InertialLabs::AverageSensorsData _average_sensors_data{};
+	device::Device::DeviceId _device_id{};
+
+	px4::atomic<hrt_abstime> _time_initialized{0};
+	px4::atomic<hrt_abstime> _time_last_valid_imu_data{0};
+
+	PX4Accelerometer _px4_accel{0};
+	PX4Gyroscope     _px4_gyro{0};
+	PX4Magnetometer  _px4_mag{0};
+
+	MapProjection _pos_ref{};
+
+	uORB::PublicationMulti<vehicle_attitude_s>        _attitude_pub{ORB_ID(vehicle_attitude)};
+	uORB::PublicationMulti<vehicle_local_position_s>  _local_position_pub{ORB_ID(vehicle_local_position)};
+	uORB::PublicationMulti<vehicle_global_position_s> _global_position_pub{ORB_ID(vehicle_global_position)};
+	uORB::PublicationMulti<sensor_baro_s>             _sensor_baro_pub{ORB_ID(sensor_baro)};
+	uORB::PublicationMulti<sensor_gps_s>              _sensor_gps_pub{ORB_ID(sensor_gps)};
+	uORB::Publication<sensor_selection_s>             _sensor_selection_pub{ORB_ID(sensor_selection)};
+	uORB::Publication<estimator_status_s>             _estimator_status_pub{ORB_ID(estimator_status)};
+
+	perf_counter_t _comms_errors{perf_alloc(PC_COUNT, MODULE_NAME ": com_err")};
+	perf_counter_t _sample_perf{perf_alloc(PC_ELAPSED, MODULE_NAME ": read")};
+
+	perf_counter_t _accel_pub_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME ": accel publish interval")};
+	perf_counter_t _gyro_pub_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME ": gyro publish interval")};
+	perf_counter_t _mag_pub_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME ": mag publish interval")};
+	perf_counter_t _gnss_pub_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME ": GNSS publish interval")};
+	perf_counter_t _baro_pub_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME ": baro publish interval")};
+
+	perf_counter_t _attitude_pub_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME ": attitude publish interval")};
+	perf_counter_t _local_position_pub_interval_perf{
+	    perf_alloc(PC_INTERVAL, MODULE_NAME ": local position publish interval")};
+	perf_counter_t _global_position_pub_interval_perf{
+	    perf_alloc(PC_INTERVAL, MODULE_NAME ": global position publish interval")};
+
+	DEFINE_PARAMETERS((ParamInt<px4::params::ILABS_MODE>)_param_ilabs_mode)
+};

src/drivers/ins/ilabs/Kconfig

@@ -0,0 +1,5 @@
+menuconfig DRIVERS_INS_ILABS
+	bool "ilabs"
+	default n
+	---help---
+		Enable support for ilabs

src/drivers/ins/ilabs/libilabs/CMakeLists.txt

@@ -0,0 +1,54 @@
+############################################################################
+#
+#   Copyright (c) 2025 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+set(LIBRARY_NAME "libilabs")
+
+set(SOURCES
+	"${CMAKE_CURRENT_SOURCE_DIR}/src/sensor.cpp"
+)
+
+add_library(${LIBRARY_NAME} ${SOURCES})
+target_compile_options(${LIBRARY_NAME}
+	PRIVATE
+		-Wno-unused-function
+)
+
+target_include_directories(${LIBRARY_NAME} PUBLIC
+	"${CMAKE_CURRENT_SOURCE_DIR}/include"
+)
+
+add_dependencies(${LIBRARY_NAME} prebuild_targets)
+
+if("${PX4_PLATFORM}" MATCHES "nuttx")
+	target_compile_definitions(${LIBRARY_NAME} PUBLIC __NUTTX__)
+endif()

src/drivers/ins/ilabs/libilabs/include/data.h

@@ -0,0 +1,426 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2025 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include <stdint.h>
+
+#include <matrix/matrix/math.hpp>
+
+#define PACKED __attribute__((packed))
+
+namespace InertialLabs {
+
+// NOLINTBEGIN(clang-analyzer-optin.performance.Padding, altera-struct-pack-align, misc-non-private-member-variables-in-classes)
+struct PACKED vec3_16_t {
+	int16_t x, y, z;
+
+	matrix::Vector3f toFloat() const {
+		return matrix::Vector3f(static_cast<float>(x), static_cast<float>(y), static_cast<float>(z));
+	}
+};
+
+struct PACKED vec3_32_t {
+	int32_t x, y, z;
+
+	matrix::Vector3f toFloat() const {
+		return matrix::Vector3f(static_cast<float>(x), static_cast<float>(y), static_cast<float>(z));
+	}
+};
+
+enum DataType : uint8_t {
+	GPS_INS_TIME_MS       = 0x01,
+	GPS_WEEK              = 0x3C,
+	ACCEL_DATA_HR         = 0x23,
+	GYRO_DATA_HR          = 0x21,
+	BARO_DATA             = 0x25,
+	MAG_DATA              = 0x24,
+	SENSOR_BIAS           = 0x26,
+	ORIENTATION_ANGLES    = 0x07,
+	VELOCITIES            = 0x12,
+	POSITION              = 0x10,
+	UNIT_STATUS           = 0x53,
+	GNSS_EXTENDED_INFO    = 0x4A,
+	GNSS_POSITION         = 0x30,
+	GNSS_VEL_TRACK        = 0x32,
+	GNSS_POS_TIMESTAMP    = 0x3E,
+	GNSS_NEW_DATA         = 0x41,
+	GNSS_JAM_STATUS       = 0xC0,
+	DIFFERENTIAL_PRESSURE = 0x28,
+	TRUE_AIRSPEED         = 0x86,
+	CALIBRATED_AIRSPEED   = 0x85,
+	WIND_SPEED            = 0x8A,
+	AIR_DATA_STATUS       = 0x8D,
+	SUPPLY_VOLTAGE        = 0x50,
+	TEMPERATURE           = 0x52,
+	UNIT_STATUS2          = 0x5A,
+	GNSS_DOP              = 0x42,
+	INS_SOLUTION_STATUS   = 0x54,
+	INS_POS_VEL_ACCURACY  = 0x5F,
+	FULL_SAT_INFO         = 0x37,
+	USED_SAT_COUNT        = 0x3B,
+	GNSS_VEL_LATENCY      = 0x3D,
+	GNSS_SOL_STATUS       = 0x38,
+	GNSS_POS_VEL_TYPE     = 0x39,
+	NEW_AIDING_DATA       = 0x65,
+	NEW_AIDING_DATA2      = 0xA1,
+	EXT_SPEED             = 0x61,
+	EXT_HOR_POS           = 0x6E,
+	EXT_ALT               = 0x6C,
+	EXT_HEADING           = 0x66,
+	EXT_AMBIENT_DATA      = 0x6B,
+	EXT_WIND_DATA         = 0x62,
+	MAG_CLB_ACCURACY      = 0x9A,
+};
+
+// Ins Solutiob Status
+enum InsSolution {
+	GOOD                       = 0,
+	KF_CONVERGENCE_IN_PROGRESS = 1,
+	GNSS_ABSENCE               = 2,
+	NO_HEADING_CORRECTION      = 3,
+	AUTONOMOUS_MODE            = 4,
+	NO_GNSS_AIDING_DATA        = 5,
+	AUTONIMUS_TIME_EXCEEDED    = 6,
+	ZUPT_MODE                  = 7,
+	INVALID                    = 8,
+};
+
+// Unit Status Word bits
+enum USW {
+	INITIAL_ALIGNMENT_FAIL = (1U << 0),
+	OPERATION_FAIL         = (1U << 1),
+	GYRO_FAIL              = (1U << 2),
+	ACCEL_FAIL             = (1U << 3),
+	MAG_FAIL               = (1U << 4),
+	ELECTRONICS_FAIL       = (1U << 5),
+	GNSS_FAIL              = (1U << 6),
+	MAG_VG3D_CLB_RUNTIME   = (1U << 7),
+	VOLTAGE_LOW            = (1U << 8),
+	VOLTAGE_HIGH           = (1U << 9),
+	GYRO_X_RATE_HIGH       = (1U << 10),
+	GYRO_Y_RATE_HIGH       = (1U << 11),
+	GYRO_Z_RATE_HIGH       = (1U << 12),
+	MAG_FIELD_HIGH         = (1U << 13),
+	TEMP_RANGE_ERR         = (1U << 14),
+	MAG_VG3D_CLB_SUCCESS   = (1U << 15),
+};
+
+// Unit Status Word2 bits
+enum USW2 {
+	ACCEL_X_HIGH            = (1U << 0),
+	ACCEL_Y_HIGH            = (1U << 1),
+	ACCEL_Z_HIGH            = (1U << 2),
+	ADU_BARO_FAIL           = (1U << 3),
+	ADU_DIFF_PRESS_FAIL     = (1U << 4),
+	MAG_AUTO_CAL_2D_RUNTIME = (1U << 5),
+	MAG_AUTO_CAL_3D_RUNTIME = (1U << 6),
+	GNSS_FUSION_OFF         = (1U << 7),
+	DIFF_PRESS_FUSION_OFF   = (1U << 8),
+	GNSS_POS_VALID          = (1U << 10),
+};
+
+// Air Data Status bits
+enum ADU {
+	BARO_INIT_FAIL           = (1U << 0),
+	DIFF_PRESS_INIT_FAIL     = (1U << 1),
+	BARO_FAIL                = (1U << 2),
+	DIFF_PRESS_FAIL          = (1U << 3),
+	BARO_RANGE_ERR           = (1U << 4),
+	DIFF_PRESS_RANGE_ERR     = (1U << 5),
+	BARO_ALT_FAIL            = (1U << 8),
+	AIRSPEED_FAIL            = (1U << 9),
+	AIRSPEED_BELOW_THRESHOLD = (1U << 10),
+};
+
+// New GPS indicator
+enum NewGpsData {
+	NEW_GNSS_POSITION    = (1U << 0),
+	NEW_GNSS_VELOCITY    = (1U << 1),
+	NEW_GNSS_HEADING     = (1U << 2),
+	NEW_VALID_PPS        = (1U << 3),
+	NEW_GNSS_BESTXYZ_LOG = (1U << 4),
+	NEW_GNSS_PSRDOP_LOG  = (1U << 5),
+	NEW_PPS              = (1U << 6),
+	NEW_RANGE_LOG        = (1U << 7),
+};
+
+enum NewAidingData {
+	NEW_AIRSPEED    = (1U << 1),
+	NEW_WIND        = (1U << 2),
+	NEW_EXT_POS     = (1U << 3),
+	NEW_HEADING     = (1U << 5),
+	NEW_AMBIENT     = (1U << 10),
+	NEW_ALTITUDE    = (1U << 11),
+	NEW_EXT_HOR_POS = (1U << 13),
+	NEW_ADC         = (1U << 15),
+};
+
+enum JammingStatus : uint8_t {
+	UNKOWN_OR_DISABLED = 0,
+	OK                 = 1,  // No significant jamming
+	WARNING            = 2,  // Interference visible but fix ok
+	CRITICAL           = 3,  // Interference visible and no fix
+};
+
+enum SpoofingStatus : uint8_t {
+	UNKOWN_OR_DEACTIVATED = 0,
+	NO                    = 1,
+	INDICATED             = 2,
+	MULTIPLE_INCIDATIONS  = 3,
+};
+
+/*
+	Every data package consist of:
+		MessageHeader (6 bytes)
+		Payload
+		Checksum (2 bytes)
+
+	Whole data package size = MessageHeader->msgLen + 2(0x55AA)
+
+	User Defined Data payload consist of:
+		Number of data structures
+		List of data structures ID
+		List of data structures Values
+
+*/
+struct PACKED MessageHeader {
+	uint16_t packageHeader;  // 0x55AA
+	uint8_t  msgType;        // always 1 for INS data
+	uint8_t  msgId;          // always 0x95
+	uint16_t msgLen;         // payload + 6(msgType + msgId + msgLen + checksum). Not included packageHeader
+};
+
+struct PACKED SensorBias {
+	int8_t gyroX;  // deg/s*0.5*1e5
+	int8_t gyroY;  // deg/s*0.5*1e5
+	int8_t gyroZ;  // deg/s*0.5*1e5
+	int8_t accX;   // g*0.5*1e6
+	int8_t accY;   // g*0.5*1e6
+	int8_t accZ;   // g*0.5*1e6
+	int8_t reserved;
+};
+
+struct PACKED GnssExtendedInfo {
+	uint8_t fixType;
+	uint8_t spoofingStatus;
+};
+
+struct PACKED GnssDop {
+	uint16_t gdop;  // *1000
+	uint16_t pdop;  // *1000
+	uint16_t hdop;  // *1000
+	uint16_t vdop;  // *1000
+	uint16_t tdop;  // *1000
+};
+
+struct PACKED InsAccuracy {
+	int32_t lat;       // m*1000
+	int32_t lon;       // m*1000
+	int32_t alt;       // m*1000
+	int32_t eastVel;   // m/s*1000
+	int32_t northVel;  // m/s*1000
+	int32_t verVel;    // m/s*1000
+};
+
+struct PACKED ExtHorPos {
+	int32_t  lat;         // deg*1.0e7
+	int32_t  lon;         // deg*1.0e7
+	uint16_t latStd;      // m*100
+	uint16_t lonStd;      // m*100
+	uint16_t posLatency;  // sec*1000
+};
+
+struct PACKED ExtAlt {
+	int32_t  alt;     // m*1000, can be WGS84 or AMSL
+	uint16_t altStd;  // m*100
+};
+
+struct PACKED ExtHeading {
+	uint16_t heading;  // deg*100
+	uint16_t std;      // deg*100
+	uint16_t latency;  // sec*1000
+};
+
+struct PACKED ExtAmbientData {
+	int16_t  airTemp;   // degC*10
+	int32_t  alt;       // m*100, can be WGS84 or AMSL
+	uint16_t absPress;  // Pa/2
+};
+
+struct PACKED ExtWindData {
+	int16_t  nWindVel;  // kt*100
+	int16_t  eWindVel;  // kt*100
+	uint16_t nStdWind;  // kt*100
+	uint16_t eStdWind;  // kt*100
+};
+
+union PACKED UDDMessageData {
+	uint32_t  gpsTimeMs;  // ms since start of GPS week
+	uint16_t  gpsWeek;
+	vec3_32_t accelDataHr;  // g * 1e6
+	vec3_32_t gyroDataHr;   // deg/s * 1e5
+	struct PACKED {
+		uint16_t pressurePa2;  // Pascals/2
+		int32_t  baroAlt;      // meters*100, can be WGS84 or AMSL
+	} baroData;
+	vec3_16_t  magData;  // nT/10
+	SensorBias sensorBias;
+	struct PACKED {
+		uint16_t yaw;    // deg*100
+		int16_t  pitch;  // deg*100
+		int16_t  roll;   // deg*100
+	} orientationAngles;     // 321 euler order?
+	vec3_32_t velocity;      // m/s * 100
+	struct PACKED {
+		int32_t lat;  // deg*1e7
+		int32_t lon;  // deg*1e7
+		int32_t alt;  // m*100, can be WGS84 or AMSL
+	} position;
+	uint16_t         unitStatus;  // set ILABS_UNIT_STATUS_*
+	GnssExtendedInfo gnssExtendedInfo;
+	struct PACKED {
+		int32_t lat;  // deg*1e7
+		int32_t lon;  // deg*1e7
+		int32_t alt;  // m*100 can be WGS84 or AMSL
+	} gnssPosition;
+	struct PACKED {
+		int32_t  horSpeed;         // m/s*100
+		uint16_t trackOverGround;  // deg*100
+		int32_t  verSpeed;         // m/s*100
+	} gnssVelTrack;
+	uint32_t       gnssPosTimestamp;  // ms
+	uint8_t        gnssNewData;
+	uint8_t        gnssJamStatus;
+	int32_t        differentialPressure;  // mbar*1e4
+	int16_t        trueAirspeed;          // m/s*100
+	int16_t        calibratedAirspeed;    // m/s*100
+	vec3_16_t      windSpeed;             // m/s*100
+	uint16_t       airDataStatus;
+	uint16_t       supplyVoltage;  // V*100
+	int16_t        temperature;    // degC*10
+	uint16_t       unitStatus2;
+	GnssDop        gnssDop;
+	uint8_t        insSolStatus;
+	InsAccuracy    insAccuracy;
+	uint8_t        usedSatCount;
+	uint16_t       gnssVelLatency;  // ms
+	uint8_t        gnssSolStatus;
+	uint8_t        gnssPosVelType;
+	uint16_t       newAidingData;
+	uint16_t       newAidingData2;
+	int16_t        externalSpeed;  // kt*100
+	ExtHorPos      extHorPos;
+	ExtAlt         extAlt;
+	ExtHeading     extHeading;
+	ExtAmbientData extAmbientAirData;
+	ExtWindData    extWindData;
+	uint8_t        magClbAccuracy;  // deg*10
+};
+
+struct GpsData {
+	uint32_t    msTow;  // ms
+	uint16_t    gpsWeek;
+	double      latitude;         // deg
+	double      longitude;        // deg
+	float       altitude;         // m, can be WGS84 or AMSL
+	float       horSpeed;         // m/s
+	float       verSpeed;         // m/s
+	float       trackOverGround;  // deg
+	GnssDop     dop;
+	uint8_t     newData;
+	uint8_t     fixType;
+	uint8_t     spoofingStatus;
+	uint8_t     jamStatus;
+	uint8_t     usedSatCount;
+	uint16_t    velLatency;  // ms
+	uint8_t     gnssSolStatus;
+	uint8_t     gnssPosVelType;
+};
+
+struct InsData {
+	float            yaw;        // deg
+	float            pitch;      // deg
+	float            roll;       // deg
+	uint32_t         msTow;      // ms
+	double           latitude;   // deg
+	double           longitude;  // deg
+	float            altitude;   // m, can be WGS84 or AMSL
+	matrix::Vector3f velocity;   // NED, in m/s
+	uint16_t         unitStatus;
+	uint16_t         unitStatus2;
+	uint8_t          solutionStatus;
+	InsAccuracy      accuracy;
+	float            baroAlt;             // m
+	float            trueAirspeed;        // m/s
+	float            calibratedAirspeed;  // m/s
+	matrix::Vector3f windSpeed;           // NED, in m/s
+	float            airspeedSf;
+	uint16_t         airDataStatus;
+	SensorBias       sensorBias;
+	float            magClbAccuracy;  // deg
+};
+
+struct ExtData {
+	uint16_t       newAidingData;
+	uint16_t       newAidingData2;
+	int16_t        speed;
+	ExtHorPos      horPos;
+	ExtAlt         altitudeData;
+	ExtHeading     headingData;
+	ExtAmbientData ambientAirData;
+	ExtWindData    windData;
+};
+
+struct SensorsData {
+	matrix::Vector3f accel;  // NED, in m/s^2
+	matrix::Vector3f gyro;   // NED, in rad/s
+	matrix::Vector3f mag;    // NED, in Gauss
+	InsData          ins{};
+	GpsData          gps{};
+	ExtData          ext{};
+	float            pressure;              // Pa
+	float            differentialPressure;  // Pa
+	float            temperature;           // degC
+	float            supplyVoltage;         // V
+};
+
+struct AverageSensorsData {
+	float            pressure;              // Pa
+	float            temperature;           // degC
+	uint8_t          count;                 // number of samples
+};
+
+// NOLINTEND(clang-analyzer-optin.performance.Padding, altera-struct-pack-align, misc-non-private-member-variables-in-classes)
+
+}  // namespace InertialLabs

src/drivers/ins/ilabs/libilabs/include/sensor.h

@@ -0,0 +1,93 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2025 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include <pthread.h>
+
+#include <px4_platform_common/Serial.hpp>
+
+#include "data.h"
+
+namespace InertialLabs {
+
+constexpr uint16_t BUFFER_SIZE{512};
+
+class Sensor {
+public:
+	// Use C-style function pointer, because we can't use std::function on some platforms
+	using DataHandler = void (*)(void *, SensorsData *);
+
+	Sensor()                          = default;
+	Sensor(const Sensor &)            = delete;
+	Sensor &operator=(const Sensor &) = delete;
+	~Sensor();
+
+	bool init(const char *serialDeviceName, void *context, DataHandler dataHandler);
+	void deinit();
+	bool isInitialized() const;
+
+	void updateData();
+
+private:
+	static void *updateDataThreadHelper(void *context) {
+		Sensor *sensor = reinterpret_cast<Sensor *>(context);
+		sensor->updateData();
+		return nullptr;
+	}
+	void resetSerial();
+
+	bool moveToBufferStart(const uint8_t *pos);
+	bool skipPackageInBufferStart();
+	bool movePackageHeaderToBufferStart();
+	bool moveValidPackageToBufferStart();
+
+	bool parseUDDPayload();
+
+	device::Serial *_serial{nullptr};
+	pthread_t       _threadId;
+	bool            _processInThread{false};
+
+	// callback. C-style class method pointer
+	void       *_context{nullptr};
+	DataHandler _dataHandler{nullptr};
+
+	bool _isInitialized{false};
+
+	uint8_t  _buf[BUFFER_SIZE]{};
+	uint16_t _bufOffset{0};
+
+	SensorsData _sensorData{};
+};
+
+}  // namespace InertialLabs

src/drivers/ins/ilabs/libilabs/src/sensor.cpp

@@ -0,0 +1,562 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2025 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include "sensor.h"
+
+#include <px4_platform_common/log.h>
+#include <px4_platform_common/time.h>
+
+#include "data.h"
+
+#ifndef MODULE_NAME
+#define MODULE_NAME "ilabs_ins_driver"  // NOLINT(cppcoreguidelines-macro-usage)
+#endif
+
+namespace {
+
+constexpr int BAUDRATE = 921600;
+
+// acceleration due to gravity in m/s^2 used in IL INS
+constexpr float GRAVITY_MSS = 9.8106f;
+
+bool isMessageHeaderCorrect(const InertialLabs::MessageHeader *header) {
+	if (!header) {
+		PX4_ERR("Invalid header pointer");
+		return false;
+	}
+
+	return header->packageHeader == 0x55AA && header->msgType == 1 && header->msgId == 0x95 &&
+		(header->msgLen + 2) <= InertialLabs::BUFFER_SIZE;  //< 2 is size of PackageHeader, that not included in msgLen
+}
+
+// sums the bytes in the supplied buffer, returns that sum mod 0xFFFF
+uint16_t checksum(const uint8_t *data, uint16_t len) {
+	uint16_t sum = 0;
+	for (uint32_t i = 0; i < len; i++) {
+		sum += data[i];
+	}
+	return sum;
+}
+
+uint16_t readPackageChecksum(const uint8_t *checksumByte) {
+	return (checksumByte[1] << 8) | checksumByte[0];
+}
+
+// Convert from right-front-up to front-right-down or ENU to NED
+matrix::Vector3f rfuToFrd(const matrix::Vector3f &vector3f) {
+	return matrix::Vector3f{vector3f(1), vector3f(0), -vector3f(2)};
+}
+
+}  // namespace
+
+namespace InertialLabs {
+
+Sensor::~Sensor() {
+	deinit();
+}
+
+bool Sensor::init(const char *serialDeviceName, void *context, DataHandler dataHandler) {
+	if (_isInitialized) {
+		PX4_ERR("Serial device already initialized. Deinitialize first.");
+		return false;
+	}
+
+	if (serialDeviceName[0] == '\0') {
+		PX4_ERR("Empty serial device name");
+		return false;
+	}
+
+	if (!context || !dataHandler) {
+		PX4_ERR("Empty data handler callback");
+		return false;
+	}
+
+	_serial = new device::Serial(serialDeviceName, BAUDRATE);
+	if (_serial == nullptr) {
+		PX4_ERR("Error creating serial device: %s", serialDeviceName);
+		px4_sleep(1);  // NOLINT(concurrency-mt-unsafe)
+		return false;
+	}
+
+	if (!_serial->isOpen() && !_serial->open()) {
+		// Open the UART. If this is successful then the UART is ready to use.
+		if (!_serial->open()) {
+			PX4_ERR("Error opening serial device: %s", serialDeviceName);
+			px4_sleep(1);  // NOLINT(concurrency-mt-unsafe)
+			resetSerial();
+			return false;
+		}
+
+		PX4_INFO("Serial device opened sucessfully: %s", serialDeviceName);
+
+		_serial->flush();
+	}
+	_context     = context;
+	_dataHandler = dataHandler;
+
+	_processInThread = true;
+	const int result = pthread_create(&_threadId, nullptr, &Sensor::updateDataThreadHelper, this);
+	if (result) {
+		PX4_ERR("Cant create working thread");
+		_processInThread = false;
+		px4_sleep(1);  // NOLINT(concurrency-mt-unsafe)
+		resetSerial();
+		return false;
+	}
+
+	pthread_detach(_threadId);
+
+	_isInitialized = true;
+	return true;
+}
+
+void Sensor::deinit() {
+	_processInThread = false;
+	_isInitialized = false;
+	resetSerial();
+}
+
+bool Sensor::isInitialized() const {
+	return _isInitialized;
+}
+
+void Sensor::updateData() {
+	while (_processInThread) {
+		bool result = moveValidPackageToBufferStart();
+		if (!result) {
+			continue;
+		}
+
+		result = parseUDDPayload();
+		if (!result) {
+			continue;
+		}
+
+		_dataHandler(_context, &_sensorData);
+
+		skipPackageInBufferStart();
+	}
+}
+
+void Sensor::resetSerial() {
+	if (_serial) {
+		(void)_serial->close();
+		delete _serial;
+		_serial = nullptr;
+	}
+}
+
+bool Sensor::moveToBufferStart(const uint8_t *pos) {
+	if (!pos) {
+		PX4_ERR("Invalid position pointer");
+		return false;
+	}
+
+	const uint16_t bytesFromBufferStart = pos - _buf;
+	if (bytesFromBufferStart > BUFFER_SIZE) {
+		PX4_ERR("Invalid position pointer");
+		return false;
+	}
+
+	if (_bufOffset < bytesFromBufferStart) {
+		PX4_ERR("Buffer offset less than bytes diff need to move");
+		return false;
+	}
+
+	memmove(_buf, pos, _bufOffset - bytesFromBufferStart);
+	_bufOffset -= bytesFromBufferStart;
+	return true;
+}
+
+bool Sensor::skipPackageInBufferStart() {
+	const MessageHeader *packageHeader = reinterpret_cast<MessageHeader *>(_buf);
+	if (!isMessageHeaderCorrect(packageHeader)) {
+		PX4_ERR("Incorrect message header in buffer start. Can't skip the package correctly.");
+		return false;
+	}
+
+	const uint8_t *endPackagePos = &_buf[packageHeader->msgLen + 2];
+
+	return moveToBufferStart(endPackagePos);
+}
+
+bool Sensor::movePackageHeaderToBufferStart() {
+	if (_bufOffset < 3) {
+		PX4_WARN("Buffer offset is too small for the package");
+		_bufOffset = 0;
+		return false;
+	}
+
+	const uint16_t packageHeader   = 0x55AA;
+	const uint8_t *startPackagePos = (const uint8_t *)memmem(&_buf[1],
+								 _bufOffset - sizeof(packageHeader),
+			 					 &packageHeader,
+								 sizeof(packageHeader));
+	if (!startPackagePos) {
+		PX4_WARN("Can't find the package start in the buffer");
+		_bufOffset = 0;
+		return false;
+	}
+
+	return moveToBufferStart(startPackagePos);
+}
+
+bool Sensor::moveValidPackageToBufferStart() {
+	const uint16_t freeBufByteCount = BUFFER_SIZE - _bufOffset;
+	if (freeBufByteCount < sizeof(MessageHeader)) {
+		_bufOffset = 0;
+		PX4_DEBUG("Buffer free space not enough for the message header. Buffer was cleaned");
+		return false;
+	}
+
+	const ssize_t nRead = _serial->readAtLeast(&_buf[_bufOffset], freeBufByteCount, freeBufByteCount, 1000);
+	if (nRead != freeBufByteCount) {
+		PX4_ERR("Can't read requested data from the serial device. Bytes requested: %d. Bytes read: %d",
+				freeBufByteCount, static_cast<int>(nRead));
+		_bufOffset = 0;
+		return false;
+	}
+
+	_bufOffset += nRead;
+
+	if (_bufOffset > BUFFER_SIZE) {
+		PX4_ERR("Buffer read bytes: %d. Buffer size: %d. Offset will be reseted", _bufOffset, BUFFER_SIZE);
+		_bufOffset = 0;
+		return false;
+	}
+
+	const MessageHeader *packageHeader = reinterpret_cast<MessageHeader *>(_buf);
+	if (!isMessageHeaderCorrect(packageHeader)) {
+		PX4_DEBUG("Message header in the buffer start is incorrect");
+		movePackageHeaderToBufferStart();
+		return false;
+	}
+
+	const uint16_t fullMessageLength = packageHeader->msgLen + 2;
+	if (fullMessageLength > _bufOffset) {
+		_bufOffset = 0;
+		PX4_ERR(
+			"The message was not read in full. This situation is not healthy. Is the buffer size enough?\n"
+			"Message was removed and buffer offset was reseted");
+		return false;
+	}
+
+	// Check checksum
+	const uint16_t calculatedChecksum = checksum(&_buf[2], packageHeader->msgLen - 2);
+	const uint16_t readChecksum       = readPackageChecksum(&_buf[fullMessageLength - 2]);
+	if (calculatedChecksum != readChecksum) {
+		PX4_ERR("Invalid package checksum. Calculated checksum: %d. Read checksum: %d", calculatedChecksum,
+				readChecksum);
+		moveToBufferStart(&_buf[fullMessageLength]);
+		return false;
+	}
+
+	return true;
+}
+
+bool Sensor::parseUDDPayload() {
+	const MessageHeader *packageHeader = reinterpret_cast<MessageHeader *>(_buf);
+
+	if (!isMessageHeaderCorrect(packageHeader)) {
+		PX4_ERR("Package header in buffer start is incorrect");
+		return false;
+	}
+
+	const uint16_t payloadSize = packageHeader->msgLen - 6;
+	const uint8_t *payload     = &_buf[6];
+
+	const uint8_t messageCount = payload[0];
+	if (messageCount == 0 || messageCount > payloadSize - 1) {
+		PX4_ERR("Invalid data package. Number of messages or messages data are incorrect");
+		movePackageHeaderToBufferStart();
+		return false;
+	}
+	// 1 byle for message count + byte list of messages types
+	const uint8_t *messageDataOffset = &payload[1 + messageCount];
+
+	for (uint8_t i = 0; i < messageCount; i++) {
+		uint8_t         messageLength = 0;
+		UDDMessageData &udd           = *(UDDMessageData *)messageDataOffset;
+		uint8_t         messageType   = payload[1 + i];
+
+		switch (messageType) {
+			case DataType::GPS_INS_TIME_MS: {
+				// this is the GPS tow timestamp in ms for when the IMU data was sampled
+				_sensorData.ins.msTow = udd.gpsTimeMs;
+				messageLength         = sizeof(udd.gpsTimeMs);
+				break;
+			}
+			case DataType::GPS_WEEK: {
+				_sensorData.gps.gpsWeek = udd.gpsWeek;
+				messageLength           = sizeof(udd.gpsWeek);
+				break;
+			}
+			case DataType::ACCEL_DATA_HR: {
+				_sensorData.accel =
+					rfuToFrd(udd.accelDataHr.toFloat()) * GRAVITY_MSS * 1.0e-6f;  // NED, in m/s^2
+				messageLength = sizeof(udd.accelDataHr);
+				break;
+			}
+			case DataType::GYRO_DATA_HR: {
+				_sensorData.gyro =
+					rfuToFrd(udd.gyroDataHr.toFloat()) * M_DEG_TO_RAD * 1.0e-5f;  // NED, in rad/s
+				messageLength = sizeof(udd.gyroDataHr);
+				break;
+			}
+			case DataType::BARO_DATA: {
+				_sensorData.pressure    = static_cast<float>(udd.baroData.pressurePa2 * 2);  // Pa
+				_sensorData.ins.baroAlt = static_cast<float>(udd.baroData.baroAlt) * 0.01f;  // m
+				messageLength           = sizeof(udd.baroData);
+				break;
+			}
+			case DataType::MAG_DATA: {
+				_sensorData.mag =
+					rfuToFrd(udd.magData.toFloat()) * 1.0e-6f;  // NED, in Gauss
+				messageLength = sizeof(udd.magData);
+				break;
+			}
+			case DataType::SENSOR_BIAS: {
+				_sensorData.ins.sensorBias = udd.sensorBias;
+				messageLength              = sizeof(udd.sensorBias);
+				break;
+			}
+			case DataType::ORIENTATION_ANGLES: {
+				_sensorData.ins.yaw   = static_cast<float>(udd.orientationAngles.yaw) * 0.01f;    // deg
+				_sensorData.ins.pitch = static_cast<float>(udd.orientationAngles.pitch) * 0.01f;  // deg
+				_sensorData.ins.roll  = static_cast<float>(udd.orientationAngles.roll) * 0.01f;   // deg
+				messageLength         = sizeof(udd.orientationAngles);
+				break;
+			}
+			case DataType::VELOCITIES: {
+				_sensorData.ins.velocity = rfuToFrd(udd.velocity.toFloat()) * 0.01f;  // NED, in m/s
+				messageLength            = sizeof(udd.velocity);
+				break;
+			}
+			case DataType::POSITION: {
+				_sensorData.ins.latitude  = static_cast<double>(udd.position.lat) * 1.0e-7;  // deg
+				_sensorData.ins.longitude = static_cast<double>(udd.position.lon) * 1.0e-7;  // deg
+				_sensorData.ins.altitude  = static_cast<float>(udd.position.alt) * 0.01f;    // m
+				messageLength             = sizeof(udd.position);
+				break;
+			}
+			case DataType::UNIT_STATUS: {
+				_sensorData.ins.unitStatus = udd.unitStatus;
+				messageLength              = sizeof(udd.unitStatus);
+				break;
+			}
+			case DataType::GNSS_EXTENDED_INFO: {
+				_sensorData.gps.fixType        = udd.gnssExtendedInfo.fixType;
+				_sensorData.gps.spoofingStatus = udd.gnssExtendedInfo.spoofingStatus;
+				messageLength                  = sizeof(udd.gnssExtendedInfo);
+				break;
+			}
+			case DataType::GNSS_POSITION: {
+				_sensorData.gps.latitude  = static_cast<double>(udd.gnssPosition.lat) * 1.0e-7;  // deg
+				_sensorData.gps.longitude = static_cast<double>(udd.gnssPosition.lon) * 1.0e-7;  // deg
+				_sensorData.gps.altitude  = static_cast<float>(udd.gnssPosition.alt) * 0.01f;    // m
+				messageLength             = sizeof(udd.gnssPosition);
+				break;
+			}
+			case DataType::GNSS_VEL_TRACK: {
+				_sensorData.gps.horSpeed =
+					static_cast<float>(udd.gnssVelTrack.horSpeed) * 0.01f;  // m/s
+				_sensorData.gps.trackOverGround =
+					static_cast<float>(udd.gnssVelTrack.trackOverGround) * 0.01f;  // deg
+				_sensorData.gps.verSpeed =
+					static_cast<float>(udd.gnssVelTrack.verSpeed) * 0.01f;  // m/s
+				messageLength = sizeof(udd.gnssVelTrack);
+				break;
+			}
+			case DataType::GNSS_POS_TIMESTAMP: {
+				_sensorData.gps.msTow = udd.gnssPosTimestamp;
+				messageLength         = sizeof(udd.gnssPosTimestamp);
+				break;
+			}
+			case DataType::GNSS_NEW_DATA: {
+				_sensorData.gps.newData = udd.gnssNewData;
+				messageLength           = sizeof(udd.gnssNewData);
+				break;
+			}
+			case DataType::GNSS_JAM_STATUS: {
+				_sensorData.gps.jamStatus = udd.gnssJamStatus;
+				messageLength             = sizeof(udd.gnssJamStatus);
+				break;
+			}
+			case DataType::DIFFERENTIAL_PRESSURE: {
+				_sensorData.differentialPressure =
+					static_cast<float>(udd.differentialPressure) * 0.01f;  // Pa
+				messageLength = sizeof(udd.differentialPressure);
+				break;
+			}
+			case DataType::TRUE_AIRSPEED: {
+				_sensorData.ins.trueAirspeed = static_cast<float>(udd.trueAirspeed) * 0.01f;  // m/s
+				messageLength                = sizeof(udd.trueAirspeed);
+				break;
+			}
+			case DataType::CALIBRATED_AIRSPEED: {
+				_sensorData.ins.calibratedAirspeed =
+					static_cast<float>(udd.calibratedAirspeed) * 0.01f;  // m/s
+				messageLength = sizeof(udd.calibratedAirspeed);
+				break;
+			}
+			case DataType::WIND_SPEED: {
+				_sensorData.ins.airspeedSf = (udd.windSpeed.toFloat()(2)) * 1.0e-3f;
+				_sensorData.ins.windSpeed  = rfuToFrd(udd.windSpeed.toFloat()) * 0.01f;  // NED, in m/s
+				_sensorData.ins.windSpeed(2) = 0.0f;
+				messageLength                = sizeof(udd.windSpeed);
+				break;
+			}
+			case DataType::AIR_DATA_STATUS: {
+				_sensorData.ins.airDataStatus = udd.airDataStatus;
+				messageLength                 = sizeof(udd.airDataStatus);
+				break;
+			}
+			case DataType::SUPPLY_VOLTAGE: {
+				_sensorData.supplyVoltage = static_cast<float>(udd.supplyVoltage) * 0.01f;  // V
+				messageLength             = sizeof(udd.supplyVoltage);
+				break;
+			}
+			case DataType::TEMPERATURE: {
+				_sensorData.temperature = static_cast<float>(udd.temperature) * 0.1f;  // degC
+				messageLength           = sizeof(udd.temperature);
+				break;
+			}
+			case DataType::UNIT_STATUS2: {
+				_sensorData.ins.unitStatus2 = udd.unitStatus2;
+				messageLength               = sizeof(udd.unitStatus2);
+				break;
+			}
+			case DataType::GNSS_DOP: {
+				_sensorData.gps.dop.gdop = udd.gnssDop.gdop;
+				_sensorData.gps.dop.pdop = udd.gnssDop.pdop;
+				_sensorData.gps.dop.hdop = udd.gnssDop.hdop;
+				_sensorData.gps.dop.vdop = udd.gnssDop.vdop;
+				_sensorData.gps.dop.tdop = udd.gnssDop.tdop;
+				messageLength            = sizeof(udd.gnssDop);
+				break;
+			}
+			case DataType::INS_SOLUTION_STATUS: {
+				_sensorData.ins.solutionStatus = udd.insSolStatus;
+				messageLength                  = sizeof(udd.insSolStatus);
+				break;
+			}
+			case DataType::INS_POS_VEL_ACCURACY: {
+				_sensorData.ins.accuracy.lat      = udd.insAccuracy.lat;
+				_sensorData.ins.accuracy.lon      = udd.insAccuracy.lon;
+				_sensorData.ins.accuracy.alt      = udd.insAccuracy.alt;
+				_sensorData.ins.accuracy.eastVel  = udd.insAccuracy.eastVel;
+				_sensorData.ins.accuracy.northVel = udd.insAccuracy.northVel;
+				_sensorData.ins.accuracy.verVel   = udd.insAccuracy.verVel;
+				messageLength                     = sizeof(udd.insAccuracy);
+				break;
+			}
+			case USED_SAT_COUNT: {
+				_sensorData.gps.usedSatCount = udd.usedSatCount;
+				messageLength                 = sizeof(udd.usedSatCount);
+				break;
+			}
+			case DataType::GNSS_VEL_LATENCY: {
+				_sensorData.gps.velLatency = udd.gnssVelLatency;
+				messageLength              = sizeof(udd.gnssVelLatency);
+				break;
+			}
+			case DataType::GNSS_SOL_STATUS: {
+				_sensorData.gps.gnssSolStatus = udd.gnssSolStatus;
+				messageLength                 = sizeof(udd.gnssSolStatus);
+				break;
+			}
+			case DataType::GNSS_POS_VEL_TYPE: {
+				_sensorData.gps.gnssPosVelType = udd.gnssPosVelType;
+				messageLength                  = sizeof(udd.gnssPosVelType);
+				break;
+			}
+			case DataType::NEW_AIDING_DATA: {
+				_sensorData.ext.newAidingData = udd.newAidingData;
+				messageLength                 = sizeof(udd.newAidingData);
+				break;
+			}
+			case DataType::NEW_AIDING_DATA2: {
+				_sensorData.ext.newAidingData2 = udd.newAidingData2;
+				messageLength                  = sizeof(udd.newAidingData2);
+				break;
+			}
+			case DataType::EXT_SPEED: {
+				_sensorData.ext.speed = udd.externalSpeed;
+				messageLength         = sizeof(udd.externalSpeed);
+				break;
+			}
+			case DataType::EXT_HOR_POS: {
+				_sensorData.ext.horPos = udd.extHorPos;
+				messageLength          = sizeof(udd.extHorPos);
+				break;
+			}
+			case DataType::EXT_ALT: {
+				_sensorData.ext.altitudeData = udd.extAlt;
+				messageLength                = sizeof(udd.extAlt);
+				break;
+			}
+			case DataType::EXT_HEADING: {
+				_sensorData.ext.headingData = udd.extHeading;
+				messageLength               = sizeof(udd.extHeading);
+				break;
+			}
+			case DataType::EXT_AMBIENT_DATA: {
+				_sensorData.ext.ambientAirData = udd.extAmbientAirData;
+				messageLength                  = sizeof(udd.extAmbientAirData);
+				break;
+			}
+			case DataType::EXT_WIND_DATA: {
+				_sensorData.ext.windData = udd.extWindData;
+				messageLength            = sizeof(udd.extWindData);
+				break;
+			}
+			case DataType::MAG_CLB_ACCURACY: {
+				_sensorData.ins.magClbAccuracy = static_cast<float>(udd.magClbAccuracy) * 0.1f;  // deg
+				messageLength                  = sizeof(udd.magClbAccuracy);
+				break;
+			}
+			default: {
+				PX4_ERR("Unknown message type: %d. Further package parsing result will be incorrect!",
+						messageType);
+				messageLength = 0;
+				break;
+			}
+		}
+
+		messageDataOffset += messageLength;
+	}
+
+	return true;
+}
+
+}  // namespace InertialLabs

src/drivers/ins/ilabs/module.yaml

@@ -0,0 +1,24 @@
+module_name: InertialLabs
+
+serial_config:
+    - command: ilabs start -d ${SERIAL_DEV}
+      port_config_param:
+        name: SENS_ILABS_CFG
+        group: Sensors
+
+parameters:
+    - group: Sensors
+      definitions:
+
+        ILABS_MODE:
+            description:
+                short: InertialLabs INS sensor mode configuration
+                long: |
+                  Configures whether the driver outputs only raw sensor output (the default),
+                  or additionally supplies INS data such as position and velocity estimates.
+            category: System
+            type: enum
+            values:
+                0: Sensors Only (default)
+                1: INS
+            default: 0