sensors/vehicle_imu: incremental step towards multi-EKF

msg/CMakeLists.txt

@@ -144,6 +144,7 @@ set(msg_files
 	vehicle_control_mode.msg
 	vehicle_global_position.msg
 	vehicle_gps_position.msg
+	vehicle_imu.msg
 	vehicle_land_detected.msg
 	vehicle_local_position.msg
 	vehicle_local_position_setpoint.msg

msg/tools/uorb_rtps_message_ids.yaml

@@ -275,6 +275,8 @@ rtps:
     id: 122
   - msg: sensor_gyro_integrated
     id: 123
+  - msg: vehicle_imu
+    id: 124
   ########## multi topics: begin ##########
   - msg: actuator_controls_0
     id: 150

msg/vehicle_imu.msg

@@ -0,0 +1,15 @@
+# IMU readings in SI-unit form.
+
+uint64 timestamp          # time since system start (microseconds)
+uint64 timestamp_sample
+
+uint32 accel_device_id          # Accelerometer unique device ID for the sensor that does not change between power cycles
+uint32 gyro_device_id           # Gyroscope unique device ID for the sensor that does not change between power cycles
+
+float32[3] delta_angle          # delta angle in the NED board axis in rad/s over the integration time frame (dt)
+float32[3] delta_velocity       # delta velocity in the NED board axis in m/s over the integration time frame (dt)
+
+uint16 dt                       # integration period in us
+
+uint8 integrated_samples        # number of samples integrated
+uint8 clip_count                # total clip count per integration period on any axis

src/modules/ekf2/ekf2_main.cpp

@@ -71,6 +71,7 @@
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/vehicle_gps_position.h>
+#include <uORB/topics/vehicle_imu.h>
 #include <uORB/topics/vehicle_land_detected.h>
 #include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/vehicle_magnetometer.h>
@@ -92,7 +93,7 @@
 using math::constrain;
 using namespace time_literals;
 
-class Ekf2 final : public ModuleBase<Ekf2>, public ModuleParams, public px4::WorkItem
+class Ekf2 final : public ModuleBase<Ekf2>, public ModuleParams, public px4::ScheduledWorkItem
 {
 public:
 	explicit Ekf2(bool replay_mode = false);
@@ -129,7 +130,7 @@ private:
 	template<typename Param>
 	bool update_mag_decl(Param &mag_decl_param);
 
-	bool publish_attitude(const sensor_combined_s &sensors, const hrt_abstime &now);
+	bool publish_attitude(const hrt_abstime &now);
 	bool publish_wind_estimate(const hrt_abstime &timestamp);
 
 	/*
@@ -241,7 +242,15 @@ private:
 	uORB::Subscription _status_sub{ORB_ID(vehicle_status)};
 	uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
 
-	uORB::SubscriptionCallbackWorkItem _sensors_sub{this, ORB_ID(sensor_combined)};
+	uORB::SubscriptionCallbackWorkItem _sensor_combined_sub{this, ORB_ID(sensor_combined)};
+	static constexpr int MAX_SENSOR_COUNT = 3;
+	uORB::SubscriptionCallbackWorkItem _vehicle_imu_subs[MAX_SENSOR_COUNT] {
+		{this, ORB_ID(vehicle_imu), 0},
+		{this, ORB_ID(vehicle_imu), 1},
+		{this, ORB_ID(vehicle_imu), 2}
+	};
+	int _imu_sub_index{-1};
+	bool _callback_registered{false};
 
 	// because we can have several distance sensor instances with different orientations
 	uORB::Subscription _range_finder_subs[ORB_MULTI_MAX_INSTANCES] {{ORB_ID(distance_sensor), 0}, {ORB_ID(distance_sensor), 1}, {ORB_ID(distance_sensor), 2}, {ORB_ID(distance_sensor), 3}};
@@ -418,6 +427,8 @@ private:
 		(ParamExtFloat<px4::params::EKF2_OF_GATE>)
 		_param_ekf2_of_gate,	///< optical flow fusion innovation consistency gate size (STD)
 
+		(ParamInt<px4::params::EKF2_IMU_ID>) _param_ekf2_imu_id,
+
 		// sensor positions in body frame
 		(ParamExtFloat<px4::params::EKF2_IMU_POS_X>) _param_ekf2_imu_pos_x,		///< X position of IMU in body frame (m)
 		(ParamExtFloat<px4::params::EKF2_IMU_POS_Y>) _param_ekf2_imu_pos_y,		///< Y position of IMU in body frame (m)
@@ -521,7 +532,7 @@ private:
 
 Ekf2::Ekf2(bool replay_mode):
 	ModuleParams(nullptr),
-	WorkItem(MODULE_NAME, px4::wq_configurations::att_pos_ctrl),
+	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::att_pos_ctrl),
 	_replay_mode(replay_mode),
 	_ekf_update_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": update")),
 	_params(_ekf.getParamHandle()),
@@ -638,14 +649,39 @@ Ekf2::~Ekf2()
 	perf_free(_ekf_update_perf);
 }
 
-bool
+bool Ekf2::init()
-Ekf2::init()
 {
-	if (!_sensors_sub.registerCallback()) {
+	const uint32_t device_id = _param_ekf2_imu_id.get();
-		PX4_ERR("sensor combined callback registration failed!");
+
-		return false;
+	// if EKF2_IMU_ID is non-zero we use the corresponding IMU, otherwise the voted primary (sensor_combined)
+	if (device_id != 0) {
+		for (int i = 0; i < MAX_SENSOR_COUNT; i++) {
+			vehicle_imu_s imu{};
+
+			if (_vehicle_imu_subs[i].copy(&imu)) {
+				if ((imu.accel_device_id > 0) && (imu.accel_device_id == device_id)) {
+					if (_vehicle_imu_subs[i].registerCallback()) {
+						PX4_INFO("subscribed to vehicle_imu:%d (%d)", i, device_id);
+						_imu_sub_index = i;
+						_callback_registered = true;
+						return true;
+					}
+				}
+			}
+		}
+
+	} else {
+		_imu_sub_index = -1;
+
+		if (_sensor_combined_sub.registerCallback()) {
+			_callback_registered = true;
+			return true;
+		}
 	}
 
+	PX4_WARN("failed to register callback, retrying in 1 second");
+	ScheduleDelayed(1_s); // retry in 1 second
+
 	return true;
 }
 
@@ -698,14 +734,56 @@ bool Ekf2::update_mag_decl(Param &mag_decl_param)
 void Ekf2::Run()
 {
 	if (should_exit()) {
-		_sensors_sub.unregisterCallback();
+		_sensor_combined_sub.unregisterCallback();
+
+		for (auto &i : _vehicle_imu_subs) {
+			i.unregisterCallback();
+		}
+
 		exit_and_cleanup();
 		return;
 	}
 
-	sensor_combined_s sensors;
+	if (!_callback_registered) {
+		init();
+		return;
+	}
 
-	if (_sensors_sub.update(&sensors)) {
+	bool updated = false;
+	imuSample imu_sample_new;
+
+	hrt_abstime imu_dt = 0; // for tracking time slip later
+	sensor_bias_s bias{};
+
+	if (_imu_sub_index >= 0) {
+		vehicle_imu_s imu;
+		updated = _vehicle_imu_subs[_imu_sub_index].update(&imu);
+
+		imu_sample_new.time_us = imu.timestamp_sample;
+		imu_sample_new.delta_ang_dt = imu.dt * 1.e-6f;
+		imu_sample_new.delta_ang = Vector3f{imu.delta_angle};
+		imu_sample_new.delta_vel_dt = imu.dt * 1.e-6f;
+		imu_sample_new.delta_vel = Vector3f{imu.delta_velocity};
+
+		imu_dt = imu.dt;
+
+		bias.accel_device_id = imu.accel_device_id;
+		bias.gyro_device_id = imu.gyro_device_id;
+
+	} else {
+		sensor_combined_s sensor_combined;
+		updated = _sensor_combined_sub.update(&sensor_combined);
+
+		imu_sample_new.time_us = sensor_combined.timestamp;
+		imu_sample_new.delta_ang_dt = sensor_combined.gyro_integral_dt * 1.e-6f;
+		imu_sample_new.delta_ang = Vector3f{sensor_combined.gyro_rad} * imu_sample_new.delta_ang_dt;
+		imu_sample_new.delta_vel_dt = sensor_combined.accelerometer_integral_dt * 1.e-6f;
+		imu_sample_new.delta_vel = Vector3f{sensor_combined.accelerometer_m_s2} * imu_sample_new.delta_vel_dt;
+
+		imu_dt = sensor_combined.gyro_integral_dt;
+	}
+
+	if (updated) {
 
 		// check for parameter updates
 		if (_parameter_update_sub.updated()) {
@@ -717,9 +795,11 @@ void Ekf2::Run()
 			updateParams();
 		}
 
+		const hrt_abstime now = imu_sample_new.time_us;
+
 		// ekf2_timestamps (using 0.1 ms relative timestamps)
 		ekf2_timestamps_s ekf2_timestamps{};
-		ekf2_timestamps.timestamp = sensors.timestamp;
+		ekf2_timestamps.timestamp = now;
 
 		ekf2_timestamps.airspeed_timestamp_rel = ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID;
 		ekf2_timestamps.distance_sensor_timestamp_rel = ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID;
@@ -747,14 +827,17 @@ void Ekf2::Run()
 
 			if (_sensor_selection_sub.copy(&_sensor_selection)) {
 				if ((sensor_selection_prev.timestamp > 0) && (_sensor_selection.timestamp > sensor_selection_prev.timestamp)) {
-					if (_sensor_selection.accel_device_id != sensor_selection_prev.accel_device_id) {
-						PX4_WARN("accel id changed, resetting IMU bias");
-						_imu_bias_reset_request = true;
-					}
 
-					if (_sensor_selection.gyro_device_id != sensor_selection_prev.gyro_device_id) {
+					if (_imu_sub_index < 0) {
-						PX4_WARN("gyro id changed, resetting IMU bias");
+						if (_sensor_selection.accel_device_id != sensor_selection_prev.accel_device_id) {
-						_imu_bias_reset_request = true;
+							PX4_WARN("accel id changed, resetting IMU bias");
+							_imu_bias_reset_request = true;
+						}
+
+						if (_sensor_selection.gyro_device_id != sensor_selection_prev.gyro_device_id) {
+							PX4_WARN("gyro id changed, resetting IMU bias");
+							_imu_bias_reset_request = true;
+						}
 					}
 				}
 			}
@@ -765,20 +848,11 @@ void Ekf2::Run()
 			_imu_bias_reset_request = !_ekf.reset_imu_bias();
 		}
 
-		const hrt_abstime now = sensors.timestamp;
-
 		// push imu data into estimator
-		imuSample imu_sample_new;
-		imu_sample_new.time_us = now;
-		imu_sample_new.delta_ang_dt = sensors.gyro_integral_dt * 1.e-6f;
-		imu_sample_new.delta_ang = Vector3f{sensors.gyro_rad} * imu_sample_new.delta_ang_dt;
-		imu_sample_new.delta_vel_dt = sensors.accelerometer_integral_dt * 1.e-6f;
-		imu_sample_new.delta_vel = Vector3f{sensors.accelerometer_m_s2} * imu_sample_new.delta_vel_dt;
-
 		_ekf.setIMUData(imu_sample_new);
 
 		// publish attitude immediately (uses quaternion from output predictor)
-		publish_attitude(sensors, now);
+		publish_attitude(now);
 
 		// read mag data
 		if (_magnetometer_sub.updated()) {
@@ -1106,7 +1180,7 @@ void Ekf2::Run()
 
 		// run the EKF update and output
 		perf_begin(_ekf_update_perf);
-		const bool updated = _ekf.update();
+		const bool ekf_updated = _ekf.update();
 		perf_end(_ekf_update_perf);
 
 		// integrate time to monitor time slippage
@@ -1115,11 +1189,11 @@ void Ekf2::Run()
 			_last_time_slip_us = 0;
 
 		} else if (_start_time_us > 0) {
-			_integrated_time_us += sensors.gyro_integral_dt;
+			_integrated_time_us += imu_dt;
 			_last_time_slip_us = (now - _start_time_us) - _integrated_time_us;
 		}
 
-		if (updated) {
+		if (ekf_updated) {
 
 			filter_control_status_u control_status;
 			_ekf.get_control_mode(&control_status.value);
@@ -1205,9 +1279,10 @@ void Ekf2::Run()
 				// Vehicle odometry angular rates
 				float gyro_bias[3];
 				_ekf.get_gyro_bias(gyro_bias);
-				odom.rollspeed = sensors.gyro_rad[0] - gyro_bias[0];
+				const Vector3f rates{imu_sample_new.delta_ang * imu_sample_new.delta_ang_dt};
-				odom.pitchspeed = sensors.gyro_rad[1] - gyro_bias[1];
+				odom.rollspeed = rates(0) - gyro_bias[0];
-				odom.yawspeed = sensors.gyro_rad[2] - gyro_bias[2];
+				odom.pitchspeed = rates(1) - gyro_bias[1];
+				odom.yawspeed = rates(2) - gyro_bias[2];
 
 				lpos.dist_bottom_valid = _ekf.get_terrain_valid();
 
@@ -1382,12 +1457,14 @@ void Ekf2::Run()
 
 			{
 				// publish all corrected sensor readings and bias estimates after mag calibration is updated above
-				sensor_bias_s bias{};
-
 				bias.timestamp = now;
 
-				bias.gyro_device_id = _sensor_selection.gyro_device_id;
+				// take device ids from sensor_selection_s if not using specific vehicle_imu_s
-				bias.accel_device_id = _sensor_selection.accel_device_id;
+				if (_imu_sub_index < 0) {
+					bias.gyro_device_id = _sensor_selection.gyro_device_id;
+					bias.accel_device_id = _sensor_selection.accel_device_id;
+				}
+
 				bias.mag_device_id = _sensor_selection.mag_device_id;
 
 				// In-run bias estimates
@@ -1593,7 +1670,7 @@ void Ekf2::Run()
 
 				// calculate noise filtered velocity innovations which are used for pre-flight checking
 				if (_vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_STANDBY) {
-					float dt_seconds = sensors.accelerometer_integral_dt * 1e-6f;
+					float dt_seconds = imu_sample_new.delta_ang_dt;
 					runPreFlightChecks(dt_seconds, control_status, _vehicle_status, innovations);
 
 				} else {
@@ -1671,7 +1748,7 @@ int Ekf2::getRangeSubIndex()
 	return -1;
 }
 
-bool Ekf2::publish_attitude(const sensor_combined_s &sensors, const hrt_abstime &now)
+bool Ekf2::publish_attitude(const hrt_abstime &now)
 {
 	if (_ekf.attitude_valid()) {
 		// generate vehicle attitude quaternion data

src/modules/ekf2/ekf2_params.c

@@ -805,6 +805,19 @@ PARAM_DEFINE_FLOAT(EKF2_TERR_NOISE, 5.0f);
  */
 PARAM_DEFINE_FLOAT(EKF2_TERR_GRAD, 0.5f);
 
+/**
+ * Device id of IMU
+ *
+ * Set to 0 to use system selected (sensor_combined) IMU,
+ * otherwise set to the device id of the desired IMU (vehicle_imu).
+ *
+ * @group EKF2
+ * @value 0 System Primary
+ * @category Developer
+ *
+ */
+PARAM_DEFINE_INT32(EKF2_IMU_ID, 0);
+
 /**
  * X position of IMU in body frame
  *

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 PrintStatus();
-	void	Stop();
-
-	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 _params_sub{ORB_ID(parameter_update)};
-	uORB::Subscription			_sensor_bias_sub{ORB_ID(sensor_bias)};			/**< sensor in-run bias correction subscription */
+	uORB::Subscription _sensor_bias_sub{ORB_ID(sensor_bias)};
-	uORB::Subscription			_sensor_correction_sub{ORB_ID(sensor_correction)};	/**< sensor thermal correction subscription */
+	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_selection_sub{this, ORB_ID(sensor_selection)};
-	uORB::SubscriptionCallbackWorkItem	_sensor_sub[MAX_SENSOR_COUNT] {				/**< sensor data subscription */
+	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 _bias{0.f, 0.f, 0.f};
-	matrix::Vector3f			_offset{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 _scale{1.f, 1.f, 1.f};
 
-	uint32_t				_selected_sensor_device_id{0};
+	uint32_t _selected_sensor_device_id{0};
-	uint8_t					_selected_sensor_sub_index{0};
+	uint8_t _selected_sensor_sub_index{0};
-	int8_t					_corrections_selected_instance{-1};
+	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 PrintStatus();
-	void	Stop();
-
-	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 _params_sub{ORB_ID(parameter_update)};
-	uORB::Subscription			_sensor_bias_sub{ORB_ID(sensor_bias)};			/**< sensor in-run bias correction subscription */
+	uORB::Subscription _sensor_bias_sub{ORB_ID(sensor_bias)};
-	uORB::Subscription			_sensor_correction_sub{ORB_ID(sensor_correction)};	/**< sensor thermal correction subscription */
+	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_selection_sub{this, ORB_ID(sensor_selection)};
-	uORB::SubscriptionCallbackWorkItem	_sensor_sub[MAX_SENSOR_COUNT] {				/**< sensor data subscription */
+	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 _bias{0.f, 0.f, 0.f};
-	matrix::Vector3f			_offset{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 _scale{1.f, 1.f, 1.f};
 
-	uint32_t				_selected_sensor_device_id{0};
+	uint32_t _selected_sensor_device_id{0};
-	uint8_t					_selected_sensor_sub_index{0};
+	uint8_t _selected_sensor_sub_index{0};
-	int8_t					_corrections_selected_instance{-1};
+	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};
+};