sensors/vehicle_imu: incremental step towards multi-EKF

src/modules/sensors/CMakeLists.txt

@@ -33,6 +33,7 @@
 
 add_subdirectory(vehicle_acceleration)
 add_subdirectory(vehicle_angular_velocity)
+add_subdirectory(vehicle_imu)
 
 px4_add_module(
 	MODULE modules__sensors
@@ -51,4 +52,5 @@ px4_add_module(
 		mathlib
 		vehicle_acceleration
 		vehicle_angular_velocity
+		vehicle_imu
 	)

src/modules/sensors/sensor_params.c

@@ -260,7 +260,7 @@ PARAM_DEFINE_FLOAT(IMU_GYRO_CUTOFF, 30.0f);
 /**
 * Gyro control data maximum publication rate
 *
-* This is the maximum rate the gyro control data (sensor_gyro_control) will be allowed to publish at.
+* This is the maximum rate the gyro control data (sensor_gyro) will be allowed to publish at.
 * Set to 0 to disable and publish at the native sensor sample rate.
 *
 * @min 0

src/modules/sensors/sensors.cpp

@@ -72,6 +72,7 @@
 #include "voted_sensors_update.h"
 #include "vehicle_acceleration/VehicleAcceleration.hpp"
 #include "vehicle_angular_velocity/VehicleAngularVelocity.hpp"
+#include "vehicle_imu/VehicleIMU.hpp"
 
 using namespace sensors;
 using namespace time_literals;
@@ -141,6 +142,9 @@ private:
 	VehicleAcceleration	_vehicle_acceleration;
 	VehicleAngularVelocity	_vehicle_angular_velocity;
 
+	static constexpr int MAX_SENSOR_COUNT = 3;
+	VehicleIMU      *_vehicle_imu_list[MAX_SENSOR_COUNT] {};
+
 
 	/**
 	 * Update our local parameter cache.
@@ -173,6 +177,8 @@ private:
 	 */
 	void		adc_poll();
 
+	void		InitializeVehicleIMU();
+
 };
 
 Sensors::Sensors(bool hil_enabled) :
@@ -188,12 +194,20 @@ Sensors::Sensors(bool hil_enabled) :
 
 	_vehicle_acceleration.Start();
 	_vehicle_angular_velocity.Start();
+
+	InitializeVehicleIMU();
 }
 
 Sensors::~Sensors()
 {
 	_vehicle_acceleration.Stop();
 	_vehicle_angular_velocity.Stop();
+
+	for (auto &i : _vehicle_imu_list) {
+		if (i != nullptr) {
+			i->Stop();
+		}
+	}
 }
 
 int
@@ -376,6 +390,37 @@ Sensors::adc_poll()
 #endif /* ADC_AIRSPEED_VOLTAGE_CHANNEL */
 }
 
+void Sensors::InitializeVehicleIMU()
+{
+	// create a VehicleIMU instance for each accel/gyro pair
+	for (uint8_t i = 0; i < MAX_SENSOR_COUNT; i++) {
+		if (_vehicle_imu_list[i] == nullptr) {
+
+			uORB::Subscription accel_sub{ORB_ID(sensor_accel_integrated), i};
+			sensor_accel_integrated_s accel{};
+			accel_sub.copy(&accel);
+
+			uORB::Subscription gyro_sub{ORB_ID(sensor_gyro_integrated), i};
+			sensor_gyro_integrated_s gyro{};
+			gyro_sub.copy(&gyro);
+
+			if (accel.device_id > 0 && gyro.device_id > 0) {
+				VehicleIMU *imu = new VehicleIMU(i, i);
+
+				if (imu != nullptr) {
+					// Start VehicleIMU instance and store
+					if (imu->Start()) {
+						_vehicle_imu_list[i] = imu;
+
+					} else {
+						delete imu;
+					}
+				}
+			}
+		}
+	}
+}
+
 void
 Sensors::run()
 {
@@ -482,6 +527,7 @@ Sensors::run()
 		 */
 		if (!_armed && hrt_elapsed_time(&last_config_update) > 500_ms) {
 			_voted_sensors_update.initializeSensors();
+			InitializeVehicleIMU();
 			last_config_update = hrt_absolute_time();
 
 		} else {
@@ -524,6 +570,12 @@ int Sensors::print_status()
 	_vehicle_acceleration.PrintStatus();
 	_vehicle_angular_velocity.PrintStatus();
 
+	for (auto &i : _vehicle_imu_list) {
+		if (i != nullptr) {
+			i->PrintStatus();
+		}
+	}
+
 	return 0;
 }
 

src/modules/sensors/vehicle_acceleration/VehicleAcceleration.hpp

@@ -58,14 +58,14 @@ public:
 	VehicleAcceleration();
 	~VehicleAcceleration() override;
 
-	void	Run() override;
+	bool Start();
+	void Stop();
 
-	bool	Start();
-	void	Stop();
-
-	void	PrintStatus();
+	void PrintStatus();
 
 private:
+	void Run() override;
+
 	void ParametersUpdate(bool force = false);
 	void SensorBiasUpdate(bool force = false);
 	void SensorCorrectionsUpdate(bool force = false);
@@ -81,26 +81,26 @@ private:
 		(ParamFloat<px4::params::SENS_BOARD_Z_OFF>) _param_sens_board_z_off
 	)
 
-	uORB::Publication<vehicle_acceleration_s>	_vehicle_acceleration_pub{ORB_ID(vehicle_acceleration)};
+	uORB::Publication<vehicle_acceleration_s> _vehicle_acceleration_pub{ORB_ID(vehicle_acceleration)};
 
-	uORB::Subscription			_params_sub{ORB_ID(parameter_update)};			/**< parameter updates subscription */
-	uORB::Subscription			_sensor_bias_sub{ORB_ID(sensor_bias)};			/**< sensor in-run bias correction subscription */
-	uORB::Subscription			_sensor_correction_sub{ORB_ID(sensor_correction)};	/**< sensor thermal correction subscription */
+	uORB::Subscription _params_sub{ORB_ID(parameter_update)};
+	uORB::Subscription _sensor_bias_sub{ORB_ID(sensor_bias)};
+	uORB::Subscription _sensor_correction_sub{ORB_ID(sensor_correction)};
 
-	uORB::SubscriptionCallbackWorkItem	_sensor_selection_sub{this, ORB_ID(sensor_selection)};	/**< selected primary sensor subscription */
-	uORB::SubscriptionCallbackWorkItem	_sensor_sub[MAX_SENSOR_COUNT] {				/**< sensor data subscription */
+	uORB::SubscriptionCallbackWorkItem _sensor_selection_sub{this, ORB_ID(sensor_selection)};
+	uORB::SubscriptionCallbackWorkItem _sensor_sub[MAX_SENSOR_COUNT] {
 		{this, ORB_ID(sensor_accel), 0},
 		{this, ORB_ID(sensor_accel), 1},
 		{this, ORB_ID(sensor_accel), 2}
 	};
 
-	matrix::Dcmf				_board_rotation;				/**< rotation matrix for the orientation that the board is mounted */
+	matrix::Dcmf _board_rotation;
 
-	matrix::Vector3f			_bias{0.f, 0.f, 0.f};
-	matrix::Vector3f			_offset{0.f, 0.f, 0.f};
-	matrix::Vector3f			_scale{1.f, 1.f, 1.f};
+	matrix::Vector3f _bias{0.f, 0.f, 0.f};
+	matrix::Vector3f _offset{0.f, 0.f, 0.f};
+	matrix::Vector3f _scale{1.f, 1.f, 1.f};
 
-	uint32_t				_selected_sensor_device_id{0};
-	uint8_t					_selected_sensor_sub_index{0};
-	int8_t					_corrections_selected_instance{-1};
+	uint32_t _selected_sensor_device_id{0};
+	uint8_t _selected_sensor_sub_index{0};
+	int8_t _corrections_selected_instance{-1};
 };

src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp

@@ -58,14 +58,14 @@ public:
 	VehicleAngularVelocity();
 	~VehicleAngularVelocity() override;
 
-	void	Run() override;
+	bool Start();
+	void Stop();
 
-	bool	Start();
-	void	Stop();
-
-	void	PrintStatus();
+	void PrintStatus();
 
 private:
+	void Run() override;
+
 	void ParametersUpdate(bool force = false);
 	void SensorBiasUpdate(bool force = false);
 	void SensorCorrectionsUpdate(bool force = false);
@@ -81,26 +81,26 @@ private:
 		(ParamFloat<px4::params::SENS_BOARD_Z_OFF>) _param_sens_board_z_off
 	)
 
-	uORB::Publication<vehicle_angular_velocity_s>	_vehicle_angular_velocity_pub{ORB_ID(vehicle_angular_velocity)};
+	uORB::Publication<vehicle_angular_velocity_s> _vehicle_angular_velocity_pub{ORB_ID(vehicle_angular_velocity)};
 
-	uORB::Subscription			_params_sub{ORB_ID(parameter_update)};			/**< parameter updates subscription */
-	uORB::Subscription			_sensor_bias_sub{ORB_ID(sensor_bias)};			/**< sensor in-run bias correction subscription */
-	uORB::Subscription			_sensor_correction_sub{ORB_ID(sensor_correction)};	/**< sensor thermal correction subscription */
+	uORB::Subscription _params_sub{ORB_ID(parameter_update)};
+	uORB::Subscription _sensor_bias_sub{ORB_ID(sensor_bias)};
+	uORB::Subscription _sensor_correction_sub{ORB_ID(sensor_correction)};
 
-	uORB::SubscriptionCallbackWorkItem	_sensor_selection_sub{this, ORB_ID(sensor_selection)};	/**< selected primary sensor subscription */
-	uORB::SubscriptionCallbackWorkItem	_sensor_sub[MAX_SENSOR_COUNT] {				/**< sensor data subscription */
+	uORB::SubscriptionCallbackWorkItem _sensor_selection_sub{this, ORB_ID(sensor_selection)};
+	uORB::SubscriptionCallbackWorkItem _sensor_sub[MAX_SENSOR_COUNT] {
 		{this, ORB_ID(sensor_gyro), 0},
 		{this, ORB_ID(sensor_gyro), 1},
 		{this, ORB_ID(sensor_gyro), 2}
 	};
 
-	matrix::Dcmf				_board_rotation;				/**< rotation matrix for the orientation that the board is mounted */
+	matrix::Dcmf _board_rotation;
 
-	matrix::Vector3f			_bias{0.f, 0.f, 0.f};
-	matrix::Vector3f			_offset{0.f, 0.f, 0.f};
-	matrix::Vector3f			_scale{1.f, 1.f, 1.f};
+	matrix::Vector3f _bias{0.f, 0.f, 0.f};
+	matrix::Vector3f _offset{0.f, 0.f, 0.f};
+	matrix::Vector3f _scale{1.f, 1.f, 1.f};
 
-	uint32_t				_selected_sensor_device_id{0};
-	uint8_t					_selected_sensor_sub_index{0};
-	int8_t					_corrections_selected_instance{-1};
+	uint32_t _selected_sensor_device_id{0};
+	uint8_t _selected_sensor_sub_index{0};
+	int8_t _corrections_selected_instance{-1};
 };

src/modules/sensors/vehicle_imu/CMakeLists.txt

@@ -0,0 +1,38 @@
+############################################################################
+#
+#   Copyright (c) 2020 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.
+#
+############################################################################
+
+px4_add_library(vehicle_imu
+	VehicleIMU.cpp
+	VehicleIMU.hpp
+)
+target_link_libraries(vehicle_imu PRIVATE px4_work_queue)

src/modules/sensors/vehicle_imu/VehicleIMU.cpp

@@ -0,0 +1,228 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020 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 "VehicleIMU.hpp"
+
+#include <px4_platform_common/log.h>
+
+using namespace matrix;
+using namespace time_literals;
+using math::radians;
+
+VehicleIMU::VehicleIMU(uint8_t accel_index, uint8_t gyro_index) :
+	ModuleParams(nullptr),
+	WorkItem(MODULE_NAME, px4::wq_configurations::att_pos_ctrl),
+	_sensor_accel_integrated_sub(this, ORB_ID(sensor_accel_integrated), accel_index),
+	_sensor_gyro_integrated_sub(this, ORB_ID(sensor_gyro_integrated), gyro_index)
+{
+}
+
+VehicleIMU::~VehicleIMU()
+{
+	Stop();
+	delete _name;
+}
+
+bool VehicleIMU::Start()
+{
+	// force initial updates
+	ParametersUpdate(true);
+
+	return _sensor_accel_integrated_sub.registerCallback() && _sensor_gyro_integrated_sub.registerCallback();
+}
+
+void VehicleIMU::Stop()
+{
+	Deinit();
+
+	// clear all registered callbacks
+	_sensor_accel_integrated_sub.unregisterCallback();
+	_sensor_gyro_integrated_sub.unregisterCallback();
+}
+
+void VehicleIMU::SensorCorrectionsUpdate(bool force)
+{
+	// check if the selected sensor has updated
+	if (_sensor_correction_sub.updated() || force) {
+		sensor_correction_s corrections{};
+
+		if (_sensor_correction_sub.copy(&corrections)) {
+
+			// accel
+			if (_accel_device_id > 0) {
+				if ((_corrections_selected_accel_instance < 0) || force) {
+					_corrections_selected_accel_instance = -1;
+
+					// find sensor_corrections index
+					for (int i = 0; i < MAX_SENSOR_COUNT; i++) {
+						if (corrections.accel_device_ids[i] == _accel_device_id) {
+							_corrections_selected_accel_instance = i;
+						}
+					}
+				}
+
+				switch (_corrections_selected_accel_instance) {
+				case 0:
+					_accel_offset = Vector3f{corrections.accel_offset_0};
+					_accel_scale = Vector3f{corrections.accel_scale_0};
+					break;
+				case 1:
+					_accel_offset = Vector3f{corrections.accel_offset_1};
+					_accel_scale = Vector3f{corrections.accel_scale_1};
+					break;
+				case 2:
+					_accel_offset = Vector3f{corrections.accel_offset_2};
+					_accel_scale = Vector3f{corrections.accel_scale_2};
+					break;
+				default:
+					_accel_offset = Vector3f{0.f, 0.f, 0.f};
+					_accel_scale = Vector3f{1.f, 1.f, 1.f};
+				}
+			}
+
+			// gyro
+			if (_gyro_device_id > 0) {
+				if ((_corrections_selected_gyro_instance < 0) || force) {
+					_corrections_selected_gyro_instance = -1;
+
+					// find sensor_corrections index
+					for (int i = 0; i < MAX_SENSOR_COUNT; i++) {
+						if (corrections.gyro_device_ids[i] == _gyro_device_id) {
+							_corrections_selected_gyro_instance = i;
+						}
+					}
+				}
+
+				switch (_corrections_selected_gyro_instance) {
+				case 0:
+					_gyro_offset = Vector3f{corrections.gyro_offset_0};
+					_gyro_scale = Vector3f{corrections.gyro_scale_0};
+					break;
+				case 1:
+					_gyro_offset = Vector3f{corrections.gyro_offset_1};
+					_gyro_scale = Vector3f{corrections.gyro_scale_1};
+					break;
+				case 2:
+					_gyro_offset = Vector3f{corrections.gyro_offset_2};
+					_gyro_scale = Vector3f{corrections.gyro_scale_2};
+					break;
+				default:
+					_gyro_offset = Vector3f{0.f, 0.f, 0.f};
+					_gyro_scale = Vector3f{1.f, 1.f, 1.f};
+				}
+			}
+		}
+	}
+}
+
+void VehicleIMU::ParametersUpdate(bool force)
+{
+	// Check if parameters have changed
+	if (_params_sub.updated() || force) {
+		// clear update
+		parameter_update_s param_update;
+		_params_sub.copy(&param_update);
+
+		updateParams();
+
+		// get transformation matrix from sensor/board to body frame
+		const Dcmf board_rotation = get_rot_matrix((enum Rotation)_param_sens_board_rot.get());
+
+		// fine tune the rotation
+		const Dcmf board_rotation_offset(Eulerf(
+				radians(_param_sens_board_x_off.get()),
+				radians(_param_sens_board_y_off.get()),
+				radians(_param_sens_board_z_off.get())));
+
+		_board_rotation = board_rotation_offset * board_rotation;
+	}
+}
+
+void VehicleIMU::Run()
+{
+	if (_sensor_accel_integrated_sub.updated() && _sensor_gyro_integrated_sub.updated()) {
+		sensor_accel_integrated_s accel;
+		_sensor_accel_integrated_sub.copy(&accel);
+		_accel_device_id = accel.device_id;
+
+		sensor_gyro_integrated_s gyro;
+		_sensor_gyro_integrated_sub.copy(&gyro);
+		_gyro_device_id = gyro.device_id;
+
+		SensorCorrectionsUpdate();
+		ParametersUpdate();
+
+		// delta angle: apply offsets, scale, and board rotation
+		Vector3f delta_angle{gyro.delta_angle};
+		const float gyro_dt = 1.e-6f * gyro.dt;
+		// apply offsets
+		delta_angle = delta_angle - (_gyro_offset * gyro_dt);
+		// apply scale
+		delta_angle = delta_angle.emult(_gyro_scale);
+		// apply board rotation
+		delta_angle = _board_rotation * delta_angle;
+
+		// delta velocity: apply offsets, scale, and board rotation
+		Vector3f delta_velocity{accel.delta_velocity};
+		const float accel_dt = 1.e-6f * accel.dt;
+		// apply offsets
+		delta_velocity = delta_velocity - (_accel_offset * accel_dt);
+		// apply scale
+		delta_velocity = delta_velocity.emult(_accel_scale);
+		// apply board rotation
+		delta_velocity = _board_rotation * delta_velocity;
+
+
+		// publich vehicle_imu
+		vehicle_imu_s imu;
+
+		imu.timestamp_sample = accel.timestamp_sample;
+		imu.accel_device_id = accel.device_id;
+		imu.gyro_device_id = gyro.device_id;
+
+		delta_angle.copyTo(imu.delta_angle);
+		delta_velocity.copyTo(imu.delta_velocity);
+
+		imu.dt = accel.dt;
+		imu.integrated_samples = accel.samples;
+		imu.clip_count = accel.clip_count;
+		imu.timestamp = hrt_absolute_time();
+
+		_vehicle_imu_pub.publish(imu);
+	}
+}
+
+void VehicleIMU::PrintStatus()
+{
+	PX4_INFO("selected IMU: accel: %d gyro: %d", _accel_device_id, _gyro_device_id);
+}

src/modules/sensors/vehicle_imu/VehicleIMU.hpp

@@ -0,0 +1,104 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020 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 <containers/List.hpp>
+#include <lib/conversion/rotation.h>
+#include <lib/mathlib/math/Limits.hpp>
+#include <lib/matrix/matrix/math.hpp>
+#include <px4_platform_common/px4_config.h>
+#include <px4_platform_common/log.h>
+#include <px4_platform_common/module_params.h>
+#include <px4_platform_common/px4_work_queue/WorkItem.hpp>
+#include <uORB/PublicationMulti.hpp>
+#include <uORB/Subscription.hpp>
+#include <uORB/SubscriptionCallback.hpp>
+#include <uORB/topics/parameter_update.h>
+#include <uORB/topics/sensor_accel_integrated.h>
+#include <uORB/topics/sensor_correction.h>
+#include <uORB/topics/sensor_gyro_integrated.h>
+#include <uORB/topics/vehicle_imu.h>
+
+class VehicleIMU : public ModuleParams, public px4::WorkItem
+{
+public:
+	VehicleIMU() = delete;
+	VehicleIMU(uint8_t accel_index = 0, uint8_t gyro_index = 0);
+
+	~VehicleIMU() override;
+
+	bool Start();
+	void Stop();
+
+	void PrintStatus();
+
+private:
+	void Run() override;
+
+	void ParametersUpdate(bool force = false);
+	void SensorCorrectionsUpdate(bool force = false);
+
+	static constexpr int MAX_SENSOR_COUNT = 3;
+
+	DEFINE_PARAMETERS(
+		(ParamInt<px4::params::SENS_BOARD_ROT>) _param_sens_board_rot,
+
+		(ParamFloat<px4::params::SENS_BOARD_X_OFF>) _param_sens_board_x_off,
+		(ParamFloat<px4::params::SENS_BOARD_Y_OFF>) _param_sens_board_y_off,
+		(ParamFloat<px4::params::SENS_BOARD_Z_OFF>) _param_sens_board_z_off
+	)
+
+	uORB::PublicationMulti<vehicle_imu_s> _vehicle_imu_pub{ORB_ID(vehicle_imu)};
+
+	uORB::Subscription _params_sub{ORB_ID(parameter_update)};
+	uORB::Subscription _sensor_correction_sub{ORB_ID(sensor_correction)};
+
+	uORB::SubscriptionCallbackWorkItem _sensor_accel_integrated_sub;
+	uORB::SubscriptionCallbackWorkItem _sensor_gyro_integrated_sub;
+
+	matrix::Dcmf _board_rotation;
+
+	matrix::Vector3f _accel_offset{0.f, 0.f, 0.f};
+	matrix::Vector3f _gyro_offset{0.f, 0.f, 0.f};
+	matrix::Vector3f _accel_scale{1.f, 1.f, 1.f};
+	matrix::Vector3f _gyro_scale{1.f, 1.f, 1.f};
+
+	char *_name{nullptr};
+
+	int8_t _corrections_selected_accel_instance{-1};
+	int8_t _corrections_selected_gyro_instance{-1};
+
+	uint32_t _accel_device_id{0};
+	uint32_t _gyro_device_id{0};
+};