sensors: refactor common corrections and rotation code

src/modules/sensors/CMakeLists.txt

@@ -31,6 +31,8 @@
 #
 ############################################################################
 
+add_subdirectory(sensor_corrections) # used by vehicle_{acceleration, angular_velocity, imu}
+include_directories(${CMAKE_CURRENT_SOURCE_DIR})
 add_subdirectory(vehicle_acceleration)
 add_subdirectory(vehicle_angular_velocity)
 add_subdirectory(vehicle_imu)

src/modules/sensors/sensor_corrections/CMakeLists.txt

@@ -0,0 +1,37 @@
+############################################################################
+#
+#   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(sensor_corrections
+	SensorCorrections.cpp
+	SensorCorrections.hpp
+)

src/modules/sensors/sensor_corrections/SensorCorrections.cpp

@@ -0,0 +1,170 @@
+/****************************************************************************
+ *
+ *   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 "SensorCorrections.hpp"
+
+using namespace matrix;
+using namespace time_literals;
+using math::radians;
+
+namespace sensors
+{
+
+SensorCorrections::SensorCorrections(ModuleParams *parent, SensorType type) :
+	ModuleParams(parent),
+	_type(type)
+{
+}
+
+void SensorCorrections::set_device_id(uint32_t device_id)
+{
+	if (_device_id != device_id) {
+		_device_id = device_id;
+		SensorCorrectionsUpdate(true);
+	}
+}
+
+const char *SensorCorrections::SensorString() const
+{
+	switch (_type) {
+	case SensorType::Accelerometer:
+		return "ACC";
+
+	case SensorType::Gyroscope:
+		return "GYRO";
+	}
+
+	return nullptr;
+}
+
+void SensorCorrections::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)) {
+
+			// selected sensor has changed, find updated index
+			if ((_corrections_selected_instance < 0) || force) {
+				_corrections_selected_instance = -1;
+
+				// find sensor_corrections index
+				for (int i = 0; i < MAX_SENSOR_COUNT; i++) {
+
+					switch (_type) {
+					case SensorType::Accelerometer:
+						if (corrections.accel_device_ids[i] == _device_id) {
+							_corrections_selected_instance = i;
+						}
+
+						break;
+
+					case SensorType::Gyroscope:
+						if (corrections.gyro_device_ids[i] == _device_id) {
+							_corrections_selected_instance = i;
+						}
+
+						break;
+					}
+				}
+			}
+
+			switch (_type) {
+			case SensorType::Accelerometer:
+				switch (_corrections_selected_instance) {
+				case 0:
+					_offset = Vector3f{corrections.accel_offset_0};
+					_scale = Vector3f{corrections.accel_scale_0};
+					return;
+				case 1:
+					_offset = Vector3f{corrections.accel_offset_1};
+					_scale = Vector3f{corrections.accel_scale_1};
+					return;
+				case 2:
+					_offset = Vector3f{corrections.accel_offset_2};
+					_scale = Vector3f{corrections.accel_scale_2};
+					return;
+				}
+
+				break;
+
+			case SensorType::Gyroscope:
+				switch (_corrections_selected_instance) {
+				case 0:
+					_offset = Vector3f{corrections.gyro_offset_0};
+					_scale = Vector3f{corrections.gyro_scale_0};
+					return;
+				case 1:
+					_offset = Vector3f{corrections.gyro_offset_1};
+					_scale = Vector3f{corrections.gyro_scale_1};
+					return;
+				case 2:
+					_offset = Vector3f{corrections.gyro_offset_2};
+					_scale = Vector3f{corrections.gyro_scale_2};
+					return;
+				}
+
+				break;
+			}
+		}
+	}
+}
+
+void SensorCorrections::ParametersUpdate()
+{
+	// 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())));
+
+	// get transformation matrix from sensor/board to body frame
+	_board_rotation = board_rotation_offset * get_rot_matrix((enum Rotation)_param_sens_board_rot.get());
+}
+
+void SensorCorrections::PrintStatus()
+{
+	if (_offset.norm() > 0.f) {
+		PX4_INFO("%s %d offset: [%.3f %.3f %.3f]", SensorString(), _device_id, (double)_offset(0), (double)_offset(1),
+			 (double)_offset(2));
+	}
+
+	if (fabsf(_scale.norm_squared() - 3.f) > FLT_EPSILON) {
+		PX4_INFO("%s %d scale: [%.3f %.3f %.3f]", SensorString(), _device_id, (double)_scale(0), (double)_scale(1),
+			 (double)_scale(2));
+	}
+}
+
+} // namespace sensors

src/modules/sensors/sensor_corrections/SensorCorrections.hpp

@@ -0,0 +1,99 @@
+/****************************************************************************
+ *
+ *   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 <lib/conversion/rotation.h>
+#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 <uORB/Subscription.hpp>
+#include <uORB/topics/sensor_correction.h>
+
+namespace sensors
+{
+
+class SensorCorrections : public ModuleParams
+{
+public:
+
+	enum class SensorType : uint8_t {
+		Accelerometer,
+		Gyroscope,
+	};
+
+	SensorCorrections() = delete;
+	SensorCorrections(ModuleParams *parent, SensorType type);
+	~SensorCorrections() override = default;
+
+	void PrintStatus();
+
+	void set_device_id(uint32_t device_id);
+	uint32_t get_device_id() const { return _device_id; }
+
+	// apply offsets and scale
+	// rotate corrected measurements from sensor to body frame
+	matrix::Vector3f Correct(const matrix::Vector3f &data) const { return _board_rotation * matrix::Vector3f{(data - _offset).emult(_scale)}; }
+
+	void ParametersUpdate();
+	void SensorCorrectionsUpdate(bool force = false);
+
+private:
+
+	static constexpr int MAX_SENSOR_COUNT = 3;
+
+	const char *SensorString() const;
+
+	uORB::Subscription _sensor_correction_sub{ORB_ID(sensor_correction)};
+
+	matrix::Dcmf _board_rotation;
+
+	matrix::Vector3f _offset{0.f, 0.f, 0.f};
+	matrix::Vector3f _scale{1.f, 1.f, 1.f};
+
+	uint32_t _device_id{0};
+	int8_t _corrections_selected_instance{-1};
+
+	const SensorType _type;
+
+	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
+	)
+};
+
+} // namespace sensors

src/modules/sensors/sensors.cpp

@@ -567,11 +567,15 @@ int Sensors::print_status()
 	PX4_INFO("Airspeed status:");
 	_airspeed_validator.print();
 
+	PX4_INFO_RAW("\n");
 	_vehicle_acceleration.PrintStatus();
+
+	PX4_INFO_RAW("\n");
 	_vehicle_angular_velocity.PrintStatus();
 
 	for (auto &i : _vehicle_imu_list) {
 		if (i != nullptr) {
+			PX4_INFO_RAW("\n");
 			i->PrintStatus();
 		}
 	}

src/modules/sensors/vehicle_acceleration/CMakeLists.txt

@@ -35,4 +35,4 @@ px4_add_library(vehicle_acceleration
 	VehicleAcceleration.cpp
 	VehicleAcceleration.hpp
 )
-target_link_libraries(vehicle_acceleration PRIVATE px4_work_queue)
+target_link_libraries(vehicle_acceleration PRIVATE sensor_corrections px4_work_queue)

src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp

@@ -37,11 +37,14 @@
 
 using namespace matrix;
 using namespace time_literals;
-using math::radians;
+
+namespace sensors
+{
 
 VehicleAcceleration::VehicleAcceleration() :
 	ModuleParams(nullptr),
-	WorkItem(MODULE_NAME, px4::wq_configurations::att_pos_ctrl)
+	WorkItem(MODULE_NAME, px4::wq_configurations::att_pos_ctrl),
+	_corrections(this, SensorCorrections::SensorType::Accelerometer)
 {
 	_lp_filter.set_cutoff_frequency(kInitialRateHz, _param_imu_accel_cutoff.get());
 }
@@ -135,46 +138,6 @@ void VehicleAcceleration::SensorBiasUpdate(bool force)
 	}
 }
 
-void VehicleAcceleration::SensorCorrectionsUpdate(bool force)
-{
-	// check if the selected sensor has updated
-	if (_sensor_correction_sub.updated() || force) {
-
-		sensor_correction_s corrections{};
-		_sensor_correction_sub.copy(&corrections);
-
-		// selected sensor has changed, find updated index
-		if ((_corrections_selected_instance < 0) || force) {
-			_corrections_selected_instance = -1;
-
-			// find sensor_corrections index
-			for (int i = 0; i < MAX_SENSOR_COUNT; i++) {
-				if (corrections.accel_device_ids[i] == _selected_sensor_device_id) {
-					_corrections_selected_instance = i;
-				}
-			}
-		}
-
-		switch (_corrections_selected_instance) {
-		case 0:
-			_offset = Vector3f{corrections.accel_offset_0};
-			_scale = Vector3f{corrections.accel_scale_0};
-			break;
-		case 1:
-			_offset = Vector3f{corrections.accel_offset_1};
-			_scale = Vector3f{corrections.accel_scale_1};
-			break;
-		case 2:
-			_offset = Vector3f{corrections.accel_offset_2};
-			_scale = Vector3f{corrections.accel_scale_2};
-			break;
-		default:
-			_offset = Vector3f{0.f, 0.f, 0.f};
-			_scale = Vector3f{1.f, 1.f, 1.f};
-		}
-	}
-}
-
 bool VehicleAcceleration::SensorSelectionUpdate(bool force)
 {
 	if (_sensor_selection_sub.updated() || (_selected_sensor_device_id == 0) || force) {
@@ -201,11 +164,8 @@ bool VehicleAcceleration::SensorSelectionUpdate(bool force)
 
 						// clear bias and corrections
 						_bias.zero();
-						_offset = Vector3f{0.f, 0.f, 0.f};
-						_scale = Vector3f{1.f, 1.f, 1.f};
 
-						// force corrections reselection
+						_corrections.set_device_id(report.device_id);
-						_corrections_selected_instance = -1;
 
 						// reset sample rate monitor
 						_sample_rate_incorrect_count = 0;
@@ -234,16 +194,7 @@ void VehicleAcceleration::ParametersUpdate(bool force)
 
 		updateParams();
 
-		// get transformation matrix from sensor/board to body frame
+		_corrections.ParametersUpdate();
-		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;
 	}
 }
 
@@ -252,7 +203,7 @@ void VehicleAcceleration::Run()
 	// update corrections first to set _selected_sensor
 	bool selection_updated = SensorSelectionUpdate();
 
-	SensorCorrectionsUpdate(selection_updated);
+	_corrections.SensorCorrectionsUpdate(selection_updated);
 	SensorBiasUpdate(selection_updated);
 	ParametersUpdate();
 
@@ -281,14 +232,8 @@ void VehicleAcceleration::Run()
 			sensor_updated = _sensor_sub[_selected_sensor_sub_index].updated();
 
 			if (!sensor_updated) {
-				// apply offsets and scale
-				Vector3f accel{(accel_filtered - _offset).emult(_scale)};
-
-				// rotate corrected measurements from sensor to body frame
-				accel = _board_rotation * accel;
-
 				// correct for in-run bias errors
-				accel -= _bias;
+				const Vector3f accel = _corrections.Correct(accel_filtered) - _bias;
 
 				// Publish vehicle_acceleration
 				vehicle_acceleration_s v_acceleration;
@@ -308,9 +253,10 @@ void VehicleAcceleration::PrintStatus()
 {
 	PX4_INFO("selected sensor: %d (%d)", _selected_sensor_device_id, _selected_sensor_sub_index);
 	PX4_INFO("bias: [%.3f %.3f %.3f]", (double)_bias(0), (double)_bias(1), (double)_bias(2));
-	PX4_INFO("offset: [%.3f %.3f %.3f]", (double)_offset(0), (double)_offset(1), (double)_offset(2));
-	PX4_INFO("scale: [%.3f %.3f %.3f]", (double)_scale(0), (double)_scale(1), (double)_scale(2));
 
 	PX4_INFO("sample rate: %.3f Hz", (double)_update_rate_hz);
-	PX4_INFO("low-pass filter cutoff: %.3f Hz", (double)_lp_filter.get_cutoff_freq());
+
+	_corrections.PrintStatus();
 }
+
+} // namespace sensors

src/modules/sensors/vehicle_acceleration/VehicleAcceleration.hpp

@@ -33,7 +33,8 @@
 
 #pragma once
 
-#include <lib/conversion/rotation.h>
+#include <sensor_corrections/SensorCorrections.hpp>
+
 #include <lib/mathlib/math/Limits.hpp>
 #include <lib/matrix/matrix/math.hpp>
 #include <lib/mathlib/math/filter/LowPassFilter2pVector3f.hpp>
@@ -47,10 +48,12 @@
 #include <uORB/topics/estimator_sensor_bias.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/sensor_accel.h>
-#include <uORB/topics/sensor_correction.h>
 #include <uORB/topics/sensor_selection.h>
 #include <uORB/topics/vehicle_acceleration.h>
 
+namespace sensors
+{
+
 class VehicleAcceleration : public ModuleParams, public px4::WorkItem
 {
 public:
@@ -69,26 +72,14 @@ private:
 	void CheckFilters();
 	void ParametersUpdate(bool force = false);
 	void SensorBiasUpdate(bool force = false);
-	void SensorCorrectionsUpdate(bool force = false);
 	bool SensorSelectionUpdate(bool force = false);
 
 	static constexpr int MAX_SENSOR_COUNT = 3;
 
-	DEFINE_PARAMETERS(
-		(ParamFloat<px4::params::IMU_ACCEL_CUTOFF>) _param_imu_accel_cutoff,
-
-		(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::Publication<vehicle_acceleration_s> _vehicle_acceleration_pub{ORB_ID(vehicle_acceleration)};
 
 	uORB::Subscription _estimator_sensor_bias_sub{ORB_ID(estimator_sensor_bias)};
 	uORB::Subscription _params_sub{ORB_ID(parameter_update)};
-	uORB::Subscription _sensor_correction_sub{ORB_ID(sensor_correction)};
 
 	uORB::SubscriptionCallbackWorkItem _sensor_selection_sub{this, ORB_ID(sensor_selection)};
 	uORB::SubscriptionCallbackWorkItem _sensor_sub[MAX_SENSOR_COUNT] {
@@ -97,13 +88,11 @@ private:
 		{this, ORB_ID(sensor_accel), 2}
 	};
 
+	SensorCorrections _corrections;
+
 	perf_counter_t _interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": interval")};
 
-	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 _acceleration_prev{0.f, 0.f, 0.f};
 
@@ -119,5 +108,10 @@ private:
 
 	uint32_t _selected_sensor_device_id{0};
 	uint8_t _selected_sensor_sub_index{0};
-	int8_t _corrections_selected_instance{-1};
+
+	DEFINE_PARAMETERS(
+		(ParamFloat<px4::params::IMU_ACCEL_CUTOFF>) _param_imu_accel_cutoff
+	)
 };
+
+} // namespace sensors

src/modules/sensors/vehicle_angular_velocity/CMakeLists.txt

@@ -35,4 +35,4 @@ px4_add_library(vehicle_angular_velocity
 	VehicleAngularVelocity.cpp
 	VehicleAngularVelocity.hpp
 )
-target_link_libraries(vehicle_angular_velocity PRIVATE px4_work_queue)
+target_link_libraries(vehicle_angular_velocity PRIVATE sensor_corrections px4_work_queue)

src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp

@@ -37,11 +37,14 @@
 
 using namespace matrix;
 using namespace time_literals;
-using math::radians;
+
+namespace sensors
+{
 
 VehicleAngularVelocity::VehicleAngularVelocity() :
 	ModuleParams(nullptr),
-	WorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl)
+	WorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl),
+	_corrections(this, SensorCorrections::SensorType::Gyroscope)
 {
 	_lp_filter_velocity.set_cutoff_frequency(kInitialRateHz, _param_imu_gyro_cutoff.get());
 	_notch_filter_velocity.setParameters(kInitialRateHz, _param_imu_gyro_nf_freq.get(), _param_imu_gyro_nf_bw.get());
@@ -150,46 +153,6 @@ void VehicleAngularVelocity::SensorBiasUpdate(bool force)
 	}
 }
 
-void VehicleAngularVelocity::SensorCorrectionsUpdate(bool force)
-{
-	// check if the selected sensor has updated
-	if (_sensor_correction_sub.updated() || force) {
-
-		sensor_correction_s corrections{};
-		_sensor_correction_sub.copy(&corrections);
-
-		// selected sensor has changed, find updated index
-		if ((_corrections_selected_instance < 0) || force) {
-			_corrections_selected_instance = -1;
-
-			// find sensor_corrections index
-			for (int i = 0; i < MAX_SENSOR_COUNT; i++) {
-				if (corrections.gyro_device_ids[i] == _selected_sensor_device_id) {
-					_corrections_selected_instance = i;
-				}
-			}
-		}
-
-		switch (_corrections_selected_instance) {
-		case 0:
-			_offset = Vector3f{corrections.gyro_offset_0};
-			_scale = Vector3f{corrections.gyro_scale_0};
-			break;
-		case 1:
-			_offset = Vector3f{corrections.gyro_offset_1};
-			_scale = Vector3f{corrections.gyro_scale_1};
-			break;
-		case 2:
-			_offset = Vector3f{corrections.gyro_offset_2};
-			_scale = Vector3f{corrections.gyro_scale_2};
-			break;
-		default:
-			_offset = Vector3f{0.f, 0.f, 0.f};
-			_scale = Vector3f{1.f, 1.f, 1.f};
-		}
-	}
-}
-
 bool VehicleAngularVelocity::SensorSelectionUpdate(bool force)
 {
 	if (_sensor_selection_sub.updated() || (_selected_sensor_device_id == 0) || force) {
@@ -216,11 +179,8 @@ bool VehicleAngularVelocity::SensorSelectionUpdate(bool force)
 
 						// clear bias and corrections
 						_bias.zero();
-						_offset = Vector3f{0.f, 0.f, 0.f};
-						_scale = Vector3f{1.f, 1.f, 1.f};
 
-						// force corrections reselection
+						_corrections.set_device_id(report.device_id);
-						_corrections_selected_instance = -1;
 
 						// reset sample rate monitor
 						_sample_rate_incorrect_count = 0;
@@ -249,16 +209,7 @@ void VehicleAngularVelocity::ParametersUpdate(bool force)
 
 		updateParams();
 
-		// get transformation matrix from sensor/board to body frame
+		_corrections.ParametersUpdate();
-		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;
 	}
 }
 
@@ -267,7 +218,7 @@ void VehicleAngularVelocity::Run()
 	// update corrections first to set _selected_sensor
 	bool selection_updated = SensorSelectionUpdate();
 
-	SensorCorrectionsUpdate(selection_updated);
+	_corrections.SensorCorrectionsUpdate(selection_updated);
 	SensorBiasUpdate(selection_updated);
 	ParametersUpdate();
 
@@ -290,10 +241,10 @@ void VehicleAngularVelocity::Run()
 			_timestamp_sample_prev = sensor_data.timestamp_sample;
 
 			// get the sensor data and correct for thermal errors (apply offsets and scale)
-			Vector3f angular_velocity_raw{(Vector3f{sensor_data.x, sensor_data.y, sensor_data.z} - _offset).emult(_scale)};
+			const Vector3f val{sensor_data.x, sensor_data.y, sensor_data.z};
 
-			// rotate corrected measurements from sensor to body frame
+			// correct for in-run bias errors
-			angular_velocity_raw = _board_rotation * angular_velocity_raw;
+			Vector3f angular_velocity_raw = _corrections.Correct(val) - _bias;
 
 			// correct for in-run bias errors
 			angular_velocity_raw -= _bias;
@@ -352,14 +303,10 @@ void VehicleAngularVelocity::PrintStatus()
 {
 	PX4_INFO("selected sensor: %d (%d)", _selected_sensor_device_id, _selected_sensor_sub_index);
 	PX4_INFO("bias: [%.3f %.3f %.3f]", (double)_bias(0), (double)_bias(1), (double)_bias(2));
-	PX4_INFO("offset: [%.3f %.3f %.3f]", (double)_offset(0), (double)_offset(1), (double)_offset(2));
-	PX4_INFO("scale: [%.3f %.3f %.3f]", (double)_scale(0), (double)_scale(1), (double)_scale(2));
 
 	PX4_INFO("sample rate: %.3f Hz", (double)_update_rate_hz);
-	PX4_INFO("low-pass filter cutoff: %.3f Hz", (double)_lp_filter_velocity.get_cutoff_freq());
 
-	if (_notch_filter_velocity.getNotchFreq() > 0.0f) {
+	_corrections.PrintStatus();
-		PX4_INFO("notch filter freq: %.3f Hz\tbandwidth: %.3f Hz", (double)_notch_filter_velocity.getNotchFreq(),
-			 (double)_notch_filter_velocity.getBandwidth());
-	}
 }
+
+} // namespace sensors

src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp

@@ -33,7 +33,8 @@
 
 #pragma once
 
-#include <lib/conversion/rotation.h>
+#include <sensor_corrections/SensorCorrections.hpp>
+
 #include <lib/mathlib/math/Limits.hpp>
 #include <lib/matrix/matrix/math.hpp>
 #include <lib/mathlib/math/filter/LowPassFilter2pVector3f.hpp>
@@ -47,12 +48,14 @@
 #include <uORB/SubscriptionCallback.hpp>
 #include <uORB/topics/estimator_sensor_bias.h>
 #include <uORB/topics/parameter_update.h>
-#include <uORB/topics/sensor_correction.h>
 #include <uORB/topics/sensor_gyro.h>
 #include <uORB/topics/sensor_selection.h>
 #include <uORB/topics/vehicle_angular_acceleration.h>
 #include <uORB/topics/vehicle_angular_velocity.h>
 
+namespace sensors
+{
+
 class VehicleAngularVelocity : public ModuleParams, public px4::WorkItem
 {
 public:
@@ -71,32 +74,15 @@ private:
 	void CheckFilters();
 	void ParametersUpdate(bool force = false);
 	void SensorBiasUpdate(bool force = false);
-	void SensorCorrectionsUpdate(bool force = false);
 	bool SensorSelectionUpdate(bool force = false);
 
 	static constexpr int MAX_SENSOR_COUNT = 3;
 
-	DEFINE_PARAMETERS(
-		(ParamFloat<px4::params::IMU_GYRO_CUTOFF>) _param_imu_gyro_cutoff,
-		(ParamFloat<px4::params::IMU_GYRO_NF_FREQ>) _param_imu_gyro_nf_freq,
-		(ParamFloat<px4::params::IMU_GYRO_NF_BW>) _param_imu_gyro_nf_bw,
-		(ParamInt<px4::params::IMU_GYRO_RATEMAX>) _param_imu_gyro_rate_max,
-
-		(ParamFloat<px4::params::IMU_DGYRO_CUTOFF>) _param_imu_dgyro_cutoff,
-
-		(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::Publication<vehicle_angular_acceleration_s> _vehicle_angular_acceleration_pub{ORB_ID(vehicle_angular_acceleration)};
 	uORB::Publication<vehicle_angular_velocity_s> _vehicle_angular_velocity_pub{ORB_ID(vehicle_angular_velocity)};
 
 	uORB::Subscription _estimator_sensor_bias_sub{ORB_ID(estimator_sensor_bias)};
 	uORB::Subscription _params_sub{ORB_ID(parameter_update)};
-	uORB::Subscription _sensor_correction_sub{ORB_ID(sensor_correction)};
 
 	uORB::SubscriptionCallbackWorkItem _sensor_selection_sub{this, ORB_ID(sensor_selection)};
 	uORB::SubscriptionCallbackWorkItem _sensor_sub[MAX_SENSOR_COUNT] {
@@ -105,13 +91,11 @@ private:
 		{this, ORB_ID(sensor_gyro), 2}
 	};
 
+	SensorCorrections _corrections;
+
 	perf_counter_t _interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": interval")};
 
-	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 _angular_acceleration_prev{0.f, 0.f, 0.f};
 	matrix::Vector3f _angular_velocity_prev{0.f, 0.f, 0.f};
@@ -134,5 +118,15 @@ private:
 
 	uint32_t _selected_sensor_device_id{0};
 	uint8_t _selected_sensor_sub_index{0};
-	int8_t _corrections_selected_instance{-1};
+
+	DEFINE_PARAMETERS(
+		(ParamFloat<px4::params::IMU_GYRO_CUTOFF>) _param_imu_gyro_cutoff,
+		(ParamFloat<px4::params::IMU_GYRO_NF_FREQ>) _param_imu_gyro_nf_freq,
+		(ParamFloat<px4::params::IMU_GYRO_NF_BW>) _param_imu_gyro_nf_bw,
+		(ParamInt<px4::params::IMU_GYRO_RATEMAX>) _param_imu_gyro_rate_max,
+
+		(ParamFloat<px4::params::IMU_DGYRO_CUTOFF>) _param_imu_dgyro_cutoff
+	)
 };
+
+} // namespace sensors

src/modules/sensors/vehicle_imu/CMakeLists.txt

@@ -35,4 +35,4 @@ px4_add_library(vehicle_imu
 	VehicleIMU.cpp
 	VehicleIMU.hpp
 )
-target_link_libraries(vehicle_imu PRIVATE px4_work_queue)
+target_link_libraries(vehicle_imu PRIVATE sensor_corrections px4_work_queue)

src/modules/sensors/vehicle_imu/VehicleIMU.cpp

@@ -37,20 +37,23 @@
 
 using namespace matrix;
 using namespace time_literals;
-using math::radians;
+
+namespace sensors
+{
 
 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)
+	_sensor_gyro_integrated_sub(this, ORB_ID(sensor_gyro_integrated), gyro_index),
+	_accel_corrections(this, SensorCorrections::SensorType::Accelerometer),
+	_gyro_corrections(this, SensorCorrections::SensorType::Gyroscope)
 {
 }
 
 VehicleIMU::~VehicleIMU()
 {
 	Stop();
-	delete _name;
 }
 
 bool VehicleIMU::Start()
@@ -70,81 +73,6 @@ void VehicleIMU::Stop()
 	_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
@@ -155,16 +83,8 @@ void VehicleIMU::ParametersUpdate(bool force)
 
 		updateParams();
 
-		// get transformation matrix from sensor/board to body frame
+		_accel_corrections.ParametersUpdate();
-		const Dcmf board_rotation = get_rot_matrix((enum Rotation)_param_sens_board_rot.get());
+		_gyro_corrections.ParametersUpdate();
-
-		// 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;
 	}
 }
 
@@ -173,39 +93,26 @@ 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;
+		_accel_corrections.set_device_id(accel.device_id);
 
 		sensor_gyro_integrated_s gyro;
 		_sensor_gyro_integrated_sub.copy(&gyro);
-		_gyro_device_id = gyro.device_id;
+		_gyro_corrections.set_device_id(gyro.device_id);
 
-		SensorCorrectionsUpdate();
 		ParametersUpdate();
+		_accel_corrections.SensorCorrectionsUpdate();
+		_gyro_corrections.SensorCorrectionsUpdate();
 
 		// 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
+		Vector3f delta_angle = _gyro_corrections.Correct(Vector3f{gyro.delta_angle} * gyro_dt) / gyro_dt;
-		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
+		Vector3f delta_velocity = _accel_corrections.Correct(Vector3f{accel.delta_velocity} * accel_dt) / accel_dt;
-		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;
@@ -224,5 +131,9 @@ void VehicleIMU::Run()
 
 void VehicleIMU::PrintStatus()
 {
-	PX4_INFO("selected IMU: accel: %d gyro: %d", _accel_device_id, _gyro_device_id);
+	PX4_INFO("selected IMU: accel: %d gyro: %d", _accel_corrections.get_device_id(), _gyro_corrections.get_device_id());
+	_accel_corrections.PrintStatus();
+	_gyro_corrections.PrintStatus();
 }
+
+} // namespace sensors

src/modules/sensors/vehicle_imu/VehicleIMU.hpp

@@ -33,23 +33,25 @@
 
 #pragma once
 
-#include <containers/List.hpp>
+#include <sensor_corrections/SensorCorrections.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_config.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>
 
+namespace sensors
+{
+
 class VehicleIMU : public ModuleParams, public px4::WorkItem
 {
 public:
@@ -64,41 +66,16 @@ public:
 	void PrintStatus();
 
 private:
+	void ParametersUpdate(bool force = false);
 	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;
+	SensorCorrections _accel_corrections;
-
+	SensorCorrections _gyro_corrections;
-	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};
 };
+
+} // namespace sensors