consolidate IMU processing and initial sensor_imu_fifo support

.ci/Jenkinsfile-hardware

@@ -744,7 +744,6 @@ void runTests() {
 
   resetParameters()
 
-  sh './Tools/HIL/nsh_param_set.py --device `find /dev/serial -name *usb-*` --name "IMU_GYRO_CAL_EN" --value "0" || true' // disable during testing
   sh './Tools/HIL/nsh_param_set.py --device `find /dev/serial -name *usb-*` --name "IMU_GYRO_FFT_EN" --value "0" || true' // disable during testing
   sh './Tools/HIL/nsh_param_set.py --device `find /dev/serial -name *usb-*` --name "SENS_IMU_AUTOCAL" --value "0" || true' // disable during testing
   sh './Tools/HIL/nsh_param_set.py --device `find /dev/serial -name *usb-*` --name "SENS_MAG_AUTOCAL" --value "0" || true' // disable during testing
@@ -853,12 +852,11 @@ void printTopics() {
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener rate_ctrl_status" || true'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener safety" || true'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_accel" || true'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_accel_fifo" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_imu_fifo" || true'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_baro" || true'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_combined" || true'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_gyro" || true'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_gyro_fft" || true'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_gyro_fifo" || true'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_mag" || true'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_preflight_mag" || true'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_selection" || true'

ROMFS/cannode/init.d/rcS

@@ -96,12 +96,6 @@ then
 	px4flow start -X &
 fi
 
-if param compare -s IMU_GYRO_CAL_EN 1
-then
-	gyro_calibration start
-fi
-
-
 if param compare -s MBE_ENABLE 1
 then
 	# conservative mag bias estimation

ROMFS/px4fmu_common/init.d-posix/rcS

@@ -295,11 +295,6 @@ then
 	gyro_fft start
 fi
 
-if param compare -s IMU_GYRO_CAL_EN 1
-then
-	gyro_calibration start
-fi
-
 # Payload deliverer module if gripper is enabled
 if param compare -s PD_GRIPPER_EN 1
 then

ROMFS/px4fmu_common/init.d/rcS

@@ -453,11 +453,6 @@ else
 		gyro_fft start
 	fi
 
-	if param compare -s IMU_GYRO_CAL_EN 1
-	then
-		gyro_calibration start
-	fi
-
 	# Check for px4flow sensor
 	if param compare -s SENS_EN_PX4FLOW 1
 	then

msg/CMakeLists.txt

@@ -167,6 +167,7 @@ set(msg_files
 	SensorGyroFft.msg
 	SensorGyroFifo.msg
 	SensorHygrometer.msg
+	SensorImuFifo.msg
 	SensorMag.msg
 	SensorOpticalFlow.msg
 	SensorPreflightMag.msg

msg/SensorImuFifo.msg

@@ -0,0 +1,26 @@
+uint64 timestamp          # time since system start (microseconds)
+uint64 timestamp_sample
+
+uint32 device_id          # unique device ID for the sensor that does not change between power cycles
+
+float32 dt           # delta time between samples (microseconds)
+uint8 samples        # number of valid samples
+
+float32 accel_scale
+float32 gyro_scale
+
+uint8 FIFO_SIZE = 24
+
+int16[24] accel_x         # gyro in the FRD board frame X-axis
+int16[24] accel_y         # gyro in the FRD board frame Y-axis
+int16[24] accel_z         # gyro in the FRD board frame Z-axis
+
+int16[24] gyro_x          # gyro in the FRD board frame X-axis
+int16[24] gyro_y          # gyro in the FRD board frame Y-axis
+int16[24] gyro_z          # gyro in the FRD board frame Z-axis
+
+float32 temperature       # temperature in degrees Celsius
+
+uint32 error_count
+
+uint8 ORB_QUEUE_LENGTH = 4

platforms/common/uORB/SubscriptionCallback.hpp

@@ -61,6 +61,11 @@ public:
 	{
 	}
 
+	SubscriptionCallback(ORB_ID id, uint32_t interval_us = 0, uint8_t instance = 0) :
+		SubscriptionInterval(id, interval_us, instance)
+	{
+	}
+
 	virtual ~SubscriptionCallback()
 	{
 		unregisterCallback();
@@ -158,6 +163,12 @@ public:
 	{
 	}
 
+	SubscriptionCallbackWorkItem(px4::WorkItem *work_item, ORB_ID id, uint8_t instance = 0) :
+		SubscriptionCallback(id, 0, instance),	// interval 0
+		_work_item(work_item)
+	{
+	}
+
 	virtual ~SubscriptionCallbackWorkItem() = default;
 
 	void call() override

posix-configs/SITL/init/test/test_imu_filtering

@@ -53,7 +53,7 @@ logger off
 
 sensors status
 listener sensor_gyro
-listener sensor_gyro_fifo
+listener sensor_imu_fifo
 listener sensor_gyro_fft
 perf
 

src/lib/sensor_calibration/Accelerometer.hpp

@@ -76,6 +76,7 @@ public:
 	const matrix::Dcmf &rotation() const { return _rotation; }
 	const Rotation &rotation_enum() const { return _rotation_enum; }
 	const matrix::Vector3f &scale() const { return _scale; }
+	const matrix::Vector3f &thermal_offset() const { return _thermal_offset; }
 
 	// apply offsets and scale
 	// rotate corrected measurements from sensor to body frame
@@ -84,6 +85,11 @@ public:
 		return _rotation * matrix::Vector3f{(data - _thermal_offset - _offset).emult(_scale)};
 	}
 
+	inline matrix::Vector3f Uncorrect(const matrix::Vector3f &corrected_data) const
+	{
+		return (_rotation.I() * corrected_data).edivide(_scale) + _thermal_offset + _offset;
+	}
+
 	// Compute sensor offset from bias (board frame)
 	matrix::Vector3f BiasCorrectedSensorOffset(const matrix::Vector3f &bias) const
 	{

src/modules/gyro_calibration/CMakeLists.txt

@@ -1,45 +0,0 @@
-############################################################################
-#
-#   Copyright (c) 2021-2022 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_module(
-	MODULE modules__gyro_calibration
-	MAIN gyro_calibration
-	COMPILE_FLAGS
-		#-DDEBUG_BUILD
-	SRCS
-		GyroCalibration.cpp
-		GyroCalibration.hpp
-	DEPENDS
-		px4_work_queue
-		sensor_calibration
-	)

src/modules/gyro_calibration/GyroCalibration.cpp

@@ -1,351 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2021-2022 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 "GyroCalibration.hpp"
-
-#include <lib/geo/geo.h>
-
-using namespace time_literals;
-using matrix::Vector3f;
-
-GyroCalibration::GyroCalibration() :
-	ModuleParams(nullptr),
-	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::lp_default)
-{
-}
-
-GyroCalibration::~GyroCalibration()
-{
-	perf_free(_loop_interval_perf);
-	perf_free(_calibration_updated_perf);
-}
-
-bool GyroCalibration::init()
-{
-	ScheduleOnInterval(INTERVAL_US);
-	return true;
-}
-
-void GyroCalibration::Run()
-{
-	if (should_exit()) {
-		ScheduleClear();
-		exit_and_cleanup();
-		return;
-	}
-
-	perf_count(_loop_interval_perf);
-
-	if (_vehicle_status_sub.updated()) {
-		vehicle_status_s vehicle_status;
-
-		if (_vehicle_status_sub.copy(&vehicle_status)) {
-			const bool armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);
-
-			if (armed != _armed) {
-				if (!_armed && armed) {
-					// disarmed -> armed: run at minimal rate until disarmed
-					ScheduleOnInterval(10_s);
-
-				} else if (_armed && !armed) {
-					// armed -> disarmed: start running again
-					ScheduleOnInterval(INTERVAL_US);
-				}
-
-				_armed = armed;
-				Reset();
-				return;
-			}
-
-			if (_system_calibrating != vehicle_status.calibration_enabled) {
-				_system_calibrating = vehicle_status.calibration_enabled;
-				Reset();
-				return;
-			}
-		}
-	}
-
-	if (_armed) {
-		// do nothing if armed
-		return;
-	}
-
-	if (_system_calibrating) {
-		// do nothing if system is calibrating
-		Reset();
-		return;
-	}
-
-
-	// Check if parameters have changed
-	if (_parameter_update_sub.updated()) {
-		// clear update
-		parameter_update_s param_update;
-
-		if (_parameter_update_sub.copy(&param_update)) {
-			// minimize updates immediately following parameter changes
-			_last_calibration_update = param_update.timestamp;
-		}
-
-		for (auto &cal : _gyro_calibration) {
-			cal.ParametersUpdate();
-		}
-
-		Reset();
-		return;
-	}
-
-
-	// collect raw data from all available gyroscopes (sensor_gyro)
-	for (int gyro = 0; gyro < _sensor_gyro_subs.size(); gyro++) {
-		sensor_gyro_s sensor_gyro;
-
-		if (_sensor_gyro_subs[gyro].update(&sensor_gyro)) {
-			if (PX4_ISFINITE(sensor_gyro.temperature)) {
-				if ((fabsf(_temperature[gyro] - sensor_gyro.temperature) > 1.f) || !PX4_ISFINITE(_temperature[gyro])) {
-					PX4_DEBUG("gyro %d temperature change, resetting all %.6f -> %.6f", gyro, (double)_temperature[gyro],
-						  (double)sensor_gyro.temperature);
-
-					_temperature[gyro] = sensor_gyro.temperature;
-
-					// reset all on any temperature change
-					Reset();
-					return;
-				}
-
-			} else {
-				_temperature[gyro] = NAN;
-			}
-
-			if (_gyro_calibration[gyro].device_id() == sensor_gyro.device_id) {
-				_gyro_calibration[gyro].SensorCorrectionsUpdate();
-				const Vector3f val{Vector3f{sensor_gyro.x, sensor_gyro.y, sensor_gyro.z} - _gyro_calibration[gyro].thermal_offset()};
-				_gyro_mean[gyro].update(val);
-				_gyro_last_update[gyro] = sensor_gyro.timestamp;
-
-			} else {
-				// setting device id, reset all
-				_gyro_calibration[gyro].set_device_id(sensor_gyro.device_id);
-				Reset();
-				return;
-			}
-		}
-
-		if ((_gyro_last_update[gyro] != 0) && (hrt_elapsed_time(&_gyro_last_update[gyro]) > 100_ms)) {
-			// remove sensor and reset on any timeout
-			_gyro_calibration[gyro].set_device_id(0);
-			_gyro_calibration[gyro].Reset();
-
-			Reset();
-			return;
-		}
-	}
-
-
-	// check all accelerometers for possible movement
-	for (int accel = 0; accel < _sensor_accel_subs.size(); accel++) {
-		sensor_accel_s sensor_accel;
-
-		if (_sensor_accel_subs[accel].update(&sensor_accel)) {
-			const Vector3f acceleration{sensor_accel.x, sensor_accel.y, sensor_accel.z};
-
-			if ((acceleration - _acceleration[accel]).longerThan(0.5f)) {
-				// reset all on any change
-				PX4_DEBUG("accel %d changed, resetting all %.5f", accel, (double)(acceleration - _acceleration[accel]).length());
-
-				_acceleration[accel] = acceleration;
-				Reset();
-				return;
-
-			} else if (acceleration.longerThan(CONSTANTS_ONE_G * 1.3f)) {
-				Reset();
-				return;
-			}
-		}
-	}
-
-	// update calibrations for all available gyros
-	if (hrt_elapsed_time(&_last_calibration_update) > 5_s) {
-
-		// check variance again before saving
-		for (int gyro = 0; gyro < _sensor_gyro_subs.size(); gyro++) {
-			if (_gyro_calibration[gyro].device_id() != 0) {
-
-				if (!_gyro_mean[gyro].valid() || _gyro_mean[gyro].variance().longerThan(0.001f)) {
-					// reset all
-					PX4_DEBUG("gyro %d variance longer than 0.001f (%.3f), resetting all",
-						  gyro, (double)_gyro_mean[gyro].variance().length());
-					Reset();
-					return;
-				}
-			}
-		}
-
-		bool calibration_updated = false;
-
-		for (int gyro = 0; gyro < _sensor_gyro_subs.size(); gyro++) {
-
-			if ((_gyro_calibration[gyro].device_id() != 0)
-			    && _gyro_mean[gyro].valid() && (_gyro_mean[gyro].count() > 100)
-			   ) {
-
-				const Vector3f old_offset{_gyro_calibration[gyro].offset()};
-				const Vector3f new_offset{_gyro_mean[gyro].mean()};
-
-				bool change_exceeds_stddev = false;
-				bool variance_significantly_better = false;
-
-				const Vector3f variance = _gyro_mean[gyro].variance();
-
-				for (int i = 0; i < 3; i++) {
-					// check if offset changed by more than 1 standard deviation
-					if (sq(new_offset(i) - old_offset(i)) > variance(i)) {
-						change_exceeds_stddev = true;
-					}
-
-					// check if current variance is significantly better than previous cal
-					if (variance(i) < 0.1f * _gyro_cal_variance[gyro](i)) {
-						variance_significantly_better = true;
-					}
-				}
-
-				// update if offset changed by more than 1 standard deviation or currently uncalibrated
-				if ((change_exceeds_stddev || variance_significantly_better || !_gyro_calibration[gyro].calibrated())
-				    && _gyro_calibration[gyro].set_offset(new_offset)
-				   ) {
-
-					calibration_updated = true;
-
-					_gyro_cal_variance[gyro] = variance;
-
-					PX4_INFO("gyro %d (%" PRIu32 ") updating offsets [%.3f, %.3f, %.3f]->[%.3f, %.3f, %.3f] %.1f degC",
-						 gyro, _gyro_calibration[gyro].device_id(),
-						 (double)old_offset(0), (double)old_offset(1), (double)old_offset(2),
-						 (double)new_offset(0), (double)new_offset(1), (double)new_offset(2),
-						 (double)_temperature[gyro]);
-
-					perf_count(_calibration_updated_perf);
-				}
-			}
-		}
-
-		// save all calibrations
-		if (calibration_updated) {
-			bool param_save = false;
-
-			for (int gyro = 0; gyro < _sensor_gyro_subs.size(); gyro++) {
-				if (_gyro_calibration[gyro].device_id() != 0) {
-					if (_gyro_calibration[gyro].ParametersSave(gyro)) {
-						param_save = true;
-					}
-				}
-			}
-
-			if (param_save) {
-				param_notify_changes();
-			}
-
-			Reset();
-		}
-	}
-}
-
-int GyroCalibration::task_spawn(int argc, char *argv[])
-{
-	GyroCalibration *instance = new GyroCalibration();
-
-	if (instance) {
-		_object.store(instance);
-		_task_id = task_id_is_work_queue;
-
-		if (instance->init()) {
-			return PX4_OK;
-		}
-
-	} else {
-		PX4_ERR("alloc failed");
-	}
-
-	delete instance;
-	_object.store(nullptr);
-	_task_id = -1;
-
-	return PX4_ERROR;
-}
-
-int GyroCalibration::print_status()
-{
-	for (int gyro = 0; gyro < _sensor_gyro_subs.size(); gyro++) {
-		if (_gyro_calibration[gyro].device_id() != 0) {
-			PX4_INFO_RAW("gyro %d (%" PRIu32 "), [%.5f, %.5f, %.5f] var: [%.9f, %.9f, %.9f] %.1f degC (count %d)\n",
-				     gyro, _gyro_calibration[gyro].device_id(),
-				     (double)_gyro_mean[gyro].mean()(0), (double)_gyro_mean[gyro].mean()(1), (double)_gyro_mean[gyro].mean()(2),
-				     (double)_gyro_mean[gyro].variance()(0), (double)_gyro_mean[gyro].variance()(1), (double)_gyro_mean[gyro].variance()(2),
-				     (double)_temperature[gyro], _gyro_mean[gyro].count());
-		}
-	}
-
-	perf_print_counter(_loop_interval_perf);
-	perf_print_counter(_calibration_updated_perf);
-	return 0;
-}
-
-int GyroCalibration::custom_command(int argc, char *argv[])
-{
-	return print_usage("unknown command");
-}
-
-int GyroCalibration::print_usage(const char *reason)
-{
-	if (reason) {
-		PX4_WARN("%s\n", reason);
-	}
-
-	PRINT_MODULE_DESCRIPTION(
-		R"DESCR_STR(
-### Description
-Simple online gyroscope calibration.
-
-)DESCR_STR");
-
-	PRINT_MODULE_USAGE_NAME("gyro_calibration", "system");
-	PRINT_MODULE_USAGE_COMMAND("start");
-	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
-
-	return 0;
-}
-
-extern "C" __EXPORT int gyro_calibration_main(int argc, char *argv[])
-{
-	return GyroCalibration::main(argc, argv);
-}

src/modules/gyro_calibration/GyroCalibration.hpp

@@ -1,114 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2021-2022 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 <px4_platform_common/defines.h>
-#include <px4_platform_common/module.h>
-#include <px4_platform_common/module_params.h>
-#include <px4_platform_common/posix.h>
-#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
-#include <drivers/drv_hrt.h>
-#include <lib/mathlib/math/WelfordMeanVector.hpp>
-#include <lib/perf/perf_counter.h>
-#include <lib/sensor_calibration/Gyroscope.hpp>
-#include <uORB/Subscription.hpp>
-#include <uORB/SubscriptionInterval.hpp>
-#include <uORB/SubscriptionMultiArray.hpp>
-#include <uORB/topics/parameter_update.h>
-#include <uORB/topics/sensor_accel.h>
-#include <uORB/topics/sensor_gyro.h>
-#include <uORB/topics/vehicle_status.h>
-
-using namespace time_literals;
-
-class GyroCalibration : public ModuleBase<GyroCalibration>, public ModuleParams, public px4::ScheduledWorkItem
-{
-public:
-	GyroCalibration();
-	~GyroCalibration() override;
-
-	/** @see ModuleBase */
-	static int task_spawn(int argc, char *argv[]);
-
-	/** @see ModuleBase */
-	static int custom_command(int argc, char *argv[]);
-
-	/** @see ModuleBase */
-	static int print_usage(const char *reason = nullptr);
-
-	bool init();
-
-	int print_status() override;
-
-private:
-	static constexpr hrt_abstime INTERVAL_US = 20000_us;
-	static constexpr int MAX_SENSORS = 4;
-
-	void Run() override;
-
-	void Reset()
-	{
-		for (int gyro = 0; gyro < MAX_SENSORS; gyro++) {
-			_gyro_mean[gyro].reset();
-			_gyro_last_update[gyro] = 0;
-		}
-
-		_last_calibration_update = hrt_absolute_time();
-	}
-
-	// return the square of two floating point numbers
-	static constexpr float sq(float var) { return var * var; }
-
-	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
-	uORB::Subscription _vehicle_status_sub{ORB_ID::vehicle_status};
-
-	uORB::SubscriptionMultiArray<sensor_accel_s, MAX_SENSORS> _sensor_accel_subs{ORB_ID::sensor_accel};
-	uORB::SubscriptionMultiArray<sensor_gyro_s, MAX_SENSORS>  _sensor_gyro_subs{ORB_ID::sensor_gyro};
-
-	calibration::Gyroscope _gyro_calibration[MAX_SENSORS] {};
-	math::WelfordMeanVector<float, 3> _gyro_mean[MAX_SENSORS] {};
-	matrix::Vector3f _gyro_cal_variance[MAX_SENSORS] {};
-	float _temperature[MAX_SENSORS] {};
-	hrt_abstime _gyro_last_update[MAX_SENSORS] {};
-
-	matrix::Vector3f _acceleration[MAX_SENSORS] {};
-
-	hrt_abstime _last_calibration_update{0};
-
-	bool _armed{false};
-	bool _system_calibrating{false};
-
-	perf_counter_t _loop_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": interval")};
-	perf_counter_t _calibration_updated_perf{perf_alloc(PC_COUNT, MODULE_NAME": calibration updated")};
-};

src/modules/gyro_calibration/Kconfig

@@ -1,12 +0,0 @@
-menuconfig MODULES_GYRO_CALIBRATION
-	bool "gyro_calibration"
-	default n
-	---help---
-		Enable support for gyro_calibration
-
-menuconfig USER_GYRO_CALIBRATION
-	bool "gyro_calibration running as userspace module"
-	default y
-	depends on BOARD_PROTECTED && MODULES_GYRO_CALIBRATION
-	---help---
-		Put gyro_calibration in userspace memory

src/modules/gyro_calibration/parameters.c

@@ -1,41 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2021 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.
- *
- ****************************************************************************/
-
-/**
-* IMU gyro auto calibration enable.
-*
-* @boolean
-* @reboot_required true
-* @group Sensors
-*/
-PARAM_DEFINE_INT32(IMU_GYRO_CAL_EN, 1);

src/modules/sensors/CMakeLists.txt

@@ -35,18 +35,12 @@ add_subdirectory(data_validator)
 
 include_directories(${CMAKE_CURRENT_SOURCE_DIR})
 
-if(CONFIG_SENSORS_VEHICLE_ACCELERATION)
-	add_subdirectory(vehicle_acceleration)
-endif()
-
-add_subdirectory(vehicle_imu)
-
 if(CONFIG_SENSORS_VEHICLE_AIR_DATA)
 	add_subdirectory(vehicle_air_data)
 endif()
 
-if(CONFIG_SENSORS_VEHICLE_ANGULAR_VELOCITY)
-	add_subdirectory(vehicle_angular_velocity)
+if(CONFIG_SENSORS_VEHICLE_IMU)
+	add_subdirectory(vehicle_imu)
 endif()
 
 if(CONFIG_SENSORS_VEHICLE_GPS_POSITION)
@@ -68,8 +62,7 @@ px4_add_module(
 		${CMAKE_CURRENT_SOURCE_DIR}
 	SRCS
 		sensors.cpp
-		voted_sensors_update.cpp
-		voted_sensors_update.h
+		sensors.hpp
 		Integrator.hpp
 	MODULE_CONFIG
 		module.yaml
@@ -78,13 +71,8 @@ px4_add_module(
 		data_validator
 		mathlib
 		sensor_calibration
-		vehicle_imu
 	)
 
-if(CONFIG_SENSORS_VEHICLE_ACCELERATION)
-	target_link_libraries(modules__sensors PRIVATE vehicle_acceleration)
-endif()
-
 if(CONFIG_SENSORS_VEHICLE_AIR_DATA)
 	target_link_libraries(modules__sensors PRIVATE vehicle_air_data)
 endif()
@@ -93,8 +81,8 @@ if(CONFIG_SENSORS_VEHICLE_AIRSPEED)
 	target_link_libraries(modules__sensors PRIVATE airspeed)
 endif()
 
-if(CONFIG_SENSORS_VEHICLE_ANGULAR_VELOCITY)
-	target_link_libraries(modules__sensors PRIVATE vehicle_angular_velocity)
+if(CONFIG_SENSORS_VEHICLE_IMU)
+	target_link_libraries(modules__sensors PRIVATE vehicle_imu)
 endif()
 
 if(CONFIG_SENSORS_VEHICLE_GPS_POSITION)

src/modules/sensors/Integrator.hpp

@@ -81,6 +81,7 @@ public:
 	 * @param reset_interval	New reset time interval for the integrator in microseconds.
 	 */
 	void set_reset_interval(uint32_t reset_interval_us) { _reset_interval_min = reset_interval_us * 1e-6f; }
+	uint32_t reset_interval_us() const { return _reset_interval_min * 1e6f; }
 
 	/**
 	 * Set required samples for reset. This won't reset the integrator.
@@ -90,6 +91,8 @@ public:
 	void set_reset_samples(uint8_t reset_samples) { _reset_samples_min = reset_samples; }
 	uint8_t get_reset_samples() const { return _reset_samples_min; }
 
+	uint8_t integrated_samples() const { return _integrated_samples; }
+
 	/**
 	 * Is the Integrator ready to reset?
 	 *
@@ -142,7 +145,7 @@ protected:
 	matrix::Vector3f _last_val{0.f, 0.f, 0.f}; /**< previous input */
 	float _integral_dt{0};
 
-	float _reset_interval_min{0.001f}; /**< the interval after which the content will be published and the integrator reset */
+	float _reset_interval_min{0.01f}; /**< the interval after which the content will be published and the integrator reset */
 	uint8_t _reset_samples_min{1};
 
 	uint8_t _integrated_samples{0};

src/modules/sensors/Kconfig

@@ -20,12 +20,8 @@ if MODULES_SENSORS
         bool "Include vehicle air data"
         default y
 
-    config SENSORS_VEHICLE_ANGULAR_VELOCITY
-        bool "Include vehicle angular velocity"
-        default y
-
-    config SENSORS_VEHICLE_ACCELERATION
-        bool "Include vehicle acceleration"
+    config SENSORS_VEHICLE_IMU
+        bool "Include vehicle IMU"
         default y
 
     config SENSORS_VEHICLE_GPS_POSITION

src/modules/sensors/sensor_params.c

@@ -210,18 +210,6 @@ PARAM_DEFINE_INT32(SENS_EN_THERMAL, -1);
  */
 PARAM_DEFINE_INT32(SENS_EXT_I2C_PRB, 1);
 
-/**
- * Sensors hub IMU mode
- *
- * @value 0 Disabled
- * @value 1 Publish primary IMU selection
- *
- * @category system
- * @reboot_required true
- * @group Sensors
- */
-PARAM_DEFINE_INT32(SENS_IMU_MODE, 1);
-
 /**
  * Enable internal barometers
  *

src/modules/sensors/sensors.cpp

@@ -43,18 +43,12 @@
 
 #include "sensors.hpp"
 
-Sensors::Sensors(bool hil_enabled) :
+Sensors::Sensors() :
 	ModuleParams(nullptr),
 	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers),
-	_hil_enabled(hil_enabled),
-	_loop_perf(perf_alloc(PC_ELAPSED, "sensors")),
-	_voted_sensors_update(hil_enabled, _vehicle_imu_sub)
+	_loop_perf(perf_alloc(PC_ELAPSED, "sensors"))
 {
-	_sensor_pub.advertise();
 
-#if defined(CONFIG_SENSORS_VEHICLE_ACCELERATION)
-	_vehicle_acceleration.Start();
-#endif // CONFIG_SENSORS_VEHICLE_ACCELERATION
 
 #if defined(CONFIG_SENSORS_VEHICLE_AIRSPEED)
 	/* Differential pressure offset */
@@ -63,6 +57,10 @@ Sensors::Sensors(bool hil_enabled) :
 	_parameter_handles.diff_pres_analog_scale = param_find("SENS_DPRES_ANSC");
 #endif /* ADC_AIRSPEED_VOLTAGE_CHANNEL */
 
+#if defined(CONFIG_SENSORS_VEHICLE_IMU)
+	_vehicle_imu.Start();
+#endif // CONFIG_SENSORS_VEHICLE_IMU
+
 	_parameter_handles.air_cmodel = param_find("CAL_AIR_CMODEL");
 	_parameter_handles.air_tube_length = param_find("CAL_AIR_TUBELEN");
 	_parameter_handles.air_tube_diameter_mm = param_find("CAL_AIR_TUBED_MM");
@@ -82,27 +80,15 @@ Sensors::Sensors(bool hil_enabled) :
 	param_find("SYS_CAL_TMAX");
 	param_find("SYS_CAL_TMIN");
 
-	_sensor_combined.accelerometer_timestamp_relative = sensor_combined_s::RELATIVE_TIMESTAMP_INVALID;
-
 	parameters_update();
-
-	InitializeVehicleIMU();
 }
 
 Sensors::~Sensors()
 {
-	// clear all registered callbacks
-	for (auto &sub : _vehicle_imu_sub) {
-		sub.unregisterCallback();
-	}
 
-#if defined(CONFIG_SENSORS_VEHICLE_ACCELERATION)
-	_vehicle_acceleration.Stop();
-#endif // CONFIG_SENSORS_VEHICLE_ACCELERATION
-
-#if defined(CONFIG_SENSORS_VEHICLE_ANGULAR_VELOCITY)
-	_vehicle_angular_velocity.Stop();
-#endif // CONFIG_SENSORS_VEHICLE_ANGULAR_VELOCITY
+#if defined(CONFIG_SENSORS_VEHICLE_IMU)
+	_vehicle_imu.Stop();
+#endif // CONFIG_SENSORS_VEHICLE_IMU
 
 #if defined(CONFIG_SENSORS_VEHICLE_AIR_DATA)
 
@@ -140,29 +126,17 @@ Sensors::~Sensors()
 
 #endif // CONFIG_SENSORS_VEHICLE_OPTICAL_FLOW
 
-	for (auto &vehicle_imu : _vehicle_imu_list) {
-		if (vehicle_imu) {
-			vehicle_imu->Stop();
-			delete vehicle_imu;
-		}
-	}
-
 	perf_free(_loop_perf);
 }
 
 bool Sensors::init()
 {
-	_vehicle_imu_sub[0].registerCallback();
-	ScheduleNow();
+	ScheduleDelayed(100_ms);
 	return true;
 }
 
 int Sensors::parameters_update()
 {
-	if (_armed) {
-		return 0;
-	}
-
 #if defined(CONFIG_SENSORS_VEHICLE_AIRSPEED)
 	/* Airspeed offset */
 	param_get(_parameter_handles.diff_pres_offset_pa, &(_parameters.diff_pres_offset_pa));
@@ -175,69 +149,6 @@ int Sensors::parameters_update()
 	param_get(_parameter_handles.air_tube_diameter_mm, &_parameters.air_tube_diameter_mm);
 #endif // CONFIG_SENSORS_VEHICLE_AIRSPEED
 
-	_voted_sensors_update.parametersUpdate();
-
-	// 1. mark all existing sensor calibrations active even if sensor is missing
-	//    this preserves the calibration in the event of a parameter export while the sensor is missing
-	// 2. ensure calibration slots are active for the number of sensors currently available
-	//    this to done to eliminate differences in the active set of parameters before and after sensor calibration
-	for (uint8_t i = 0; i < MAX_SENSOR_COUNT; i++) {
-		// sensor_accel
-		{
-			const uint32_t device_id_accel = calibration::GetCalibrationParamInt32("ACC",  "ID", i);
-
-			if (device_id_accel != 0) {
-				calibration::Accelerometer accel_cal(device_id_accel);
-			}
-
-			uORB::SubscriptionData<sensor_accel_s> sensor_accel_sub{ORB_ID(sensor_accel), i};
-
-			if (sensor_accel_sub.advertised() && (sensor_accel_sub.get().device_id != 0)) {
-				calibration::Accelerometer cal;
-				cal.set_calibration_index(i);
-				cal.ParametersLoad();
-			}
-		}
-
-
-		// sensor_gyro
-		{
-			const uint32_t device_id_gyro = calibration::GetCalibrationParamInt32("GYRO", "ID", i);
-
-			if (device_id_gyro != 0) {
-				calibration::Gyroscope gyro_cal(device_id_gyro);
-			}
-
-			uORB::SubscriptionData<sensor_gyro_s> sensor_gyro_sub{ORB_ID(sensor_gyro), i};
-
-			if (sensor_gyro_sub.advertised() && (sensor_gyro_sub.get().device_id != 0)) {
-				calibration::Gyroscope cal;
-				cal.set_calibration_index(i);
-				cal.ParametersLoad();
-			}
-		}
-
-
-#if defined(CONFIG_SENSORS_VEHICLE_MAGNETOMETER)
-		// sensor_mag
-		{
-			uint32_t device_id_mag = calibration::GetCalibrationParamInt32("MAG",  "ID", i);
-
-			if (device_id_mag != 0) {
-				calibration::Magnetometer mag_cal(device_id_mag);
-			}
-
-			uORB::SubscriptionData<sensor_mag_s> sensor_mag_sub{ORB_ID(sensor_mag), i};
-
-			if (sensor_mag_sub.advertised() && (sensor_mag_sub.get().device_id != 0)) {
-				calibration::Magnetometer cal;
-				cal.set_calibration_index(i);
-				cal.ParametersLoad();
-			}
-		}
-#endif // CONFIG_SENSORS_VEHICLE_MAGNETOMETER
-	}
-
 #if defined(CONFIG_SENSORS_VEHICLE_AIR_DATA)
 	InitializeVehicleAirData();
 #endif // CONFIG_SENSORS_VEHICLE_AIR_DATA
@@ -367,11 +278,6 @@ void Sensors::diff_pres_poll()
 
 void Sensors::adc_poll()
 {
-	/* only read if not in HIL mode */
-	if (_hil_enabled) {
-		return;
-	}
-
 #ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL
 
 	if (_parameters.diff_pres_analog_scale > 0.0f) {
@@ -447,41 +353,6 @@ void Sensors::InitializeVehicleGPSPosition()
 }
 #endif // CONFIG_SENSORS_VEHICLE_GPS_POSITION
 
-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), i};
-			uORB::Subscription gyro_sub{ORB_ID(sensor_gyro), i};
-
-			if (accel_sub.advertised() && gyro_sub.advertised()) {
-				// if the sensors module is responsible for voting (SENS_IMU_MODE 1) then run every VehicleIMU in the same WQ
-				//   otherwise each VehicleIMU runs in a corresponding INSx WQ
-				const bool multi_mode = (_param_sens_imu_mode.get() == 0);
-				const px4::wq_config_t &wq_config = multi_mode ? px4::ins_instance_to_wq(i) : px4::wq_configurations::INS0;
-
-				VehicleIMU *imu = new VehicleIMU(i, i, i, wq_config);
-
-				if (imu != nullptr) {
-					// Start VehicleIMU instance and store
-					if (imu->Start()) {
-						_vehicle_imu_list[i] = imu;
-
-					} else {
-						delete imu;
-					}
-				}
-
-			} else {
-				// abort on first failure, try again later
-				return;
-			}
-		}
-	}
-}
-
 #if defined(CONFIG_SENSORS_VEHICLE_MAGNETOMETER)
 void Sensors::InitializeVehicleMagnetometer()
 {
@@ -517,109 +388,21 @@ void Sensors::InitializeVehicleOpticalFlow()
 void Sensors::Run()
 {
 	if (should_exit()) {
-		// clear all registered callbacks
-		for (auto &sub : _vehicle_imu_sub) {
-			sub.unregisterCallback();
-		}
-
 		exit_and_cleanup();
 		return;
 	}
 
 	perf_begin(_loop_perf);
 
-	// check vehicle status for changes to publication state
-	if (_vcontrol_mode_sub.updated()) {
-		vehicle_control_mode_s vcontrol_mode{};
+	// check for parameter updates
+	if (_parameter_update_sub.updated()) {
+		// clear update
+		parameter_update_s pupdate;
+		_parameter_update_sub.copy(&pupdate);
 
-		if (_vcontrol_mode_sub.copy(&vcontrol_mode)) {
-			_armed = vcontrol_mode.flag_armed;
-		}
-	}
-
-	// keep adding sensors as long as we are not armed,
-	// when not adding sensors poll for param updates
-	if ((!_armed && hrt_elapsed_time(&_last_config_update) > 500_ms) || (_last_config_update == 0)) {
-
-		bool updated = false;
-
-#if defined(CONFIG_SENSORS_VEHICLE_AIR_DATA)
-		const int n_baro = orb_group_count(ORB_ID(sensor_baro));
-
-		if (n_baro != _n_baro) {
-			_n_baro = n_baro;
-			updated = true;
-		}
-
-#endif // CONFIG_SENSORS_VEHICLE_AIR_DATA
-
-#if defined(CONFIG_SENSORS_VEHICLE_GPS_POSITION)
-		const int n_gps = orb_group_count(ORB_ID(sensor_gps));
-
-		if (n_gps != _n_gps) {
-			_n_gps = n_gps;
-			updated = true;
-		}
-
-#endif // CONFIG_SENSORS_VEHICLE_GPS_POSITION
-
-#if defined(CONFIG_SENSORS_VEHICLE_MAGNETOMETER)
-		const int n_mag = orb_group_count(ORB_ID(sensor_mag));
-
-		if (n_mag != _n_mag) {
-			_n_mag = n_mag;
-			updated = true;
-		}
-
-#endif // CONFIG_SENSORS_VEHICLE_MAGNETOMETER
-
-#if defined(CONFIG_SENSORS_VEHICLE_OPTICAL_FLOW)
-		const int n_optical_flow = orb_group_count(ORB_ID(sensor_optical_flow));
-
-		if (n_optical_flow != _n_optical_flow) {
-			_n_optical_flow = n_optical_flow;
-			updated = true;
-		}
-
-#endif // CONFIG_SENSORS_VEHICLE_OPTICAL_FLOW
-
-
-		const int n_accel = orb_group_count(ORB_ID(sensor_accel));
-		const int n_gyro  = orb_group_count(ORB_ID(sensor_gyro));
-
-		if ((n_accel != _n_accel) || (n_gyro != _n_gyro) || updated) {
-			_n_accel = n_accel;
-			_n_gyro = n_gyro;
-
-			parameters_update();
-		}
-
-		// sensor device id (not just orb_group_count) must be populated before IMU init can succeed
-		_voted_sensors_update.initializeSensors();
-		InitializeVehicleIMU();
-
-		_last_config_update = hrt_absolute_time();
-
-	} else {
-		// check for parameter updates
-		if (_parameter_update_sub.updated()) {
-			// clear update
-			parameter_update_s pupdate;
-			_parameter_update_sub.copy(&pupdate);
-
-			// update parameters from storage
-			parameters_update();
-			updateParams();
-		}
-	}
-
-	_voted_sensors_update.sensorsPoll(_sensor_combined);
-
-	if (_sensor_combined.timestamp != _sensor_combined_prev_timestamp) {
-
-		_voted_sensors_update.setRelativeTimestamps(_sensor_combined);
-		_sensor_pub.publish(_sensor_combined);
-		_sensor_combined_prev_timestamp = _sensor_combined.timestamp;
+		// update parameters from storage
+		updateParams();
+		parameters_update();
 	}
 
 #if defined(CONFIG_SENSORS_VEHICLE_AIRSPEED)
@@ -628,43 +411,12 @@ void Sensors::Run()
 	diff_pres_poll();
 #endif // CONFIG_SENSORS_VEHICLE_AIRSPEED
 
-	// backup schedule as a watchdog timeout
-	ScheduleDelayed(10_ms);
-
 	perf_end(_loop_perf);
 }
 
 int Sensors::task_spawn(int argc, char *argv[])
 {
-	bool hil_enabled = false;
-	bool error_flag = false;
-
-	int myoptind = 1;
-	int ch;
-	const char *myoptarg = nullptr;
-
-	while ((ch = px4_getopt(argc, argv, "h", &myoptind, &myoptarg)) != EOF) {
-		switch (ch) {
-		case 'h':
-			hil_enabled = true;
-			break;
-
-		case '?':
-			error_flag = true;
-			break;
-
-		default:
-			PX4_WARN("unrecognized flag");
-			error_flag = true;
-			break;
-		}
-	}
-
-	if (error_flag) {
-		return PX4_ERROR;
-	}
-
-	Sensors *instance = new Sensors(hil_enabled);
+	Sensors *instance = new Sensors();
 
 	if (instance) {
 		_object.store(instance);
@@ -687,7 +439,10 @@ int Sensors::task_spawn(int argc, char *argv[])
 
 int Sensors::print_status()
 {
-	_voted_sensors_update.printStatus();
+#if defined(CONFIG_SENSORS_VEHICLE_IMU)
+	PX4_INFO_RAW("\n");
+	_vehicle_imu.PrintStatus();
+#endif // CONFIG_SENSORS_VEHICLE_IMU
 
 #if defined(CONFIG_SENSORS_VEHICLE_MAGNETOMETER)
 
@@ -722,16 +477,6 @@ int Sensors::print_status()
 
 #endif // CONFIG_SENSORS_VEHICLE_OPTICAL_FLOW
 
-#if defined(CONFIG_SENSORS_VEHICLE_ACCELERATION)
-	PX4_INFO_RAW("\n");
-	_vehicle_acceleration.PrintStatus();
-#endif // CONFIG_SENSORS_VEHICLE_ACCELERATION
-
-#if defined(CONFIG_SENSORS_VEHICLE_ANGULAR_VELOCITY)
-	PX4_INFO_RAW("\n");
-	_vehicle_angular_velocity.PrintStatus();
-#endif // CONFIG_SENSORS_VEHICLE_ANGULAR_VELOCITY
-
 #if defined(CONFIG_SENSORS_VEHICLE_GPS_POSITION)
 
 	if (_vehicle_gps_position) {
@@ -741,15 +486,6 @@ int Sensors::print_status()
 
 #endif // CONFIG_SENSORS_VEHICLE_GPS_POSITION
 
-	PX4_INFO_RAW("\n");
-
-	for (auto &i : _vehicle_imu_list) {
-		if (i != nullptr) {
-			PX4_INFO_RAW("\n");
-			i->PrintStatus();
-		}
-	}
-
 	return 0;
 }
 
@@ -787,7 +523,6 @@ It runs in its own thread and polls on the currently selected gyro topic.
 
 	PRINT_MODULE_USAGE_NAME("sensors", "system");
 	PRINT_MODULE_USAGE_COMMAND("start");
-	PRINT_MODULE_USAGE_PARAM_FLAG('h', "Start in HIL mode", true);
 	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
 
 	return 0;

src/modules/sensors/sensors.hpp

@@ -45,16 +45,7 @@
 #include <px4_platform_common/getopt.h>
 #include <px4_platform_common/module.h>
 #include <px4_platform_common/module_params.h>
-#include <uORB/Publication.hpp>
-#include <uORB/PublicationMulti.hpp>
 #include <uORB/Subscription.hpp>
-#include <uORB/SubscriptionCallback.hpp>
-#include "voted_sensors_update.h"
-#include "vehicle_imu/VehicleIMU.hpp"
-
-#if defined(CONFIG_SENSORS_VEHICLE_ACCELERATION)
-# include "vehicle_acceleration/VehicleAcceleration.hpp"
-#endif // CONFIG_SENSORS_VEHICLE_ACCELERATION
 
 #if defined(CONFIG_SENSORS_VEHICLE_AIRSPEED)
 # include <drivers/drv_sensor.h>
@@ -70,9 +61,9 @@
 # include "vehicle_air_data/VehicleAirData.hpp"
 #endif // CONFIG_SENSORS_VEHICLE_AIR_DATA
 
-#if defined(CONFIG_SENSORS_VEHICLE_ANGULAR_VELOCITY)
-# include "vehicle_angular_velocity/VehicleAngularVelocity.hpp"
-#endif // CONFIG_SENSORS_VEHICLE_ANGULAR_VELOCITY
+#if defined(CONFIG_SENSORS_VEHICLE_IMU)
+# include "vehicle_imu/VehicleIMU.hpp"
+#endif // CONFIG_SENSORS_VEHICLE_IMU
 
 #if defined(CONFIG_SENSORS_VEHICLE_GPS_POSITION)
 # include "vehicle_gps_position/VehicleGPSPosition.hpp"
@@ -80,8 +71,6 @@
 
 #if defined(CONFIG_SENSORS_VEHICLE_MAGNETOMETER)
 # include "vehicle_magnetometer/VehicleMagnetometer.hpp"
-# include <lib/sensor_calibration/Magnetometer.hpp>
-# include <uORB/topics/sensor_mag.h>
 #endif // CONFIG_SENSORS_VEHICLE_MAGNETOMETER
 
 #if defined(CONFIG_SENSORS_VEHICLE_OPTICAL_FLOW)
@@ -98,7 +87,7 @@ using namespace time_literals;
 class Sensors : public ModuleBase<Sensors>, public ModuleParams, public px4::ScheduledWorkItem
 {
 public:
-	explicit Sensors(bool hil_enabled);
+	Sensors();
 	~Sensors() override;
 
 	/** @see ModuleBase */
@@ -123,44 +112,13 @@ private:
 	int		parameters_update();
 
 	void		InitializeVehicleAirData();
-
 	void		InitializeVehicleGPSPosition();
-
-	void		InitializeVehicleIMU();
-
 	void		InitializeVehicleMagnetometer();
-
 	void		InitializeVehicleOpticalFlow();
 
-	const bool _hil_enabled;	/**< if true, HIL is active */
-
 	perf_counter_t	_loop_perf;	/**< loop performance counter */
 
-	VehicleIMU *_vehicle_imu_list[MAX_SENSOR_COUNT] {};
-
-	VotedSensorsUpdate _voted_sensors_update;
-
-	sensor_combined_s _sensor_combined{};
-
-	hrt_abstime     _last_config_update{0};
-	hrt_abstime     _sensor_combined_prev_timestamp{0};
-
-	uint8_t _n_accel{0};
-	uint8_t _n_gyro{0};
-
-	bool _armed{false};		/**< arming status of the vehicle */
-
-	uORB::SubscriptionCallbackWorkItem _vehicle_imu_sub[MAX_SENSOR_COUNT] {
-		{this, ORB_ID(vehicle_imu), 0},
-		{this, ORB_ID(vehicle_imu), 1},
-		{this, ORB_ID(vehicle_imu), 2},
-		{this, ORB_ID(vehicle_imu), 3}
-	};
-
 	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
-	uORB::Subscription _vcontrol_mode_sub{ORB_ID(vehicle_control_mode)};
-
-	uORB::Publication<sensor_combined_s> _sensor_pub{ORB_ID(sensor_combined)};
 
 #if defined(CONFIG_SENSORS_VEHICLE_AIRSPEED)
 	/**
@@ -201,54 +159,46 @@ private:
 # endif // ADC_AIRSPEED_VOLTAGE_CHANNEL
 
 	struct Parameters {
-		float diff_pres_offset_pa;
+		float diff_pres_offset_pa{0.f};
 #ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL
-		float diff_pres_analog_scale;
+		float diff_pres_analog_scale {0.f};
 #endif /* ADC_AIRSPEED_VOLTAGE_CHANNEL */
 
-		int32_t air_cmodel;
-		float air_tube_length;
-		float air_tube_diameter_mm;
+		int32_t air_cmodel{0};
+		float air_tube_length{0.f};
+		float air_tube_diameter_mm{0.f};
 	} _parameters{}; /**< local copies of interesting parameters */
 
 	struct ParameterHandles {
-		param_t diff_pres_offset_pa;
+		param_t diff_pres_offset_pa{PARAM_INVALID};
 #ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL
-		param_t diff_pres_analog_scale;
+		param_t diff_pres_analog_scale {PARAM_INVALID};
 #endif /* ADC_AIRSPEED_VOLTAGE_CHANNEL */
 
-		param_t air_cmodel;
-		param_t air_tube_length;
-		param_t air_tube_diameter_mm;
+		param_t air_cmodel{PARAM_INVALID};
+		param_t air_tube_length{PARAM_INVALID};
+		param_t air_tube_diameter_mm{PARAM_INVALID};
 	} _parameter_handles{};		/**< handles for interesting parameters */
 #endif // CONFIG_SENSORS_VEHICLE_AIRSPEED
 
-#if defined(CONFIG_SENSORS_VEHICLE_ACCELERATION)
-	VehicleAcceleration _vehicle_acceleration;
-#endif // CONFIG_SENSORS_VEHICLE_ACCELERATION
-
 #if defined(CONFIG_SENSORS_VEHICLE_AIR_DATA)
 	VehicleAirData *_vehicle_air_data {nullptr};
-	uint8_t _n_baro{0};
 #endif // CONFIG_SENSORS_VEHICLE_AIR_DATA
 
-#if defined(CONFIG_SENSORS_VEHICLE_ANGULAR_VELOCITY)
-	VehicleAngularVelocity	_vehicle_angular_velocity;
-#endif // CONFIG_SENSORS_VEHICLE_ANGULAR_VELOCITY
+#if defined(CONFIG_SENSORS_VEHICLE_IMU)
+	VehicleIMU _vehicle_imu {};
+#endif // CONFIG_SENSORS_VEHICLE_IMU
 
 #if defined(CONFIG_SENSORS_VEHICLE_MAGNETOMETER)
 	VehicleMagnetometer *_vehicle_magnetometer {nullptr};
-	uint8_t _n_mag{0};
 #endif // CONFIG_SENSORS_VEHICLE_MAGNETOMETER
 
 #if defined(CONFIG_SENSORS_VEHICLE_GPS_POSITION)
 	VehicleGPSPosition *_vehicle_gps_position {nullptr};
-	uint8_t _n_gps{0};
 #endif // CONFIG_SENSORS_VEHICLE_GPS_POSITION
 
 #if defined(CONFIG_SENSORS_VEHICLE_OPTICAL_FLOW)
 	VehicleOpticalFlow *_vehicle_optical_flow {nullptr};
-	uint8_t _n_optical_flow{0};
 #endif // CONFIG_SENSORS_VEHICLE_OPTICAL_FLOW
 
 	DEFINE_PARAMETERS(
@@ -259,8 +209,7 @@ private:
 		(ParamBool<px4::params::SYS_HAS_GPS>) _param_sys_has_gps,
 #endif // CONFIG_SENSORS_VEHICLE_GPS_POSITION
 #if defined(CONFIG_SENSORS_VEHICLE_MAGNETOMETER)
-		(ParamInt<px4::params::SYS_HAS_MAG>) _param_sys_has_mag,
+		(ParamBool<px4::params::SYS_HAS_MAG>) _param_sys_has_mag
 #endif // CONFIG_SENSORS_VEHICLE_MAGNETOMETER
-		(ParamBool<px4::params::SENS_IMU_MODE>) _param_sens_imu_mode
 	)
-};
+};

src/modules/sensors/vehicle_acceleration/CMakeLists.txt

@@ -1,46 +0,0 @@
-############################################################################
-#
-#   Copyright (c) 2019 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_acceleration
-	VehicleAcceleration.cpp
-	VehicleAcceleration.hpp
-)
-
-target_compile_options(vehicle_acceleration PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
-
-target_link_libraries(vehicle_acceleration
-	PRIVATE
-		mathlib
-		px4_work_queue
-		sensor_calibration
-)

src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp

@@ -1,271 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2019-2022 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 "VehicleAcceleration.hpp"
-
-#include <px4_platform_common/log.h>
-
-#include <uORB/topics/vehicle_imu_status.h>
-
-using namespace matrix;
-
-namespace sensors
-{
-
-VehicleAcceleration::VehicleAcceleration() :
-	ModuleParams(nullptr),
-	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers)
-{
-	_vehicle_acceleration_pub.advertise();
-
-	CheckAndUpdateFilters();
-}
-
-VehicleAcceleration::~VehicleAcceleration()
-{
-	Stop();
-}
-
-bool VehicleAcceleration::Start()
-{
-	// force initial updates
-	ParametersUpdate(true);
-
-	// sensor_selection needed to change the active sensor if the primary stops updating
-	if (!_sensor_selection_sub.registerCallback()) {
-		PX4_ERR("callback registration failed");
-		return false;
-	}
-
-	if (!SensorSelectionUpdate(true)) {
-		_sensor_sub.registerCallback();
-	}
-
-	return true;
-}
-
-void VehicleAcceleration::Stop()
-{
-	// clear all registered callbacks
-	_sensor_sub.unregisterCallback();
-	_sensor_selection_sub.unregisterCallback();
-
-	Deinit();
-}
-
-void VehicleAcceleration::CheckAndUpdateFilters()
-{
-	bool sample_rate_changed = false;
-
-	// get sample rate from vehicle_imu_status publication
-	for (uint8_t i = 0; i < MAX_SENSOR_COUNT; i++) {
-		uORB::SubscriptionData<vehicle_imu_status_s> imu_status{ORB_ID(vehicle_imu_status), i};
-
-		const float sample_rate_hz = imu_status.get().accel_rate_hz;
-
-		if (imu_status.advertised() && (imu_status.get().timestamp != 0)
-		    && (imu_status.get().accel_device_id != 0) && (imu_status.get().accel_device_id == _calibration.device_id())
-		    && PX4_ISFINITE(sample_rate_hz) && (sample_rate_hz > 0)) {
-
-			// check if sample rate error is greater than 1%
-			if (!PX4_ISFINITE(_filter_sample_rate) || (fabsf(sample_rate_hz - _filter_sample_rate) / _filter_sample_rate) > 0.01f) {
-				PX4_DEBUG("sample rate changed: %.3f Hz -> %.3f Hz", (double)_filter_sample_rate, (double)sample_rate_hz);
-				_filter_sample_rate = sample_rate_hz;
-				sample_rate_changed = true;
-
-				// determine number of sensor samples that will get closest to the desired rate
-				if (_param_imu_integ_rate.get() > 0) {
-					const float configured_interval_us = 1e6f / _param_imu_integ_rate.get();
-					const float sample_interval_avg = 1e6f / sample_rate_hz;
-					const uint8_t samples = math::constrain(roundf(configured_interval_us / sample_interval_avg), 1.f,
-										(float)sensor_accel_s::ORB_QUEUE_LENGTH);
-
-					_sensor_sub.set_required_updates(samples);
-
-				} else {
-					_sensor_sub.set_required_updates(1);
-				}
-
-				break;
-			}
-		}
-	}
-
-	// update software low pass filters
-	if (sample_rate_changed || (fabsf(_lp_filter.get_cutoff_freq() - _param_imu_accel_cutoff.get()) > 0.1f)) {
-		_lp_filter.set_cutoff_frequency(_filter_sample_rate, _param_imu_accel_cutoff.get());
-		_lp_filter.reset(_acceleration_prev);
-	}
-}
-
-void VehicleAcceleration::SensorBiasUpdate(bool force)
-{
-	// find corresponding estimated sensor bias
-	if (_estimator_selector_status_sub.updated()) {
-		estimator_selector_status_s estimator_selector_status;
-
-		if (_estimator_selector_status_sub.copy(&estimator_selector_status)) {
-			_estimator_sensor_bias_sub.ChangeInstance(estimator_selector_status.primary_instance);
-		}
-	}
-
-	if (_estimator_sensor_bias_sub.updated() || force) {
-		estimator_sensor_bias_s bias;
-
-		if (_estimator_sensor_bias_sub.copy(&bias)) {
-			if (bias.accel_device_id == _calibration.device_id()) {
-				_bias = Vector3f{bias.accel_bias};
-
-			} else {
-				_bias.zero();
-			}
-		}
-	}
-}
-
-bool VehicleAcceleration::SensorSelectionUpdate(bool force)
-{
-	if (_sensor_selection_sub.updated() || (_calibration.device_id() == 0) || force) {
-		sensor_selection_s sensor_selection{};
-		_sensor_selection_sub.copy(&sensor_selection);
-
-		if ((sensor_selection.accel_device_id != 0) && (_calibration.device_id() != sensor_selection.accel_device_id)) {
-			for (uint8_t i = 0; i < MAX_SENSOR_COUNT; i++) {
-				uORB::SubscriptionData<sensor_accel_s> sensor_accel_sub{ORB_ID(sensor_accel), i};
-
-				const uint32_t device_id = sensor_accel_sub.get().device_id;
-
-				if (sensor_accel_sub.advertised() && (sensor_accel_sub.get().timestamp != 0)
-				    && (device_id != 0) && (device_id == sensor_selection.accel_device_id)) {
-
-					if (_sensor_sub.ChangeInstance(i) && _sensor_sub.registerCallback()) {
-						PX4_DEBUG("selected sensor changed %" PRIu32 " -> %" PRIu32 "", _calibration.device_id(), device_id);
-
-						// clear bias and corrections
-						_bias.zero();
-
-						_calibration.set_device_id(device_id);
-
-						CheckAndUpdateFilters();
-
-						return true;
-					}
-				}
-			}
-
-			PX4_ERR("unable to find or subscribe to selected sensor (%" PRIu32 ")", sensor_selection.accel_device_id);
-			_calibration.set_device_id(0);
-		}
-	}
-
-	return false;
-}
-
-void VehicleAcceleration::ParametersUpdate(bool force)
-{
-	// Check if parameters have changed
-	if (_parameter_update_sub.updated() || force) {
-		// clear update
-		parameter_update_s param_update;
-		_parameter_update_sub.copy(&param_update);
-
-		updateParams();
-
-		_calibration.ParametersUpdate();
-
-		CheckAndUpdateFilters();
-	}
-}
-
-void VehicleAcceleration::Run()
-{
-	// backup schedule
-	ScheduleDelayed(10_ms);
-
-	// update corrections first to set _selected_sensor
-	bool selection_updated = SensorSelectionUpdate();
-
-	_calibration.SensorCorrectionsUpdate(selection_updated);
-	SensorBiasUpdate(selection_updated);
-	ParametersUpdate();
-
-	// require valid sensor sample rate to run
-	if (!PX4_ISFINITE(_filter_sample_rate)) {
-		CheckAndUpdateFilters();
-
-		if (!PX4_ISFINITE(_filter_sample_rate)) {
-			return;
-		}
-	}
-
-	// process all outstanding messages
-	sensor_accel_s sensor_data;
-
-	while (_sensor_sub.update(&sensor_data)) {
-		const Vector3f accel_raw{sensor_data.x, sensor_data.y, sensor_data.z};
-
-		if (accel_raw.isAllFinite()) {
-			// Apply calibration and filter
-			//  - calibration offsets, scale factors, and thermal scale (if available)
-			//  - estimated in run bias (if available)
-			//  - biquad low-pass filter
-			const Vector3f accel_corrected = _calibration.Correct(accel_raw) - _bias;
-			const Vector3f accel_filtered = _lp_filter.apply(accel_corrected);
-
-			_acceleration_prev = accel_corrected;
-
-			// publish once all new samples are processed
-			if (!_sensor_sub.updated()) {
-				// Publish vehicle_acceleration
-				vehicle_acceleration_s v_acceleration;
-				v_acceleration.timestamp_sample = sensor_data.timestamp_sample;
-				accel_filtered.copyTo(v_acceleration.xyz);
-				v_acceleration.timestamp = hrt_absolute_time();
-				_vehicle_acceleration_pub.publish(v_acceleration);
-
-				return;
-			}
-		}
-	}
-}
-
-void VehicleAcceleration::PrintStatus()
-{
-	PX4_INFO_RAW("[vehicle_acceleration] selected sensor: %" PRIu32 ", rate: %.1f Hz, estimated bias: [%.4f %.4f %.4f]\n",
-		     _calibration.device_id(), (double)_filter_sample_rate,
-		     (double)_bias(0), (double)_bias(1), (double)_bias(2));
-
-	_calibration.PrintStatus();
-}
-
-} // namespace sensors

src/modules/sensors/vehicle_acceleration/imu_accel_parameters.c

@@ -1,46 +0,0 @@
-/****************************************************************************
- *
- *   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.
- *
- ****************************************************************************/
-
-/**
-* Low pass filter cutoff frequency for accel
-*
-* The cutoff frequency for the 2nd order butterworth filter on the primary accelerometer.
-* This only affects the signal sent to the controllers, not the estimators. 0 disables the filter.
-*
-* @min 0
-* @max 1000
-* @unit Hz
-* @reboot_required true
-* @group Sensors
-*/
-PARAM_DEFINE_FLOAT(IMU_ACCEL_CUTOFF, 30.0f);

src/modules/sensors/vehicle_angular_velocity/CMakeLists.txt

@@ -1,50 +0,0 @@
-############################################################################
-#
-#   Copyright (c) 2019 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_angular_velocity
-	VehicleAngularVelocity.cpp
-	VehicleAngularVelocity.hpp
-)
-
-target_compile_options(vehicle_angular_velocity
-	PRIVATE
-		${MAX_CUSTOM_OPT_LEVEL}
-		#-DDEBUG_BUILD
-)
-
-target_link_libraries(vehicle_angular_velocity
-	PRIVATE
-		mathlib
-		px4_work_queue
-		sensor_calibration
-)

src/modules/sensors/vehicle_imu/CMakeLists.txt

@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2019-2023 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
@@ -34,12 +34,23 @@
 px4_add_library(vehicle_imu
 	VehicleIMU.cpp
 	VehicleIMU.hpp
+
+	VehicleAcceleration.cpp
+	VehicleAcceleration.hpp
+	VehicleAngularVelocity.cpp
+	VehicleAngularVelocity.hpp
 )
 
 target_compile_options(vehicle_imu
 	PRIVATE
 		${MAX_CUSTOM_OPT_LEVEL}
 		#-DDEBUG_BUILD
+		-Wno-error
 )
 
-target_link_libraries(vehicle_imu PRIVATE px4_work_queue sensor_calibration)
+target_link_libraries(vehicle_imu
+	PRIVATE
+		mathlib
+		px4_work_queue
+		sensor_calibration
+)

src/modules/sensors/vehicle_imu/IMU.hpp

@@ -0,0 +1,127 @@
+
+
+#pragma once
+
+#include <Integrator.hpp>
+
+#include <lib/sensor_calibration/Accelerometer.hpp>
+#include <lib/sensor_calibration/Gyroscope.hpp>
+#include <lib/mathlib/math/WelfordMean.hpp>
+#include <lib/mathlib/math/WelfordMeanVector.hpp>
+
+#include <uORB/PublicationMulti.hpp>
+#include <uORB/topics/vehicle_imu.h>
+#include <uORB/topics/vehicle_imu_status.h>
+
+namespace sensors
+{
+
+struct IMU {
+
+	struct InFlightCalibration {
+		uint8_t instance{0};
+		matrix::Vector3f offset{};
+		matrix::Vector3f bias_variance{};
+		bool valid{false};
+	};
+
+	struct {
+
+		calibration::Accelerometer calibration{};
+		math::WelfordMeanVector<float, 3> raw_mean{};
+
+
+
+
+		// FIFO only
+		matrix::Vector3f integral{};
+		int16_t last_raw_sample[3] {};
+		float fifo_scale{1.f};
+
+		math::WelfordMean<float> mean_publish_interval_us{}; // only needed for FIFO
+
+
+		// redundant for FIFO case
+		hrt_abstime timestamp_sample_last{0};
+		uint32_t device_id{0};
+		uint32_t error_count{0};
+		math::WelfordMean<float> temperature{};              // redundant for FIFO case
+		math::WelfordMean<float> mean_sample_interval_us{};
+		bool interval_configured{false};
+
+
+		// non-FIFO only
+		sensors::Integrator integrator{};
+
+
+
+
+		matrix::Vector3f estimated_bias{};
+		matrix::Vector3f estimated_bias_variance{};
+
+		uint32_t clipping_total[3] {}; // clipping
+		uint8_t clipping_flags{0};
+
+		matrix::Vector3f acceleration_prev{}; // acceleration from the previous IMU measurement for vibration metrics
+
+		float vibration_metric{0.f}; // high frequency vibration level in the accelerometer data (m/s/s)
+
+
+		InFlightCalibration learned_calibration[3] {};
+
+	} accel{};
+
+	struct {
+		hrt_abstime timestamp_sample_last{0};
+		uint32_t device_id{0};
+		calibration::Gyroscope calibration{};
+		math::WelfordMean<float> mean_publish_interval_us{};
+		math::WelfordMean<float> mean_sample_interval_us{};
+		math::WelfordMeanVector<float, 3> raw_mean{};
+		math::WelfordMean<float> temperature{};
+
+		uint32_t error_count{0};
+		sensors::IntegratorConing integrator{};
+
+		matrix::Vector3f estimated_bias{};
+		matrix::Vector3f estimated_bias_variance{};
+
+		uint32_t clipping_total[3] {}; // clipping
+		uint8_t clipping_flags{0};
+
+		matrix::Vector3f angular_velocity_prev{}; // angular velocity from the previous IMU measurement for vibration metrics
+
+		float vibration_metric{0.f}; // high frequency vibration level in the gyro data (rad/s)
+
+		float coning_norm_accum{0}; // average IMU delta angle coning correction (rad^2)
+		float coning_norm_accum_total_time_s{0};
+
+		bool interval_configured{false};
+
+		InFlightCalibration learned_calibration[3] {};
+	} gyro{};
+
+	bool primary{false};
+
+	uORB::PublicationMulti<vehicle_imu_s> vehicle_imu_pub{ORB_ID(vehicle_imu)};
+	uORB::PublicationMulti<vehicle_imu_status_s> vehicle_imu_status_pub{ORB_ID(vehicle_imu_status)};
+
+	hrt_abstime time_last_status_publication{0};
+
+	hrt_abstime time_last_valid{0};
+	hrt_abstime time_last_invalid{0};
+
+	// last valid
+	// last invalid
+
+
+	hrt_abstime time_last_move_detect_us{0};
+	hrt_abstime time_still_detect_us{0};
+	bool at_rest{false};
+
+
+	// noise when still?
+
+};
+
+} // namespace sensors

src/modules/sensors/vehicle_imu/VehicleAcceleration.cpp

@@ -0,0 +1,106 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2019-2023 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 "VehicleAcceleration.hpp"
+
+using namespace matrix;
+
+namespace sensors
+{
+
+VehicleAcceleration::VehicleAcceleration() :
+	ModuleParams(nullptr)
+{
+	_vehicle_acceleration_pub.advertise();
+}
+
+void VehicleAcceleration::ParametersUpdate()
+{
+	updateParams();
+
+	_filter_config_check = true;
+}
+
+void VehicleAcceleration::updateAccel(IMU &imu, const matrix::Vector3f &accel_raw, const float sample_rate_hz)
+{
+	bool filter_reset = false;
+
+	if ((_selected_sensor_device_id != imu.accel.calibration.device_id()) || _filter_config_check) {
+
+		bool sample_rate_changed = false;
+
+		if (imu.accel.mean_publish_interval_us.valid() && PX4_ISFINITE(sample_rate_hz) && (sample_rate_hz > 0)) {
+			// check if sample rate error is greater than 5%
+			if (!PX4_ISFINITE(_filter_sample_rate) || (fabsf(sample_rate_hz - _filter_sample_rate) / _filter_sample_rate) > 0.05f) {
+				PX4_DEBUG("sample rate changed: %.3f Hz -> %.3f Hz", (double)_filter_sample_rate, (double)sample_rate_hz);
+				_filter_sample_rate = sample_rate_hz;
+				sample_rate_changed = true;
+			}
+		}
+
+		// update software low pass filters
+		if (sample_rate_changed || (fabsf(_lp_filter.get_cutoff_freq() - _param_imu_accel_cutoff.get()) > 0.1f)) {
+			filter_reset = true;
+		}
+
+		_selected_sensor_device_id = imu.accel.calibration.device_id();
+		_filter_config_check = false;
+	}
+
+	if (filter_reset) {
+		_lp_filter.set_cutoff_frequency(_filter_sample_rate, _param_imu_accel_cutoff.get());
+	}
+
+	// Apply calibration and filter
+	//  - calibration offsets, scale factors, and thermal scale (if available)
+	//  - estimated in run bias (if available)
+	//  - biquad low-pass filter
+	const Vector3f accel_corrected = imu.accel.calibration.Correct(accel_raw) - imu.accel.estimated_bias;
+
+	const Vector3f accel_filtered = filter_reset ? _lp_filter.reset(accel_corrected) : _lp_filter.apply(accel_corrected);
+
+	// Publish vehicle_acceleration
+	vehicle_acceleration_s v_acceleration;
+	v_acceleration.timestamp_sample = imu.accel.timestamp_sample_last;
+	accel_filtered.copyTo(v_acceleration.xyz);
+	v_acceleration.timestamp = hrt_absolute_time();
+	_vehicle_acceleration_pub.publish(v_acceleration);
+}
+
+void VehicleAcceleration::PrintStatus()
+{
+	PX4_INFO_RAW("[vehicle_acceleration] selected sensor: %" PRIu32 ", rate: %.1f Hz\n",
+		     _selected_sensor_device_id, (double)_filter_sample_rate);
+}
+
+} // namespace sensors

src/modules/sensors/vehicle_imu/VehicleAcceleration.hpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2019 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2019-2023 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
@@ -33,22 +33,15 @@
 
 #pragma once
 
-#include <lib/sensor_calibration/Accelerometer.hpp>
+#include "IMU.hpp"
+
 #include <lib/mathlib/math/Limits.hpp>
 #include <lib/matrix/matrix/math.hpp>
 #include <lib/mathlib/math/filter/LowPassFilter2p.hpp>
 #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/ScheduledWorkItem.hpp>
 #include <uORB/Publication.hpp>
-#include <uORB/Subscription.hpp>
-#include <uORB/SubscriptionCallback.hpp>
-#include <uORB/topics/estimator_selector_status.h>
-#include <uORB/topics/estimator_sensor_bias.h>
-#include <uORB/topics/parameter_update.h>
-#include <uORB/topics/sensor_accel.h>
-#include <uORB/topics/sensor_selection.h>
 #include <uORB/topics/vehicle_acceleration.h>
 
 using namespace time_literals;
@@ -56,46 +49,31 @@ using namespace time_literals;
 namespace sensors
 {
 
-class VehicleAcceleration : public ModuleParams, public px4::ScheduledWorkItem
+class VehicleAcceleration : public ModuleParams
 {
 public:
 	VehicleAcceleration();
-	~VehicleAcceleration() override;
+	~VehicleAcceleration() override = default;
 
-	bool Start();
-	void Stop();
+	void ParametersUpdate();
 
 	void PrintStatus();
 
+	void updateAccel(IMU &imu, const matrix::Vector3f &accel_raw, const float sample_rate_hz);
+
 private:
-	void Run() override;
-
-	void CheckAndUpdateFilters();
-	void ParametersUpdate(bool force = false);
-	void SensorBiasUpdate(bool force = false);
-	bool SensorSelectionUpdate(bool force = false);
-
-	static constexpr int MAX_SENSOR_COUNT = 4;
-
 	uORB::Publication<vehicle_acceleration_s> _vehicle_acceleration_pub{ORB_ID(vehicle_acceleration)};
 
-	uORB::Subscription _estimator_selector_status_sub{ORB_ID(estimator_selector_status)};
-	uORB::Subscription _estimator_sensor_bias_sub{ORB_ID(estimator_sensor_bias)};
-
-	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
-
-	uORB::SubscriptionCallbackWorkItem _sensor_selection_sub{this, ORB_ID(sensor_selection)};
-	uORB::SubscriptionCallbackWorkItem _sensor_sub{this, ORB_ID(sensor_accel)};
-
-	calibration::Accelerometer _calibration{};
-
-	matrix::Vector3f _bias{};
-
 	matrix::Vector3f _acceleration_prev{};
 
-	float _filter_sample_rate{NAN};
+	uint32_t _selected_sensor_device_id{0};
 
-	math::LowPassFilter2p<matrix::Vector3f> _lp_filter{};
+	static constexpr float kDefaultAccelSampleFreqHz{800.f};
+	static constexpr float kDefaultAccelCutoffFreqHz{30.f};
+	math::LowPassFilter2p<matrix::Vector3f> _lp_filter{kDefaultAccelSampleFreqHz, kDefaultAccelCutoffFreqHz};
+	float _filter_sample_rate{kDefaultAccelSampleFreqHz};
+
+	bool _filter_config_check{true};
 
 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::IMU_ACCEL_CUTOFF>) _param_imu_accel_cutoff,

src/modules/sensors/vehicle_imu/VehicleAngularVelocity.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2019-2022 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2019-2023 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
@@ -35,27 +35,24 @@
 
 #include <px4_platform_common/log.h>
 
-#include <uORB/topics/vehicle_imu_status.h>
-
 using namespace matrix;
 
 namespace sensors
 {
 
 VehicleAngularVelocity::VehicleAngularVelocity() :
-	ModuleParams(nullptr),
-	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl)
+	ModuleParams(nullptr)
 {
+	// force initial updates
+	ParametersUpdate();
+
 	_vehicle_angular_velocity_pub.advertise();
 }
 
 VehicleAngularVelocity::~VehicleAngularVelocity()
 {
-	Stop();
-
 	perf_free(_cycle_perf);
 	perf_free(_filter_reset_perf);
-	perf_free(_selection_changed_perf);
 
 #if !defined(CONSTRAINED_FLASH)
 	delete[] _dynamic_notch_filter_esc_rpm;
@@ -68,49 +65,8 @@ VehicleAngularVelocity::~VehicleAngularVelocity()
 #endif // CONSTRAINED_FLASH
 }
 
-bool VehicleAngularVelocity::Start()
+bool VehicleAngularVelocity::UpdateSampleRate(float sample_rate_hz, float publish_rate_hz)
 {
-	// force initial updates
-	ParametersUpdate(true);
-
-	// sensor_selection needed to change the active sensor if the primary stops updating
-	if (!_sensor_selection_sub.registerCallback()) {
-		PX4_ERR("callback registration failed");
-		return false;
-	}
-
-	if (!SensorSelectionUpdate(true)) {
-		ScheduleNow();
-	}
-
-	return true;
-}
-
-void VehicleAngularVelocity::Stop()
-{
-	// clear all registered callbacks
-	_sensor_sub.unregisterCallback();
-	_sensor_gyro_fifo_sub.unregisterCallback();
-	_sensor_selection_sub.unregisterCallback();
-
-	Deinit();
-}
-
-bool VehicleAngularVelocity::UpdateSampleRate()
-{
-	float sample_rate_hz = NAN;
-	float publish_rate_hz = NAN;
-
-	for (uint8_t i = 0; i < MAX_SENSOR_COUNT; i++) {
-		uORB::SubscriptionData<vehicle_imu_status_s> imu_status{ORB_ID(vehicle_imu_status), i};
-
-		if (imu_status.get().gyro_device_id == _selected_sensor_device_id) {
-			sample_rate_hz = imu_status.get().gyro_raw_rate_hz;
-			publish_rate_hz = imu_status.get().gyro_rate_hz;
-			break;
-		}
-	}
-
 	// calculate sensor update rate
 	if (PX4_ISFINITE(sample_rate_hz) && (sample_rate_hz > 10) && (sample_rate_hz < 10'000)
 	    && PX4_ISFINITE(publish_rate_hz) && (publish_rate_hz > 0)
@@ -135,22 +91,22 @@ bool VehicleAngularVelocity::UpdateSampleRate()
 				const float configured_interval_us = 1e6f / _param_imu_gyro_ratemax.get();
 				const float publish_interval_us = 1e6f / publish_rate_hz;
 
-				const uint8_t samples = roundf(configured_interval_us / publish_interval_us);
+				// const uint8_t samples = roundf(configured_interval_us / publish_interval_us);
 
-				if (_fifo_available) {
-					_sensor_gyro_fifo_sub.set_required_updates(math::constrain(samples, (uint8_t)1, sensor_gyro_fifo_s::ORB_QUEUE_LENGTH));
+				// if (_fifo_available) {
+				// 	_sensor_fifo_sub.set_required_updates(math::constrain(samples, (uint8_t)1, sensor_imu_fifo_s::ORB_QUEUE_LENGTH));
 
-				} else {
-					_sensor_sub.set_required_updates(math::constrain(samples, (uint8_t)1, sensor_gyro_s::ORB_QUEUE_LENGTH));
-				}
+				// } else {
+				// 	_sensor_sub.set_required_updates(math::constrain(samples, (uint8_t)1, sensor_gyro_s::ORB_QUEUE_LENGTH));
+				// }
 
 				// publish interval (constrained 100 Hz - 8 kHz)
 				_publish_interval_min_us = math::constrain((int)roundf(configured_interval_us - (publish_interval_us * 0.5f)), 125,
 							   10000);
 
 			} else {
-				_sensor_sub.set_required_updates(1);
-				_sensor_gyro_fifo_sub.set_required_updates(1);
+				//_sensor_sub.set_required_updates(1);
+				//_sensor_fifo_sub.set_required_updates(1);
 				_publish_interval_min_us = 0;
 			}
 		}
@@ -159,12 +115,12 @@ bool VehicleAngularVelocity::UpdateSampleRate()
 	return PX4_ISFINITE(_filter_sample_rate_hz) && (_filter_sample_rate_hz > 0);
 }
 
-void VehicleAngularVelocity::ResetFilters(const hrt_abstime &time_now_us)
+void VehicleAngularVelocity::ResetFilters(const hrt_abstime &time_now_us, const IMU &imu)
 {
 	if ((_filter_sample_rate_hz > 0) && PX4_ISFINITE(_filter_sample_rate_hz)) {
 
-		const Vector3f angular_velocity_uncalibrated{GetResetAngularVelocity()};
-		const Vector3f angular_acceleration_uncalibrated{GetResetAngularAcceleration()};
+		const Vector3f angular_velocity_uncalibrated{GetResetAngularVelocity(imu)};
+		const Vector3f angular_acceleration_uncalibrated{GetResetAngularAcceleration(imu)};
 
 		for (int axis = 0; axis < 3; axis++) {
 			// angular velocity low pass
@@ -203,308 +159,141 @@ void VehicleAngularVelocity::ResetFilters(const hrt_abstime &time_now_us)
 	}
 }
 
-void VehicleAngularVelocity::SensorBiasUpdate(bool force)
+void VehicleAngularVelocity::ParametersUpdate()
 {
-	// find corresponding estimated sensor bias
-	if (_estimator_selector_status_sub.updated()) {
-		estimator_selector_status_s estimator_selector_status;
+	const bool nf0_enabled_prev = (_param_imu_gyro_nf0_frq.get() > 0.f) && (_param_imu_gyro_nf0_bw.get() > 0.f);
+	const bool nf1_enabled_prev = (_param_imu_gyro_nf1_frq.get() > 0.f) && (_param_imu_gyro_nf1_bw.get() > 0.f);
 
-		if (_estimator_selector_status_sub.copy(&estimator_selector_status)) {
-			_estimator_sensor_bias_sub.ChangeInstance(estimator_selector_status.primary_instance);
+	updateParams();
+
+	const bool nf0_enabled = (_param_imu_gyro_nf0_frq.get() > 0.f) && (_param_imu_gyro_nf0_bw.get() > 0.f);
+	const bool nf1_enabled = (_param_imu_gyro_nf1_frq.get() > 0.f) && (_param_imu_gyro_nf1_bw.get() > 0.f);
+
+	// IMU_GYRO_RATEMAX
+	if (_param_imu_gyro_ratemax.get() <= 0) {
+		const int32_t imu_gyro_ratemax = _param_imu_gyro_ratemax.get();
+		_param_imu_gyro_ratemax.reset();
+		PX4_WARN("IMU_GYRO_RATEMAX invalid (%" PRId32 "), resetting to default %" PRId32 ")", imu_gyro_ratemax,
+			 _param_imu_gyro_ratemax.get());
+	}
+
+	// constrain IMU_GYRO_RATEMAX 50-10,000 Hz
+	const int32_t imu_gyro_ratemax = constrain(_param_imu_gyro_ratemax.get(), (int32_t)50, (int32_t)10'000);
+
+	if (imu_gyro_ratemax != _param_imu_gyro_ratemax.get()) {
+		PX4_WARN("IMU_GYRO_RATEMAX updated %" PRId32 " -> %" PRIu32, _param_imu_gyro_ratemax.get(), imu_gyro_ratemax);
+		_param_imu_gyro_ratemax.set(imu_gyro_ratemax);
+		_param_imu_gyro_ratemax.commit_no_notification();
+	}
+
+	// gyro low pass cutoff frequency changed
+	for (auto &lp : _lp_filter_velocity) {
+		if (fabsf(lp.get_cutoff_freq() - _param_imu_gyro_cutoff.get()) > 0.01f) {
+			_reset_filters = true;
+			break;
 		}
 	}
 
-	if (_estimator_sensor_bias_sub.updated() || force) {
-		estimator_sensor_bias_s bias;
+	// gyro notch filter 0 frequency or bandwidth changed
+	for (auto &nf : _notch_filter0_velocity) {
+		const bool nf_freq_changed = (fabsf(nf.getNotchFreq() - _param_imu_gyro_nf0_frq.get()) > 0.01f);
+		const bool nf_bw_changed   = (fabsf(nf.getBandwidth() - _param_imu_gyro_nf0_bw.get()) > 0.01f);
 
-		if (_estimator_sensor_bias_sub.copy(&bias) && (bias.gyro_device_id == _selected_sensor_device_id)) {
-			_bias = Vector3f{bias.gyro_bias};
-
-		} else {
-			_bias.zero();
-		}
-	}
-}
-
-bool VehicleAngularVelocity::SensorSelectionUpdate(const hrt_abstime &time_now_us, bool force)
-{
-	if (_sensor_selection_sub.updated() || (_selected_sensor_device_id == 0) || force) {
-		sensor_selection_s sensor_selection{};
-		_sensor_selection_sub.copy(&sensor_selection);
-
-		bool selected_device_id_valid = false;
-		uint32_t device_id = sensor_selection.gyro_device_id;
-		uint32_t device_id_first_valid_imu = 0;
-
-		// use vehicle_imu_status to do basic sensor selection validation
-		for (uint8_t i = 0; i < MAX_SENSOR_COUNT; i++) {
-			uORB::SubscriptionData<vehicle_imu_status_s> imu_status{ORB_ID(vehicle_imu_status), i};
-
-			if (imu_status.advertised()
-			    && (imu_status.get().timestamp != 0) && (time_now_us < imu_status.get().timestamp + 1_s)
-			    && (imu_status.get().gyro_device_id != 0)) {
-				// vehicle_imu_status gyro valid
-
-				if ((device_id != 0) && (imu_status.get().gyro_device_id == device_id)) {
-					selected_device_id_valid = true;
-				}
-
-				// record first valid IMU as a backup option
-				if (device_id_first_valid_imu == 0) {
-					device_id_first_valid_imu = imu_status.get().gyro_device_id;
-				}
-			}
-		}
-
-		// if no gyro selected or healthy then use fallback
-		if ((device_id == 0) || !selected_device_id_valid) {
-			device_id = device_id_first_valid_imu;
-		}
-
-		if ((_selected_sensor_device_id != device_id) || force) {
-
-			const bool device_id_valid = (device_id != 0);
-
-			// see if the selected sensor publishes sensor_gyro_fifo
-			for (uint8_t i = 0; i < MAX_SENSOR_COUNT; i++) {
-				uORB::SubscriptionData<sensor_gyro_fifo_s> sensor_gyro_fifo_sub{ORB_ID(sensor_gyro_fifo), i};
-
-				if (sensor_gyro_fifo_sub.advertised()
-				    && (sensor_gyro_fifo_sub.get().timestamp != 0)
-				    && (sensor_gyro_fifo_sub.get().device_id != 0)
-				    && (time_now_us < sensor_gyro_fifo_sub.get().timestamp + 1_s)) {
-
-					// if no gyro was selected use the first valid sensor_gyro_fifo
-					if (!device_id_valid) {
-						device_id = sensor_gyro_fifo_sub.get().device_id;
-						PX4_WARN("no gyro selected, using sensor_gyro_fifo:%" PRIu8 " %" PRIu32, i, sensor_gyro_fifo_sub.get().device_id);
-					}
-
-					if (sensor_gyro_fifo_sub.get().device_id == device_id) {
-						if (_sensor_gyro_fifo_sub.ChangeInstance(i) && _sensor_gyro_fifo_sub.registerCallback()) {
-							// make sure non-FIFO sub is unregistered
-							_sensor_sub.unregisterCallback();
-
-							_calibration.set_device_id(sensor_gyro_fifo_sub.get().device_id);
-
-							_selected_sensor_device_id = sensor_gyro_fifo_sub.get().device_id;
-
-							_timestamp_sample_last = 0;
-							_filter_sample_rate_hz = 1.f / (sensor_gyro_fifo_sub.get().dt * 1e-6f);
-							_update_sample_rate = true;
-							_reset_filters = true;
-							_bias.zero();
-							_fifo_available = true;
-
-							perf_count(_selection_changed_perf);
-							PX4_DEBUG("selecting sensor_gyro_fifo:%" PRIu8 " %" PRIu32, i, _selected_sensor_device_id);
-							return true;
-
-						} else {
-							PX4_ERR("unable to register callback for sensor_gyro_fifo:%" PRIu8 " %" PRIu32,
-								i, sensor_gyro_fifo_sub.get().device_id);
-						}
-					}
-				}
-			}
-
-			for (uint8_t i = 0; i < MAX_SENSOR_COUNT; i++) {
-				uORB::SubscriptionData<sensor_gyro_s> sensor_gyro_sub{ORB_ID(sensor_gyro), i};
-
-				if (sensor_gyro_sub.advertised()
-				    && (sensor_gyro_sub.get().timestamp != 0)
-				    && (sensor_gyro_sub.get().device_id != 0)
-				    && (time_now_us < sensor_gyro_sub.get().timestamp + 1_s)) {
-
-					// if no gyro was selected use the first valid sensor_gyro
-					if (!device_id_valid) {
-						device_id = sensor_gyro_sub.get().device_id;
-						PX4_WARN("no gyro selected, using sensor_gyro:%" PRIu8 " %" PRIu32, i, sensor_gyro_sub.get().device_id);
-					}
-
-					if (sensor_gyro_sub.get().device_id == device_id) {
-						if (_sensor_sub.ChangeInstance(i) && _sensor_sub.registerCallback()) {
-							// make sure FIFO sub is unregistered
-							_sensor_gyro_fifo_sub.unregisterCallback();
-
-							_calibration.set_device_id(sensor_gyro_sub.get().device_id);
-
-							_selected_sensor_device_id = sensor_gyro_sub.get().device_id;
-
-							_timestamp_sample_last = 0;
-							_filter_sample_rate_hz = NAN;
-							_update_sample_rate = true;
-							_reset_filters = true;
-							_bias.zero();
-							_fifo_available = false;
-
-							perf_count(_selection_changed_perf);
-							PX4_DEBUG("selecting sensor_gyro:%" PRIu8 " %" PRIu32, i, _selected_sensor_device_id);
-							return true;
-
-						} else {
-							PX4_ERR("unable to register callback for sensor_gyro:%" PRIu8 " %" PRIu32,
-								i, sensor_gyro_sub.get().device_id);
-						}
-					}
-				}
-			}
-
-			if (device_id != 0) {
-				PX4_ERR("unable to find or subscribe to selected sensor (%" PRIu32 ")", device_id);
-			}
-
-			_selected_sensor_device_id = 0;
+		if ((nf0_enabled_prev != nf0_enabled) || (nf0_enabled && (nf_freq_changed || nf_bw_changed))) {
+			_reset_filters = true;
+			break;
 		}
 	}
 
-	return false;
-}
+	// gyro notch filter 1 frequency or bandwidth changed
+	for (auto &nf : _notch_filter1_velocity) {
+		const bool nf_freq_changed = (fabsf(nf.getNotchFreq() - _param_imu_gyro_nf1_frq.get()) > 0.01f);
+		const bool nf_bw_changed   = (fabsf(nf.getBandwidth() - _param_imu_gyro_nf1_bw.get()) > 0.01f);
 
-void VehicleAngularVelocity::ParametersUpdate(bool force)
-{
-	// Check if parameters have changed
-	if (_parameter_update_sub.updated() || force) {
-		// clear update
-		parameter_update_s param_update;
-		_parameter_update_sub.copy(&param_update);
-
-		const bool nf0_enabled_prev = (_param_imu_gyro_nf0_frq.get() > 0.f) && (_param_imu_gyro_nf0_bw.get() > 0.f);
-		const bool nf1_enabled_prev = (_param_imu_gyro_nf1_frq.get() > 0.f) && (_param_imu_gyro_nf1_bw.get() > 0.f);
-
-		updateParams();
-
-		const bool nf0_enabled = (_param_imu_gyro_nf0_frq.get() > 0.f) && (_param_imu_gyro_nf0_bw.get() > 0.f);
-		const bool nf1_enabled = (_param_imu_gyro_nf1_frq.get() > 0.f) && (_param_imu_gyro_nf1_bw.get() > 0.f);
-
-		_calibration.ParametersUpdate();
-
-		// IMU_GYRO_RATEMAX
-		if (_param_imu_gyro_ratemax.get() <= 0) {
-			const int32_t imu_gyro_ratemax = _param_imu_gyro_ratemax.get();
-			_param_imu_gyro_ratemax.reset();
-			PX4_WARN("IMU_GYRO_RATEMAX invalid (%" PRId32 "), resetting to default %" PRId32 ")", imu_gyro_ratemax,
-				 _param_imu_gyro_ratemax.get());
+		if ((nf1_enabled_prev != nf1_enabled) || (nf1_enabled && (nf_freq_changed || nf_bw_changed))) {
+			_reset_filters = true;
+			break;
 		}
+	}
 
-		// constrain IMU_GYRO_RATEMAX 50-10,000 Hz
-		const int32_t imu_gyro_ratemax = constrain(_param_imu_gyro_ratemax.get(), (int32_t)50, (int32_t)10'000);
-
-		if (imu_gyro_ratemax != _param_imu_gyro_ratemax.get()) {
-			PX4_WARN("IMU_GYRO_RATEMAX updated %" PRId32 " -> %" PRIu32, _param_imu_gyro_ratemax.get(), imu_gyro_ratemax);
-			_param_imu_gyro_ratemax.set(imu_gyro_ratemax);
-			_param_imu_gyro_ratemax.commit_no_notification();
-		}
-
-		// gyro low pass cutoff frequency changed
-		for (auto &lp : _lp_filter_velocity) {
-			if (fabsf(lp.get_cutoff_freq() - _param_imu_gyro_cutoff.get()) > 0.01f) {
-				_reset_filters = true;
-				break;
-			}
-		}
-
-		// gyro notch filter 0 frequency or bandwidth changed
-		for (auto &nf : _notch_filter0_velocity) {
-			const bool nf_freq_changed = (fabsf(nf.getNotchFreq() - _param_imu_gyro_nf0_frq.get()) > 0.01f);
-			const bool nf_bw_changed   = (fabsf(nf.getBandwidth() - _param_imu_gyro_nf0_bw.get()) > 0.01f);
-
-			if ((nf0_enabled_prev != nf0_enabled) || (nf0_enabled && (nf_freq_changed || nf_bw_changed))) {
-				_reset_filters = true;
-				break;
-			}
-		}
-
-		// gyro notch filter 1 frequency or bandwidth changed
-		for (auto &nf : _notch_filter1_velocity) {
-			const bool nf_freq_changed = (fabsf(nf.getNotchFreq() - _param_imu_gyro_nf1_frq.get()) > 0.01f);
-			const bool nf_bw_changed   = (fabsf(nf.getBandwidth() - _param_imu_gyro_nf1_bw.get()) > 0.01f);
-
-			if ((nf1_enabled_prev != nf1_enabled) || (nf1_enabled && (nf_freq_changed || nf_bw_changed))) {
-				_reset_filters = true;
-				break;
-			}
-		}
-
-		// gyro derivative low pass cutoff changed
-		for (auto &lp : _lp_filter_acceleration) {
-			if (fabsf(lp.getCutoffFreq() - _param_imu_dgyro_cutoff.get()) > 0.01f) {
-				_reset_filters = true;
-				break;
-			}
+	// gyro derivative low pass cutoff changed
+	for (auto &lp : _lp_filter_acceleration) {
+		if (fabsf(lp.getCutoffFreq() - _param_imu_dgyro_cutoff.get()) > 0.01f) {
+			_reset_filters = true;
+			break;
 		}
+	}
 
 #if !defined(CONSTRAINED_FLASH)
 
-		if (_param_imu_gyro_dnf_en.get() & DynamicNotch::EscRpm) {
+	if (_param_imu_gyro_dnf_en.get() & DynamicNotch::EscRpm) {
 
-			const int32_t esc_rpm_harmonics = math::constrain(_param_imu_gyro_dnf_hmc.get(), (int32_t)1, (int32_t)10);
+		const int32_t esc_rpm_harmonics = math::constrain(_param_imu_gyro_dnf_hmc.get(), (int32_t)1, (int32_t)10);
 
-			if (_dynamic_notch_filter_esc_rpm && (esc_rpm_harmonics != _esc_rpm_harmonics)) {
-				delete[] _dynamic_notch_filter_esc_rpm;
-				_dynamic_notch_filter_esc_rpm = nullptr;
-				_esc_rpm_harmonics = 0;
-			}
+		if (_dynamic_notch_filter_esc_rpm && (esc_rpm_harmonics != _esc_rpm_harmonics)) {
+			delete[] _dynamic_notch_filter_esc_rpm;
+			_dynamic_notch_filter_esc_rpm = nullptr;
+			_esc_rpm_harmonics = 0;
+		}
 
-			if (_dynamic_notch_filter_esc_rpm == nullptr) {
+		if (_dynamic_notch_filter_esc_rpm == nullptr) {
 
-				_dynamic_notch_filter_esc_rpm = new NotchFilterHarmonic[esc_rpm_harmonics];
+			_dynamic_notch_filter_esc_rpm = new NotchFilterHarmonic[esc_rpm_harmonics];
 
-				if (_dynamic_notch_filter_esc_rpm) {
-					_esc_rpm_harmonics = esc_rpm_harmonics;
+			if (_dynamic_notch_filter_esc_rpm) {
+				_esc_rpm_harmonics = esc_rpm_harmonics;
 
-					if (_dynamic_notch_filter_esc_rpm_disable_perf == nullptr) {
-						_dynamic_notch_filter_esc_rpm_disable_perf = perf_alloc(PC_COUNT,
-								MODULE_NAME": gyro dynamic notch filter ESC RPM disable");
-					}
-
-					if (_dynamic_notch_filter_esc_rpm_init_perf == nullptr) {
-						_dynamic_notch_filter_esc_rpm_init_perf = perf_alloc(PC_COUNT,
-								MODULE_NAME": gyro dynamic notch filter ESC RPM init");
-					}
-
-					if (_dynamic_notch_filter_esc_rpm_update_perf == nullptr) {
-						_dynamic_notch_filter_esc_rpm_update_perf = perf_alloc(PC_COUNT,
-								MODULE_NAME": gyro dynamic notch filter ESC RPM update");
-					}
-
-				} else {
-					_esc_rpm_harmonics = 0;
-
-					perf_free(_dynamic_notch_filter_esc_rpm_disable_perf);
-					perf_free(_dynamic_notch_filter_esc_rpm_init_perf);
-					perf_free(_dynamic_notch_filter_esc_rpm_update_perf);
-
-					_dynamic_notch_filter_esc_rpm_disable_perf = nullptr;
-					_dynamic_notch_filter_esc_rpm_init_perf = nullptr;
-					_dynamic_notch_filter_esc_rpm_update_perf = nullptr;
+				if (_dynamic_notch_filter_esc_rpm_disable_perf == nullptr) {
+					_dynamic_notch_filter_esc_rpm_disable_perf = perf_alloc(PC_COUNT,
+							MODULE_NAME": gyro dynamic notch filter ESC RPM disable");
 				}
-			}
 
-		} else {
-			DisableDynamicNotchEscRpm();
+				if (_dynamic_notch_filter_esc_rpm_init_perf == nullptr) {
+					_dynamic_notch_filter_esc_rpm_init_perf = perf_alloc(PC_COUNT,
+							MODULE_NAME": gyro dynamic notch filter ESC RPM init");
+				}
+
+				if (_dynamic_notch_filter_esc_rpm_update_perf == nullptr) {
+					_dynamic_notch_filter_esc_rpm_update_perf = perf_alloc(PC_COUNT,
+							MODULE_NAME": gyro dynamic notch filter ESC RPM update");
+				}
+
+			} else {
+				_esc_rpm_harmonics = 0;
+
+				perf_free(_dynamic_notch_filter_esc_rpm_disable_perf);
+				perf_free(_dynamic_notch_filter_esc_rpm_init_perf);
+				perf_free(_dynamic_notch_filter_esc_rpm_update_perf);
+
+				_dynamic_notch_filter_esc_rpm_disable_perf = nullptr;
+				_dynamic_notch_filter_esc_rpm_init_perf = nullptr;
+				_dynamic_notch_filter_esc_rpm_update_perf = nullptr;
+			}
 		}
 
-		if (_param_imu_gyro_dnf_en.get() & DynamicNotch::FFT) {
-			if (_dynamic_notch_filter_fft_disable_perf == nullptr) {
-				_dynamic_notch_filter_fft_disable_perf = perf_alloc(PC_COUNT, MODULE_NAME": gyro dynamic notch filter FFT disable");
-				_dynamic_notch_filter_fft_update_perf = perf_alloc(PC_COUNT, MODULE_NAME": gyro dynamic notch filter FFT update");
-			}
+	} else {
+		DisableDynamicNotchEscRpm();
+	}
 
-		} else {
-			DisableDynamicNotchFFT();
+	if (_param_imu_gyro_dnf_en.get() & DynamicNotch::FFT) {
+		if (_dynamic_notch_filter_fft_disable_perf == nullptr) {
+			_dynamic_notch_filter_fft_disable_perf = perf_alloc(PC_COUNT, MODULE_NAME": gyro dynamic notch filter FFT disable");
+			_dynamic_notch_filter_fft_update_perf = perf_alloc(PC_COUNT, MODULE_NAME": gyro dynamic notch filter FFT update");
 		}
 
+	} else {
+		DisableDynamicNotchFFT();
+	}
+
 #endif // !CONSTRAINED_FLASH
-	}
 }
 
-Vector3f VehicleAngularVelocity::GetResetAngularVelocity() const
+Vector3f VehicleAngularVelocity::GetResetAngularVelocity(const IMU &imu) const
 {
 	if (_last_publish != 0) {
 		// angular velocity filtering is performed on raw unscaled data
 		//  start with last valid vehicle body frame angular velocity and compute equivalent raw data (for current sensor selection)
-		Vector3f angular_velocity_uncalibrated{_calibration.Uncorrect(_angular_velocity + _bias)};
+		Vector3f angular_velocity_uncalibrated{imu.gyro.calibration.Uncorrect(_angular_velocity + imu.gyro.estimated_bias)};
 
 		if (angular_velocity_uncalibrated.isAllFinite()) {
 			return angular_velocity_uncalibrated;
@@ -514,12 +303,12 @@ Vector3f VehicleAngularVelocity::GetResetAngularVelocity() const
 	return Vector3f{0.f, 0.f, 0.f};
 }
 
-Vector3f VehicleAngularVelocity::GetResetAngularAcceleration() const
+Vector3f VehicleAngularVelocity::GetResetAngularAcceleration(const IMU &imu) const
 {
 	if (_last_publish != 0) {
 		// angular acceleration filtering is performed on unscaled angular velocity data
 		//  start with last valid vehicle body frame angular acceleration and compute equivalent raw data (for current sensor selection)
-		Vector3f angular_acceleration{_calibration.rotation().I() *_angular_acceleration};
+		Vector3f angular_acceleration{imu.gyro.calibration.rotation().I() *_angular_acceleration};
 
 		if (angular_acceleration.isAllFinite()) {
 			return angular_acceleration;
@@ -781,137 +570,150 @@ float VehicleAngularVelocity::FilterAngularAcceleration(int axis, float inverse_
 	return angular_acceleration_filtered;
 }
 
-void VehicleAngularVelocity::Run()
+void VehicleAngularVelocity::update(IMU &imu)
 {
-	perf_begin(_cycle_perf);
-
-	// backup schedule
-	ScheduleDelayed(10_ms);
-
 	const hrt_abstime time_now_us = hrt_absolute_time();
 
 	// update corrections first to set _selected_sensor
-	const bool selection_updated = SensorSelectionUpdate(time_now_us);
+	const bool selection_updated = (_selected_sensor_device_id != imu.gyro.calibration.device_id());
 
-	if (selection_updated || _update_sample_rate) {
-		if (!UpdateSampleRate()) {
+	if (selection_updated) {
+
+		// TODO: parent register callback
+		{
+			// make sure non-FIFO sub is unregistered
+
+			_timestamp_sample_last = 0;
+			//_filter_sample_rate_hz = 1.f / (sensor_fifo_data.dt * 1e-6f);
+			_update_sample_rate = true;
+			_reset_filters = true;
+			_fifo_available = true;
+
+			PX4_DEBUG("selecting sensor_imu_fifo:%" PRIu8 " %" PRIu32, 0, _selected_sensor_device_id);
+		}
+
+		if (!UpdateSampleRate(1e6f / imu.gyro.mean_sample_interval_us.mean(),
+				      1e6f / imu.gyro.mean_publish_interval_us.mean())) {
 			// sensor sample rate required to run
 			perf_end(_cycle_perf);
 			return;
 		}
+
+		_selected_sensor_device_id = imu.gyro.calibration.device_id();
 	}
 
-	ParametersUpdate();
-
-	_calibration.SensorCorrectionsUpdate(selection_updated);
-	SensorBiasUpdate(selection_updated);
-
 	if (_reset_filters) {
-		ResetFilters(time_now_us);
-
-		if (_reset_filters) {
-			// not safe to run until filters configured
-			perf_end(_cycle_perf);
-			return;
-		}
+		ResetFilters(time_now_us, imu);
 	}
 
 	UpdateDynamicNotchEscRpm(time_now_us);
 	UpdateDynamicNotchFFT(time_now_us);
 
-	if (_fifo_available) {
-		// process all outstanding fifo messages
-		sensor_gyro_fifo_s sensor_fifo_data;
+}
 
-		while (_sensor_gyro_fifo_sub.update(&sensor_fifo_data)) {
-			const float inverse_dt_s = 1e6f / sensor_fifo_data.dt;
-			const int N = sensor_fifo_data.samples;
-			static constexpr int FIFO_SIZE_MAX = sizeof(sensor_fifo_data.x) / sizeof(sensor_fifo_data.x[0]);
+void VehicleAngularVelocity::updateSensorImuFifo(IMU &imu, const sensor_imu_fifo_s &sensor_fifo_data)
+{
+	perf_begin(_cycle_perf);
 
-			if ((sensor_fifo_data.dt > 0) && (N > 0) && (N <= FIFO_SIZE_MAX)) {
-				Vector3f angular_velocity_uncalibrated;
-				Vector3f angular_acceleration_uncalibrated;
+	const hrt_abstime time_now_us = hrt_absolute_time();
 
-				int16_t *raw_data_array[] {sensor_fifo_data.x, sensor_fifo_data.y, sensor_fifo_data.z};
+	update(imu);
 
-				for (int axis = 0; axis < 3; axis++) {
-					// copy raw int16 sensor samples to float array for filtering
-					float data[FIFO_SIZE_MAX];
+	UpdateDynamicNotchEscRpm(time_now_us);
+	UpdateDynamicNotchFFT(time_now_us);
 
-					for (int n = 0; n < N; n++) {
-						data[n] = sensor_fifo_data.scale * raw_data_array[axis][n];
-					}
+	const float inverse_dt_s = 1e6f / sensor_fifo_data.dt;
+	const int N = sensor_fifo_data.samples;
+	static constexpr int FIFO_SIZE_MAX = sizeof(sensor_fifo_data.gyro_x) / sizeof(sensor_fifo_data.gyro_x[0]);
 
-					// save last filtered sample
-					angular_velocity_uncalibrated(axis) = FilterAngularVelocity(axis, data, N);
-					angular_acceleration_uncalibrated(axis) = FilterAngularAcceleration(axis, inverse_dt_s, data, N);
-				}
+	if ((sensor_fifo_data.dt > 0) && (N > 0) && (N <= FIFO_SIZE_MAX)) {
+		Vector3f angular_velocity_uncalibrated;
+		Vector3f angular_acceleration_uncalibrated;
 
-				// Publish
-				if (!_sensor_gyro_fifo_sub.updated()) {
-					if (CalibrateAndPublish(sensor_fifo_data.timestamp_sample,
-								angular_velocity_uncalibrated,
-								angular_acceleration_uncalibrated)) {
+		const int16_t *raw_data_array[] {sensor_fifo_data.gyro_x, sensor_fifo_data.gyro_y, sensor_fifo_data.gyro_z};
 
-						perf_end(_cycle_perf);
-						return;
-					}
-				}
+		for (int axis = 0; axis < 3; axis++) {
+			// copy raw int16 sensor samples to float array for filtering
+			float data[FIFO_SIZE_MAX];
+
+			for (int n = 0; n < N; n++) {
+				data[n] = sensor_fifo_data.gyro_scale * raw_data_array[axis][n];
 			}
+
+			// save last filtered sample
+			angular_velocity_uncalibrated(axis) = FilterAngularVelocity(axis, data, N);
+			angular_acceleration_uncalibrated(axis) = FilterAngularAcceleration(axis, inverse_dt_s, data, N);
 		}
 
-	} else {
-		// process all outstanding messages
-		sensor_gyro_s sensor_data;
+		// Publish
+		//if (!_sensor_fifo_sub.updated()) {
+		if (hrt_elapsed_time(&sensor_fifo_data.timestamp) < 10_ms) {
+			if (CalibrateAndPublish(sensor_fifo_data.timestamp_sample, imu, angular_velocity_uncalibrated,
+						angular_acceleration_uncalibrated)) {
 
-		while (_sensor_sub.update(&sensor_data)) {
-			if (Vector3f(sensor_data.x, sensor_data.y, sensor_data.z).isAllFinite()) {
-
-				if (_timestamp_sample_last == 0 || (sensor_data.timestamp_sample <= _timestamp_sample_last)) {
-					_timestamp_sample_last = sensor_data.timestamp_sample - 1e6f / _filter_sample_rate_hz;
-				}
-
-				const float inverse_dt_s = 1.f / math::constrain(((sensor_data.timestamp_sample - _timestamp_sample_last) * 1e-6f),
-							   0.00002f, 0.02f);
-				_timestamp_sample_last = sensor_data.timestamp_sample;
-
-				Vector3f angular_velocity_uncalibrated;
-				Vector3f angular_acceleration_uncalibrated;
-
-				float raw_data_array[] {sensor_data.x, sensor_data.y, sensor_data.z};
-
-				for (int axis = 0; axis < 3; axis++) {
-					// copy sensor sample to float array for filtering
-					float data[1] {raw_data_array[axis]};
-
-					// save last filtered sample
-					angular_velocity_uncalibrated(axis) = FilterAngularVelocity(axis, data);
-					angular_acceleration_uncalibrated(axis) = FilterAngularAcceleration(axis, inverse_dt_s, data);
-				}
-
-				// Publish
-				if (!_sensor_sub.updated()) {
-					if (CalibrateAndPublish(sensor_data.timestamp_sample,
-								angular_velocity_uncalibrated,
-								angular_acceleration_uncalibrated)) {
-
-						perf_end(_cycle_perf);
-						return;
-					}
-				}
+				perf_end(_cycle_perf);
+				return;
 			}
 		}
 	}
 
-	// force reselection on timeout
-	if (time_now_us > _last_publish + 500_ms) {
-		SensorSelectionUpdate(true);
+	perf_end(_cycle_perf);
+}
+
+void VehicleAngularVelocity::updateSensorGyro(IMU &imu, const sensor_gyro_s &sensor_data)
+{
+	perf_begin(_cycle_perf);
+
+	const hrt_abstime time_now_us = hrt_absolute_time();
+
+	update(imu);
+
+	UpdateDynamicNotchEscRpm(time_now_us);
+	UpdateDynamicNotchFFT(time_now_us);
+
+	// process all outstanding messages
+
+	if (PX4_ISFINITE(sensor_data.x) && PX4_ISFINITE(sensor_data.y) && PX4_ISFINITE(sensor_data.z)) {
+
+		if (_timestamp_sample_last == 0 || (sensor_data.timestamp_sample <= _timestamp_sample_last)) {
+			_timestamp_sample_last = sensor_data.timestamp_sample - 1e6f / _filter_sample_rate_hz;
+		}
+
+		const float inverse_dt_s = 1.f / math::constrain(((sensor_data.timestamp_sample - _timestamp_sample_last) * 1e-6f),
+					   0.00002f, 0.02f);
+		_timestamp_sample_last = sensor_data.timestamp_sample;
+
+		Vector3f angular_velocity_uncalibrated;
+		Vector3f angular_acceleration_uncalibrated;
+
+		float raw_data_array[] {sensor_data.x, sensor_data.y, sensor_data.z};
+
+		for (int axis = 0; axis < 3; axis++) {
+			// copy sensor sample to float array for filtering
+			float data[1] {raw_data_array[axis]};
+
+			// save last filtered sample
+			angular_velocity_uncalibrated(axis) = FilterAngularVelocity(axis, data);
+			angular_acceleration_uncalibrated(axis) = FilterAngularAcceleration(axis, inverse_dt_s, data);
+		}
+
+		// Publish
+		//if (!_sensor_sub.updated()) {
+		if (true) {
+			if (CalibrateAndPublish(sensor_data.timestamp_sample, imu, angular_velocity_uncalibrated,
+						angular_acceleration_uncalibrated)) {
+
+				perf_end(_cycle_perf);
+				return;
+			}
+		}
 	}
 
 	perf_end(_cycle_perf);
 }
 
 bool VehicleAngularVelocity::CalibrateAndPublish(const hrt_abstime &timestamp_sample,
+		const IMU &imu,
 		const Vector3f &angular_velocity_uncalibrated,
 		const Vector3f &angular_acceleration_uncalibrated)
 {
@@ -922,11 +724,11 @@ bool VehicleAngularVelocity::CalibrateAndPublish(const hrt_abstime &timestamp_sa
 		angular_velocity.timestamp_sample = timestamp_sample;
 
 		// Angular velocity: rotate sensor frame to board, scale raw data to SI, apply calibration, and remove in-run estimated bias
-		_angular_velocity = _calibration.Correct(angular_velocity_uncalibrated) - _bias;
+		_angular_velocity = imu.gyro.calibration.Correct(angular_velocity_uncalibrated) - imu.gyro.estimated_bias;
 		_angular_velocity.copyTo(angular_velocity.xyz);
 
 		// Angular acceleration: rotate sensor frame to board, scale raw data to SI, apply any additional configured rotation
-		_angular_acceleration = _calibration.rotation() * angular_acceleration_uncalibrated;
+		_angular_acceleration = imu.gyro.calibration.rotation() * angular_acceleration_uncalibrated;
 		_angular_acceleration.copyTo(angular_velocity.xyz_derivative);
 
 		angular_velocity.timestamp = hrt_absolute_time();
@@ -944,16 +746,11 @@ bool VehicleAngularVelocity::CalibrateAndPublish(const hrt_abstime &timestamp_sa
 
 void VehicleAngularVelocity::PrintStatus()
 {
-	PX4_INFO_RAW("[vehicle_angular_velocity] selected sensor: %" PRIu32
-		     ", rate: %.1f Hz %s, estimated bias: [%.5f %.5f %.5f]\n",
-		     _calibration.device_id(), (double)_filter_sample_rate_hz, _fifo_available ? "FIFO" : "",
-		     (double)_bias(0), (double)_bias(1), (double)_bias(2));
-
-	_calibration.PrintStatus();
+	PX4_INFO_RAW("[vehicle_angular_velocity] selected sensor: %" PRIu32 ", rate: %.1f Hz %s\n",
+		     _selected_sensor_device_id, (double)_filter_sample_rate_hz, _fifo_available ? "FIFO" : "");
 
 	perf_print_counter(_cycle_perf);
 	perf_print_counter(_filter_reset_perf);
-	perf_print_counter(_selection_changed_perf);
 #if !defined(CONSTRAINED_FLASH)
 	perf_print_counter(_dynamic_notch_filter_esc_rpm_disable_perf);
 	perf_print_counter(_dynamic_notch_filter_esc_rpm_init_perf);

src/modules/sensors/vehicle_imu/VehicleAngularVelocity.hpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2019-2022 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2019-2023 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
@@ -33,28 +33,24 @@
 
 #pragma once
 
+#include "IMU.hpp"
+
 #include <containers/Bitset.hpp>
-#include <lib/sensor_calibration/Gyroscope.hpp>
 #include <lib/mathlib/math/Limits.hpp>
 #include <lib/matrix/matrix/math.hpp>
 #include <lib/mathlib/math/filter/AlphaFilter.hpp>
 #include <lib/mathlib/math/filter/LowPassFilter2p.hpp>
 #include <lib/mathlib/math/filter/NotchFilter.hpp>
+#include <lib/perf/perf_counter.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/ScheduledWorkItem.hpp>
 #include <uORB/Publication.hpp>
 #include <uORB/Subscription.hpp>
-#include <uORB/SubscriptionCallback.hpp>
 #include <uORB/topics/esc_status.h>
-#include <uORB/topics/estimator_selector_status.h>
-#include <uORB/topics/estimator_sensor_bias.h>
-#include <uORB/topics/parameter_update.h>
 #include <uORB/topics/sensor_gyro.h>
 #include <uORB/topics/sensor_gyro_fft.h>
-#include <uORB/topics/sensor_gyro_fifo.h>
-#include <uORB/topics/sensor_selection.h>
+#include <uORB/topics/sensor_imu_fifo.h>
 #include <uORB/topics/vehicle_angular_velocity.h>
 
 using namespace time_literals;
@@ -62,61 +58,49 @@ using namespace time_literals;
 namespace sensors
 {
 
-class VehicleAngularVelocity : public ModuleParams, public px4::ScheduledWorkItem
+class VehicleAngularVelocity : public ModuleParams
 {
 public:
 	VehicleAngularVelocity();
 	~VehicleAngularVelocity() override;
 
 	void PrintStatus();
-	bool Start();
-	void Stop();
+
+	void ParametersUpdate();
+
+	// IMU& ?
+	void updateSensorGyro(IMU &imu, const sensor_gyro_s &sensor_data);
+	void updateSensorImuFifo(IMU &imu, const sensor_imu_fifo_s &sensor_fifo_data);
 
 private:
-	void Run() override;
 
-	bool CalibrateAndPublish(const hrt_abstime &timestamp_sample, const matrix::Vector3f &angular_velocity_uncalibrated,
-				 const matrix::Vector3f &angular_acceleration_uncalibrated);
+	bool CalibrateAndPublish(const hrt_abstime &timestamp_sample, const IMU &imu,
+				 const matrix::Vector3f &angular_velocity_uncalibrated, const matrix::Vector3f &angular_acceleration_uncalibrated);
 
 	inline float FilterAngularVelocity(int axis, float data[], int N = 1);
 	inline float FilterAngularAcceleration(int axis, float inverse_dt_s, float data[], int N = 1);
 
 	void DisableDynamicNotchEscRpm();
 	void DisableDynamicNotchFFT();
-	void ParametersUpdate(bool force = false);
 
-	void ResetFilters(const hrt_abstime &time_now_us);
-	void SensorBiasUpdate(bool force = false);
-	bool SensorSelectionUpdate(const hrt_abstime &time_now_us, bool force = false);
+	void ResetFilters(const hrt_abstime &time_now_us, const IMU &imu);
 	void UpdateDynamicNotchEscRpm(const hrt_abstime &time_now_us, bool force = false);
 	void UpdateDynamicNotchFFT(const hrt_abstime &time_now_us, bool force = false);
-	bool UpdateSampleRate();
+	bool UpdateSampleRate(float sample_rate_hz, float publish_rate_hz);
+
+	void update(IMU &imu);
 
 	// scaled appropriately for current sensor
-	matrix::Vector3f GetResetAngularVelocity() const;
-	matrix::Vector3f GetResetAngularAcceleration() const;
+	matrix::Vector3f GetResetAngularVelocity(const IMU &imu) const;
+	matrix::Vector3f GetResetAngularAcceleration(const IMU &imu) const;
 
-	static constexpr int MAX_SENSOR_COUNT = 4;
+	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 _estimator_selector_status_sub{ORB_ID(estimator_selector_status)};
-	uORB::Subscription _estimator_sensor_bias_sub{ORB_ID(estimator_sensor_bias)};
 #if !defined(CONSTRAINED_FLASH)
 	uORB::Subscription _esc_status_sub {ORB_ID(esc_status)};
 	uORB::Subscription _sensor_gyro_fft_sub {ORB_ID(sensor_gyro_fft)};
 #endif // !CONSTRAINED_FLASH
 
-	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
-
-	uORB::SubscriptionCallbackWorkItem _sensor_selection_sub{this, ORB_ID(sensor_selection)};
-	uORB::SubscriptionCallbackWorkItem _sensor_sub{this, ORB_ID(sensor_gyro)};
-	uORB::SubscriptionCallbackWorkItem _sensor_gyro_fifo_sub{this, ORB_ID(sensor_gyro_fifo)};
-
-	calibration::Gyroscope _calibration{};
-
-	matrix::Vector3f _bias{};
-
 	matrix::Vector3f _angular_velocity{};
 	matrix::Vector3f _angular_acceleration{};
 
@@ -140,7 +124,7 @@ private:
 		FFT    = 2,
 	};
 
-	static constexpr hrt_abstime DYNAMIC_NOTCH_FITLER_TIMEOUT = 3_s;
+	static constexpr hrt_abstime DYNAMIC_NOTCH_FITLER_TIMEOUT = 1_s;
 
 	// ESC RPM
 	static constexpr int MAX_NUM_ESCS = sizeof(esc_status_s::esc) / sizeof(esc_status_s::esc[0]);
@@ -179,7 +163,6 @@ private:
 
 	perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": gyro filter")};
 	perf_counter_t _filter_reset_perf{perf_alloc(PC_COUNT, MODULE_NAME": gyro filter reset")};
-	perf_counter_t _selection_changed_perf{perf_alloc(PC_COUNT, MODULE_NAME": gyro selection changed")};
 
 	DEFINE_PARAMETERS(
 #if !defined(CONSTRAINED_FLASH)

src/modules/sensors/vehicle_imu/VehicleIMU.hpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020-2022 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2019-2023 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
@@ -33,30 +33,41 @@
 
 #pragma once
 
-#include <Integrator.hpp>
+#include "IMU.hpp"
+#include "VehicleAcceleration.hpp"
+#include "VehicleAngularVelocity.hpp"
+
+#include <include/containers/Bitset.hpp>
 
 #include <lib/mathlib/math/Limits.hpp>
-#include <lib/mathlib/math/WelfordMean.hpp>
-#include <lib/mathlib/math/WelfordMeanVector.hpp>
+
 #include <lib/matrix/matrix/math.hpp>
-#include <lib/perf/perf_counter.h>
-#include <lib/sensor_calibration/Accelerometer.hpp>
-#include <lib/sensor_calibration/Gyroscope.hpp>
+#include <lib/mathlib/math/filter/AlphaFilter.hpp>
+#include <lib/mathlib/math/filter/LowPassFilter2p.hpp>
+#include <lib/mathlib/math/filter/NotchFilter.hpp>
 #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/ScheduledWorkItem.hpp>
+
+// publications
+#include <uORB/Publication.hpp>
 #include <uORB/PublicationMulti.hpp>
+#include <uORB/topics/sensor_combined.h>
+#include <uORB/topics/sensors_status_imu.h>
+#include <uORB/topics/sensor_selection.h>
+
+// subscriptions
 #include <uORB/Subscription.hpp>
-#include <uORB/SubscriptionMultiArray.hpp>
 #include <uORB/SubscriptionCallback.hpp>
+#include <uORB/SubscriptionMultiArray.hpp>
+#include <uORB/topics/estimator_selector_status.h>
 #include <uORB/topics/estimator_sensor_bias.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/sensor_accel.h>
 #include <uORB/topics/sensor_gyro.h>
+#include <uORB/topics/sensor_imu_fifo.h>
 #include <uORB/topics/vehicle_control_mode.h>
-#include <uORB/topics/vehicle_imu.h>
-#include <uORB/topics/vehicle_imu_status.h>
 
 using namespace time_literals;
 
@@ -66,140 +77,129 @@ namespace sensors
 class VehicleIMU : public ModuleParams, public px4::ScheduledWorkItem
 {
 public:
-	VehicleIMU() = delete;
-	VehicleIMU(int instance, uint8_t accel_index, uint8_t gyro_index, const px4::wq_config_t &config);
-
+	VehicleIMU();
 	~VehicleIMU() override;
 
+	void PrintStatus();
 	bool Start();
 	void Stop();
 
-	void PrintStatus();
-
 private:
-	bool ParametersUpdate(bool force = false);
-	bool Publish();
 	void Run() override;
 
-	bool UpdateAccel();
-	bool UpdateGyro();
+	bool ParametersUpdate(bool force = false);
 
-	void UpdateIntegratorConfiguration();
+	int8_t findAccelInstance(uint32_t device_id);
 
-	inline void UpdateAccelVibrationMetrics(const matrix::Vector3f &acceleration);
-	inline void UpdateGyroVibrationMetrics(const matrix::Vector3f &angular_velocity);
+	bool PublishImu(sensors::IMU &imu, const matrix::Vector3f &delta_angle, const uint16_t delta_angle_dt,
+			const matrix::Vector3f &delta_velocity, const uint16_t delta_velocity_dt);
+	void PublishSensorsStatusIMU();
+
+	void SensorBiasUpdate();
+	bool SensorSelectionUpdate(bool force = false);
+	void UpdateSensorImuFifo(uint8_t sensor_instance);
+	void UpdateSensorAccel(uint8_t sensor_instance);
+	void UpdateSensorGyro(uint8_t sensor_instance);
 
 	void SensorCalibrationUpdate();
 	void SensorCalibrationSaveAccel();
 	void SensorCalibrationSaveGyro();
 
+	void updateGyroCalibration();
+
 	// return the square of two floating point numbers
 	static constexpr float sq(float var) { return var * var; }
 
-	uORB::PublicationMulti<vehicle_imu_s> _vehicle_imu_pub{ORB_ID(vehicle_imu)};
-	uORB::PublicationMulti<vehicle_imu_status_s> _vehicle_imu_status_pub{ORB_ID(vehicle_imu_status)};
+	static constexpr int MAX_SENSOR_COUNT = 4;
 
-	static constexpr hrt_abstime kIMUStatusPublishingInterval{100_ms};
-
-	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
-
-	// Used to check, save and use learned magnetometer biases
+	uORB::Subscription _estimator_selector_status_sub{ORB_ID(estimator_selector_status)};
 	uORB::SubscriptionMultiArray<estimator_sensor_bias_s> _estimator_sensor_bias_subs{ORB_ID::estimator_sensor_bias};
-
-	uORB::Subscription _sensor_accel_sub;
-	uORB::SubscriptionCallbackWorkItem _sensor_gyro_sub;
-
+	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
+	uORB::SubscriptionCallbackWorkItem _sensor_selection_sub{this, ORB_ID(sensor_selection)};
 	uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
 
-	calibration::Accelerometer _accel_calibration{};
-	calibration::Gyroscope _gyro_calibration{};
-
-	sensors::Integrator       _accel_integrator{};
-	sensors::IntegratorConing _gyro_integrator{};
-
-	uint32_t _imu_integration_interval_us{5000};
-
-	hrt_abstime _accel_timestamp_sample_last{0};
-	hrt_abstime _gyro_timestamp_sample_last{0};
-	hrt_abstime _gyro_timestamp_last{0};
-
-	math::WelfordMeanVector<float, 3> _raw_accel_mean{};
-	math::WelfordMeanVector<float, 3> _raw_gyro_mean{};
-
-	math::WelfordMean<float> _accel_mean_interval_us{};
-	math::WelfordMean<float> _accel_fifo_mean_interval_us{};
-
-	math::WelfordMean<float> _gyro_mean_interval_us{};
-	math::WelfordMean<float> _gyro_fifo_mean_interval_us{};
-
-	math::WelfordMean<float> _gyro_update_latency_mean_us{};
-	math::WelfordMean<float> _gyro_publish_latency_mean_us{};
-
-	float _accel_interval_us{NAN};
-	float _gyro_interval_us{NAN};
-
-	unsigned _accel_last_generation{0};
-	unsigned _gyro_last_generation{0};
-
-	float _accel_temperature_sum{NAN};
-	float _gyro_temperature_sum{NAN};
-
-	int _accel_temperature_sum_count{0};
-	int _gyro_temperature_sum_count{0};
-
-	matrix::Vector3f _acceleration_prev{};     // acceleration from the previous IMU measurement for vibration metrics
-	matrix::Vector3f _angular_velocity_prev{}; // angular velocity from the previous IMU measurement for vibration metrics
-
-	vehicle_imu_status_s _status{};
-
-	float _coning_norm_accum{0};
-	float _coning_norm_accum_total_time_s{0};
-
-	uint8_t     _delta_angle_clipping{0};
-	uint8_t     _delta_velocity_clipping{0};
-
-	hrt_abstime _last_accel_clipping_notify_time{0};
-	hrt_abstime _last_gyro_clipping_notify_time{0};
-
-	uint64_t    _last_accel_clipping_notify_total_count{0};
-	uint64_t    _last_gyro_clipping_notify_total_count{0};
-
-	orb_advert_t _mavlink_log_pub{nullptr};
-
-	uint32_t _backup_schedule_timeout_us{20000};
-
-	bool _data_gap{false};
-	bool _update_integrator_config{true};
-	bool _intervals_configured{false};
-	bool _publish_status{true};
-
-	const uint8_t _instance;
-
-	bool _armed{false};
-
-	bool _accel_cal_available{false};
-	bool _gyro_cal_available{false};
-
-	struct InFlightCalibration {
-		matrix::Vector3f offset{};
-		matrix::Vector3f bias_variance{};
-		bool valid{false};
+	uORB::SubscriptionCallbackWorkItem _sensor_imu_fifo_subs[MAX_SENSOR_COUNT] {
+		{this, ORB_ID::sensor_imu_fifo, 0},
+		{this, ORB_ID::sensor_imu_fifo, 1},
+		{this, ORB_ID::sensor_imu_fifo, 2},
+		{this, ORB_ID::sensor_imu_fifo, 3}
 	};
 
-	InFlightCalibration _accel_learned_calibration[ORB_MULTI_MAX_INSTANCES] {};
-	InFlightCalibration _gyro_learned_calibration[ORB_MULTI_MAX_INSTANCES] {};
+	uORB::SubscriptionCallbackWorkItem _sensor_accel_subs[MAX_SENSOR_COUNT] {
+		{this, ORB_ID::sensor_accel, 0},
+		{this, ORB_ID::sensor_accel, 1},
+		{this, ORB_ID::sensor_accel, 2},
+		{this, ORB_ID::sensor_accel, 3}
+	};
+
+	uORB::SubscriptionCallbackWorkItem _sensor_gyro_subs[MAX_SENSOR_COUNT] {
+		{this, ORB_ID::sensor_gyro, 0},
+		{this, ORB_ID::sensor_gyro, 1},
+		{this, ORB_ID::sensor_gyro, 2},
+		{this, ORB_ID::sensor_gyro, 3}
+	};
+
+	IMU _imus[MAX_SENSOR_COUNT] {};
+
+
+
+	// struct SensorData {
+	// 	DataValidatorGroup voter{1};
+	// 	unsigned int last_failover_count{0};
+	// 	int32_t priority[MAX_SENSOR_COUNT] {};
+	// 	int32_t priority_configured[MAX_SENSOR_COUNT] {};
+	// 	uint8_t last_best_vote{0}; /**< index of the latest best vote */
+	// 	uint8_t subscription_count{0};
+	// 	bool advertised[MAX_SENSOR_COUNT] {false, false, false};
+	// };
+
+	// SensorData _accel{};
+	// SensorData _gyro{};
+
+	hrt_abstime _last_error_message{0};
+	orb_advert_t _mavlink_log_pub{nullptr};
+
+	matrix::Vector3f _accel_diff[MAX_SENSOR_COUNT] {};		/**< filtered accel differences between IMU units (m/s/s) */
+	matrix::Vector3f _gyro_diff[MAX_SENSOR_COUNT] {};			/**< filtered gyro differences between IMU uinits (rad/s) */
+
+
+
+	// TODO:
+	uORB::Publication<sensor_combined_s>    _sensor_combined_pub{ORB_ID(sensor_combined)}; // legacy
+	uORB::Publication<sensor_selection_s>   _sensor_selection_pub{ORB_ID(sensor_selection)};
+	uORB::Publication<sensors_status_imu_s> _sensors_status_imu_pub{ORB_ID(sensors_status_imu)};
+
+	VehicleAcceleration _vehicle_acceleration{};
+	VehicleAngularVelocity _vehicle_angular_velocity{};
+
+	uint32_t _selected_accel_device_id{0};
+	uint32_t _selected_gyro_device_id{0};
+	int8_t _selected_imu_index{-1};
+
+
+	// calibration
+	hrt_abstime _last_calibration_update{0};
 
 	static constexpr hrt_abstime INFLIGHT_CALIBRATION_QUIET_PERIOD_US{30_s};
 
 	hrt_abstime _in_flight_calibration_check_timestamp_last{0};
+	bool _accel_cal_available{false};
+	bool _gyro_cal_available{false};
 
-	perf_counter_t _accel_generation_gap_perf{perf_alloc(PC_COUNT, MODULE_NAME": accel data gap")};
-	perf_counter_t _gyro_generation_gap_perf{perf_alloc(PC_COUNT, MODULE_NAME": gyro data gap")};
+
+	bool _armed{false};
+
+
+	perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": IMU update")};
+	perf_counter_t _selection_changed_perf{perf_alloc(PC_COUNT, MODULE_NAME": IMU selection changed")};
 
 	DEFINE_PARAMETERS(
+		(ParamBool<px4::params::SENS_IMU_AUTOCAL>) _param_sens_imu_autocal,
+		(ParamBool<px4::params::SENS_IMU_MODE>) _param_sens_imu_mode,
 		(ParamInt<px4::params::IMU_INTEG_RATE>) _param_imu_integ_rate,
-		(ParamBool<px4::params::SENS_IMU_AUTOCAL>) _param_sens_imu_autocal
+		(ParamInt<px4::params::IMU_GYRO_RATEMAX>) _param_imu_gyro_ratemax
 	)
+
 };
 
 } // namespace sensors

src/modules/sensors/vehicle_imu/imu_gyro_parameters.c

@@ -99,7 +99,6 @@ PARAM_DEFINE_FLOAT(IMU_GYRO_NF1_FRQ, 0.0f);
 */
 PARAM_DEFINE_FLOAT(IMU_GYRO_NF1_BW, 20.0f);
 
-
 /**
 * Low pass filter cutoff frequency for gyro
 *

src/modules/sensors/vehicle_imu/imu_parameters.c

@@ -31,6 +31,20 @@
  *
  ****************************************************************************/
 
+/**
+* Low pass filter cutoff frequency for accel
+*
+* The cutoff frequency for the 2nd order butterworth filter on the primary accelerometer.
+* This only affects the signal sent to the controllers, not the estimators. 0 disables the filter.
+*
+* @min 0
+* @max 1000
+* @unit Hz
+* @reboot_required true
+* @group Sensors
+*/
+PARAM_DEFINE_FLOAT(IMU_ACCEL_CUTOFF, 30.0f);
+
 /**
 * IMU integration rate.
 *
@@ -60,3 +74,15 @@ PARAM_DEFINE_INT32(IMU_INTEG_RATE, 200);
  * @group Sensors
  */
 PARAM_DEFINE_INT32(SENS_IMU_AUTOCAL, 1);
+
+/**
+ * Sensors hub IMU mode
+ *
+ * @value 0 Disabled
+ * @value 1 Publish primary IMU selection
+ *
+ * @category system
+ * @reboot_required true
+ * @group Sensors
+ */
+PARAM_DEFINE_INT32(SENS_IMU_MODE, 1);

src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp

@@ -111,6 +111,31 @@ bool VehicleMagnetometer::ParametersUpdate(bool force)
 			_param_cal_mag_sides.commit();
 		}
 
+
+		// 1. mark all existing sensor calibrations active even if sensor is missing
+		//    this preserves the calibration in the event of a parameter export while the sensor is missing
+		// 2. ensure calibration slots are active for the number of sensors currently available
+		//    this to done to eliminate differences in the active set of parameters before and after sensor calibration
+		for (uint8_t i = 0; i < MAX_SENSOR_COUNT; i++) {
+			// sensor_mag
+			{
+				uint32_t device_id_mag = calibration::GetCalibrationParamInt32("MAG",  "ID", i);
+
+				if (device_id_mag != 0) {
+					calibration::Magnetometer mag_cal(device_id_mag);
+				}
+
+				uORB::SubscriptionData<sensor_mag_s> sensor_mag_sub{ORB_ID(sensor_mag), i};
+
+				if (sensor_mag_sub.advertised() && (sensor_mag_sub.get().device_id != 0)) {
+					calibration::Magnetometer cal;
+					cal.set_calibration_index(i);
+					cal.ParametersLoad();
+				}
+			}
+		}
+
+
 		// Mag compensation type
 		MagCompensationType mag_comp_typ = static_cast<MagCompensationType>(_param_mag_comp_typ.get());
 
@@ -143,6 +168,7 @@ bool VehicleMagnetometer::ParametersUpdate(bool force)
 
 
 		if (!_armed) {
+
 			bool calibration_updated = false;
 
 			// update mag priority (CAL_MAGx_PRIO)

src/modules/sensors/voted_sensors_update.cpp

@@ -1,497 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2016-2022 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.
- *
- ****************************************************************************/
-
-/**
- * @file voted_sensors_update.cpp
- *
- * @author Beat Kueng <beat-kueng@gmx.net>
- */
-
-#include "voted_sensors_update.h"
-
-#include <lib/sensor_calibration/Utilities.hpp>
-#include <lib/geo/geo.h>
-#include <lib/systemlib/mavlink_log.h>
-#include <uORB/Subscription.hpp>
-
-using namespace sensors;
-using namespace matrix;
-using namespace time_literals;
-
-VotedSensorsUpdate::VotedSensorsUpdate(bool hil_enabled,
-				       uORB::SubscriptionCallbackWorkItem(&vehicle_imu_sub)[MAX_SENSOR_COUNT]) :
-	ModuleParams(nullptr),
-	_vehicle_imu_sub(vehicle_imu_sub),
-	_hil_enabled(hil_enabled)
-{
-	_sensor_selection_pub.advertise();
-	_sensors_status_imu_pub.advertise();
-
-	if (_hil_enabled) { // HIL has less accurate timing so increase the timeouts a bit
-		_gyro.voter.set_timeout(500000);
-		_accel.voter.set_timeout(500000);
-	}
-
-	initializeSensors();
-
-	parametersUpdate();
-}
-
-void VotedSensorsUpdate::initializeSensors()
-{
-	initSensorClass(_gyro, MAX_SENSOR_COUNT);
-	initSensorClass(_accel, MAX_SENSOR_COUNT);
-}
-
-void VotedSensorsUpdate::parametersUpdate()
-{
-	_parameter_update = true;
-
-	updateParams();
-
-	// run through all IMUs
-	for (uint8_t uorb_index = 0; uorb_index < MAX_SENSOR_COUNT; uorb_index++) {
-		uORB::SubscriptionData<vehicle_imu_s> imu{ORB_ID(vehicle_imu), uorb_index};
-		imu.update();
-
-		if (imu.advertised() && (imu.get().timestamp != 0)
-		    && (imu.get().accel_device_id != 0) && (imu.get().gyro_device_id != 0)) {
-
-			// find corresponding configured accel priority
-			int8_t accel_cal_index = calibration::FindCurrentCalibrationIndex("ACC", imu.get().accel_device_id);
-
-			if (accel_cal_index >= 0) {
-				// found matching CAL_ACCx_PRIO
-				int32_t accel_priority_old = _accel.priority_configured[uorb_index];
-
-				_accel.priority_configured[uorb_index] = calibration::GetCalibrationParamInt32("ACC", "PRIO", accel_cal_index);
-
-				if (accel_priority_old != _accel.priority_configured[uorb_index]) {
-					if (_accel.priority_configured[uorb_index] == 0) {
-						// disabled
-						_accel.priority[uorb_index] = 0;
-
-					} else {
-						// change relative priority to incorporate any sensor faults
-						int priority_change = _accel.priority_configured[uorb_index] - accel_priority_old;
-						_accel.priority[uorb_index] = math::constrain(_accel.priority[uorb_index] + priority_change, static_cast<int32_t>(1),
-									      static_cast<int32_t>(100));
-					}
-				}
-			}
-
-			// find corresponding configured gyro priority
-			int8_t gyro_cal_index = calibration::FindCurrentCalibrationIndex("GYRO", imu.get().gyro_device_id);
-
-			if (gyro_cal_index >= 0) {
-				// found matching CAL_GYROx_PRIO
-				int32_t gyro_priority_old = _gyro.priority_configured[uorb_index];
-
-				_gyro.priority_configured[uorb_index] = calibration::GetCalibrationParamInt32("GYRO", "PRIO", gyro_cal_index);
-
-				if (gyro_priority_old != _gyro.priority_configured[uorb_index]) {
-					if (_gyro.priority_configured[uorb_index] == 0) {
-						// disabled
-						_gyro.priority[uorb_index] = 0;
-
-					} else {
-						// change relative priority to incorporate any sensor faults
-						int priority_change = _gyro.priority_configured[uorb_index] - gyro_priority_old;
-						_gyro.priority[uorb_index] = math::constrain(_gyro.priority[uorb_index] + priority_change, static_cast<int32_t>(1),
-									     static_cast<int32_t>(100));
-					}
-				}
-			}
-		}
-	}
-}
-
-void VotedSensorsUpdate::imuPoll(struct sensor_combined_s &raw)
-{
-	const hrt_abstime time_now_us = hrt_absolute_time();
-
-	for (int uorb_index = 0; uorb_index < MAX_SENSOR_COUNT; uorb_index++) {
-		vehicle_imu_s imu_report;
-
-		if ((_accel.priority[uorb_index] > 0) && (_gyro.priority[uorb_index] > 0)
-		    && _vehicle_imu_sub[uorb_index].update(&imu_report)) {
-
-			// copy corresponding vehicle_imu_status for accel & gyro error counts
-			vehicle_imu_status_s imu_status{};
-			_vehicle_imu_status_subs[uorb_index].copy(&imu_status);
-
-			_accel_device_id[uorb_index] = imu_report.accel_device_id;
-			_gyro_device_id[uorb_index] = imu_report.gyro_device_id;
-
-			// convert the delta velocities to an equivalent acceleration
-			const float accel_dt_inv = 1.e6f / (float)imu_report.delta_velocity_dt;
-			Vector3f accel_data = Vector3f{imu_report.delta_velocity} * accel_dt_inv;
-
-
-			// convert the delta angles to an equivalent angular rate
-			const float gyro_dt_inv = 1.e6f / (float)imu_report.delta_angle_dt;
-			Vector3f gyro_rate = Vector3f{imu_report.delta_angle} * gyro_dt_inv;
-
-			_last_sensor_data[uorb_index].timestamp = imu_report.timestamp_sample;
-			_last_sensor_data[uorb_index].accelerometer_m_s2[0] = accel_data(0);
-			_last_sensor_data[uorb_index].accelerometer_m_s2[1] = accel_data(1);
-			_last_sensor_data[uorb_index].accelerometer_m_s2[2] = accel_data(2);
-			_last_sensor_data[uorb_index].accelerometer_integral_dt = imu_report.delta_velocity_dt;
-			_last_sensor_data[uorb_index].accelerometer_clipping = imu_report.delta_velocity_clipping;
-			_last_sensor_data[uorb_index].gyro_rad[0] = gyro_rate(0);
-			_last_sensor_data[uorb_index].gyro_rad[1] = gyro_rate(1);
-			_last_sensor_data[uorb_index].gyro_rad[2] = gyro_rate(2);
-			_last_sensor_data[uorb_index].gyro_integral_dt = imu_report.delta_angle_dt;
-			_last_sensor_data[uorb_index].gyro_clipping = imu_report.delta_angle_clipping;
-			_last_sensor_data[uorb_index].accel_calibration_count = imu_report.accel_calibration_count;
-			_last_sensor_data[uorb_index].gyro_calibration_count = imu_report.gyro_calibration_count;
-
-			_last_accel_timestamp[uorb_index] = imu_report.timestamp_sample;
-
-			_accel.voter.put(uorb_index, imu_report.timestamp, _last_sensor_data[uorb_index].accelerometer_m_s2,
-					 imu_status.accel_error_count, _accel.priority[uorb_index]);
-
-			_gyro.voter.put(uorb_index, imu_report.timestamp, _last_sensor_data[uorb_index].gyro_rad,
-					imu_status.gyro_error_count, _gyro.priority[uorb_index]);
-		}
-	}
-
-	// find the best sensor
-	int accel_best_index = _accel.last_best_vote;
-	int gyro_best_index = _gyro.last_best_vote;
-
-	if (!_parameter_update) {
-		// update current accel/gyro selection, skipped on cycles where parameters update
-		_accel.voter.get_best(time_now_us, &accel_best_index);
-		_gyro.voter.get_best(time_now_us, &gyro_best_index);
-
-		if (!_param_sens_imu_mode.get() && ((_selection.timestamp != 0) || (_sensor_selection_sub.updated()))) {
-			// use sensor_selection to find best
-			if (_sensor_selection_sub.update(&_selection)) {
-				// reset inconsistency checks against primary
-				for (int sensor_index = 0; sensor_index < MAX_SENSOR_COUNT; sensor_index++) {
-					_accel_diff[sensor_index].zero();
-					_gyro_diff[sensor_index].zero();
-				}
-			}
-
-			for (int i = 0; i < MAX_SENSOR_COUNT; i++) {
-				if ((_accel_device_id[i] != 0) && (_accel_device_id[i] == _selection.accel_device_id)) {
-					accel_best_index = i;
-				}
-
-				if ((_gyro_device_id[i] != 0) && (_gyro_device_id[i] == _selection.gyro_device_id)) {
-					gyro_best_index = i;
-				}
-			}
-
-		} else {
-			// use sensor voter to find best if SENS_IMU_MODE is enabled or ORB_ID(sensor_selection) has never published
-			checkFailover(_accel, "Accel", events::px4::enums::sensor_type_t::accel);
-			checkFailover(_gyro, "Gyro", events::px4::enums::sensor_type_t::gyro);
-		}
-	}
-
-	// write data for the best sensor to output variables
-	if ((accel_best_index >= 0) && (accel_best_index < MAX_SENSOR_COUNT) && (_accel_device_id[accel_best_index] != 0)
-	    && (gyro_best_index >= 0) && (gyro_best_index < MAX_SENSOR_COUNT) && (_gyro_device_id[gyro_best_index] != 0)) {
-
-		raw.timestamp = _last_sensor_data[gyro_best_index].timestamp;
-		memcpy(&raw.accelerometer_m_s2, &_last_sensor_data[accel_best_index].accelerometer_m_s2,
-		       sizeof(raw.accelerometer_m_s2));
-		memcpy(&raw.gyro_rad, &_last_sensor_data[gyro_best_index].gyro_rad, sizeof(raw.gyro_rad));
-
-		raw.accelerometer_integral_dt = _last_sensor_data[accel_best_index].accelerometer_integral_dt;
-		raw.accelerometer_clipping    = _last_sensor_data[accel_best_index].accelerometer_clipping;
-		raw.accel_calibration_count   = _last_sensor_data[accel_best_index].accel_calibration_count;
-
-		raw.gyro_integral_dt          = _last_sensor_data[gyro_best_index].gyro_integral_dt;
-		raw.gyro_clipping             = _last_sensor_data[gyro_best_index].gyro_clipping;
-		raw.gyro_calibration_count    = _last_sensor_data[gyro_best_index].gyro_calibration_count;
-
-		if ((accel_best_index != _accel.last_best_vote) || (_selection.accel_device_id != _accel_device_id[accel_best_index])) {
-			_accel.last_best_vote = (uint8_t)accel_best_index;
-			_selection.accel_device_id = _accel_device_id[accel_best_index];
-			_selection_changed = true;
-		}
-
-		if ((_gyro.last_best_vote != gyro_best_index) || (_selection.gyro_device_id != _gyro_device_id[gyro_best_index])) {
-			_gyro.last_best_vote = (uint8_t)gyro_best_index;
-			_selection.gyro_device_id = _gyro_device_id[gyro_best_index];
-			_selection_changed = true;
-
-			// clear all registered callbacks
-			for (auto &sub : _vehicle_imu_sub) {
-				sub.unregisterCallback();
-			}
-
-			for (int i = 0; i < MAX_SENSOR_COUNT; i++) {
-				vehicle_imu_s report{};
-
-				if (_vehicle_imu_sub[i].copy(&report)) {
-					if ((report.gyro_device_id != 0) && (report.gyro_device_id == _gyro_device_id[gyro_best_index])) {
-						_vehicle_imu_sub[i].registerCallback();
-					}
-				}
-			}
-		}
-	}
-
-	// publish sensor selection if changed
-	if (_param_sens_imu_mode.get() || (_selection.timestamp == 0)) {
-		if (_selection_changed) {
-			// don't publish until selected IDs are valid
-			if (_selection.accel_device_id > 0 && _selection.gyro_device_id > 0) {
-				_selection.timestamp = hrt_absolute_time();
-				_sensor_selection_pub.publish(_selection);
-				_selection_changed = false;
-			}
-
-			for (int sensor_index = 0; sensor_index < MAX_SENSOR_COUNT; sensor_index++) {
-				_accel_diff[sensor_index].zero();
-				_gyro_diff[sensor_index].zero();
-			}
-		}
-	}
-}
-
-bool VotedSensorsUpdate::checkFailover(SensorData &sensor, const char *sensor_name,
-				       events::px4::enums::sensor_type_t sensor_type)
-{
-	if (sensor.last_failover_count != sensor.voter.failover_count() && !_hil_enabled) {
-
-		uint32_t flags = sensor.voter.failover_state();
-		int failover_index = sensor.voter.failover_index();
-
-		if (flags == DataValidator::ERROR_FLAG_NO_ERROR) {
-			if (failover_index != -1) {
-				// we switched due to a non-critical reason. No need to panic.
-				PX4_INFO("%s sensor switch from #%i", sensor_name, failover_index);
-			}
-
-		} else {
-			if (failover_index != -1) {
-
-				const hrt_abstime now = hrt_absolute_time();
-
-				if (now - _last_error_message > 3_s) {
-					mavlink_log_emergency(&_mavlink_log_pub, "%s #%i fail: %s%s%s%s%s!\t",
-							      sensor_name,
-							      failover_index,
-							      ((flags & DataValidator::ERROR_FLAG_NO_DATA) ? " OFF" : ""),
-							      ((flags & DataValidator::ERROR_FLAG_STALE_DATA) ? " STALE" : ""),
-							      ((flags & DataValidator::ERROR_FLAG_TIMEOUT) ? " TIMEOUT" : ""),
-							      ((flags & DataValidator::ERROR_FLAG_HIGH_ERRCOUNT) ? " ERR CNT" : ""),
-							      ((flags & DataValidator::ERROR_FLAG_HIGH_ERRDENSITY) ? " ERR DNST" : ""));
-
-					events::px4::enums::sensor_failover_reason_t failover_reason{};
-
-					if (flags & DataValidator::ERROR_FLAG_NO_DATA) { failover_reason = failover_reason | events::px4::enums::sensor_failover_reason_t::no_data; }
-
-					if (flags & DataValidator::ERROR_FLAG_STALE_DATA) { failover_reason = failover_reason | events::px4::enums::sensor_failover_reason_t::stale_data; }
-
-					if (flags & DataValidator::ERROR_FLAG_TIMEOUT) { failover_reason = failover_reason | events::px4::enums::sensor_failover_reason_t::timeout; }
-
-					if (flags & DataValidator::ERROR_FLAG_HIGH_ERRCOUNT) { failover_reason = failover_reason | events::px4::enums::sensor_failover_reason_t::high_error_count; }
-
-					if (flags & DataValidator::ERROR_FLAG_HIGH_ERRDENSITY) { failover_reason = failover_reason | events::px4::enums::sensor_failover_reason_t::high_error_density; }
-
-					/* EVENT
-					 * @description
-					 * Land immediately and check the system.
-					 */
-					events::send<events::px4::enums::sensor_type_t, uint8_t, events::px4::enums::sensor_failover_reason_t>(
-						events::ID("sensor_failover"), events::Log::Emergency, "{1} sensor #{2} failure: {3}", sensor_type, failover_index,
-						failover_reason);
-
-					_last_error_message = now;
-				}
-
-				// reduce priority of failed sensor to the minimum
-				sensor.priority[failover_index] = 1;
-			}
-		}
-
-		sensor.last_failover_count = sensor.voter.failover_count();
-		return true;
-	}
-
-	return false;
-}
-
-void VotedSensorsUpdate::initSensorClass(SensorData &sensor_data, uint8_t sensor_count_max)
-{
-	bool added = false;
-	int max_sensor_index = -1;
-
-	for (unsigned i = 0; i < sensor_count_max; i++) {
-
-		max_sensor_index = i;
-
-		if (!sensor_data.advertised[i] && sensor_data.subscription[i].advertised()) {
-			sensor_data.advertised[i] = true;
-			sensor_data.priority[i] = DEFAULT_PRIORITY;
-			sensor_data.priority_configured[i] = DEFAULT_PRIORITY;
-
-			if (i > 0) {
-				/* the first always exists, but for each further sensor, add a new validator */
-				if (sensor_data.voter.add_new_validator()) {
-					added = true;
-
-				} else {
-					PX4_ERR("failed to add validator for sensor %s %i", sensor_data.subscription[i].get_topic()->o_name, i);
-				}
-			}
-		}
-	}
-
-	// never decrease the sensor count, as we could end up with mismatching validators
-	if (max_sensor_index + 1 > sensor_data.subscription_count) {
-		sensor_data.subscription_count = max_sensor_index + 1;
-	}
-
-	if (added) {
-		// force parameter refresh if anything was added
-		parametersUpdate();
-	}
-}
-
-void VotedSensorsUpdate::printStatus()
-{
-	PX4_INFO_RAW("selected gyro: %" PRIu32 " (%" PRIu8 ")\n", _selection.gyro_device_id, _gyro.last_best_vote);
-	_gyro.voter.print();
-
-	PX4_INFO_RAW("\n");
-	PX4_INFO_RAW("selected accel: %" PRIu32 " (%" PRIu8 ")\n", _selection.accel_device_id, _accel.last_best_vote);
-	_accel.voter.print();
-}
-
-void VotedSensorsUpdate::sensorsPoll(sensor_combined_s &raw)
-{
-	imuPoll(raw);
-
-	calcAccelInconsistency();
-	calcGyroInconsistency();
-
-	sensors_status_imu_s status{};
-	status.accel_device_id_primary = _selection.accel_device_id;
-	status.gyro_device_id_primary = _selection.gyro_device_id;
-
-	static_assert(MAX_SENSOR_COUNT == (sizeof(sensors_status_imu_s::accel_inconsistency_m_s_s) / sizeof(
-			sensors_status_imu_s::accel_inconsistency_m_s_s[0])), "check sensors_status_imu accel_inconsistency_m_s_s size");
-	static_assert(MAX_SENSOR_COUNT == (sizeof(sensors_status_imu_s::gyro_inconsistency_rad_s) / sizeof(
-			sensors_status_imu_s::gyro_inconsistency_rad_s[0])), "check sensors_status_imu accel_inconsistency_m_s_s size");
-
-	for (int i = 0; i < MAX_SENSOR_COUNT; i++) {
-		if ((_accel_device_id[i] != 0) && (_accel.priority[i] > 0)) {
-			status.accel_device_ids[i] = _accel_device_id[i];
-			status.accel_inconsistency_m_s_s[i] = _accel_diff[i].norm();
-			status.accel_healthy[i] = (_accel.voter.get_sensor_state(i) == DataValidator::ERROR_FLAG_NO_ERROR);
-			status.accel_priority[i] = _accel.voter.get_sensor_priority(i);
-		}
-
-		if ((_gyro_device_id[i] != 0) && (_gyro.priority[i] > 0)) {
-			status.gyro_device_ids[i] = _gyro_device_id[i];
-			status.gyro_inconsistency_rad_s[i] = _gyro_diff[i].norm();
-			status.gyro_healthy[i] = (_gyro.voter.get_sensor_state(i) == DataValidator::ERROR_FLAG_NO_ERROR);
-			status.gyro_priority[i] = _gyro.voter.get_sensor_priority(i);
-		}
-	}
-
-
-	status.timestamp = hrt_absolute_time();
-	_sensors_status_imu_pub.publish(status);
-
-	if (_parameter_update) {
-		// reset
-		_parameter_update = false;
-	}
-}
-
-void VotedSensorsUpdate::setRelativeTimestamps(sensor_combined_s &raw)
-{
-	if (_last_accel_timestamp[_accel.last_best_vote]) {
-		raw.accelerometer_timestamp_relative = (int32_t)((int64_t)_last_accel_timestamp[_accel.last_best_vote] -
-						       (int64_t)raw.timestamp);
-	}
-}
-
-void VotedSensorsUpdate::calcAccelInconsistency()
-{
-	Vector3f accel_mean{};
-	Vector3f accel_all[MAX_SENSOR_COUNT] {};
-	uint8_t accel_count = 0;
-
-	for (int sensor_index = 0; sensor_index < MAX_SENSOR_COUNT; sensor_index++) {
-		if ((_accel_device_id[sensor_index] != 0) && (_accel.priority[sensor_index] > 0)) {
-			accel_count++;
-			accel_all[sensor_index] = Vector3f{_last_sensor_data[sensor_index].accelerometer_m_s2};
-			accel_mean += accel_all[sensor_index];
-		}
-	}
-
-	if (accel_count > 0) {
-		accel_mean /= accel_count;
-
-		for (int sensor_index = 0; sensor_index < MAX_SENSOR_COUNT; sensor_index++) {
-			if ((_accel_device_id[sensor_index] != 0) && (_accel.priority[sensor_index] > 0)) {
-				_accel_diff[sensor_index] = 0.95f * _accel_diff[sensor_index] + 0.05f * (accel_all[sensor_index] - accel_mean);
-			}
-		}
-	}
-}
-
-void VotedSensorsUpdate::calcGyroInconsistency()
-{
-	Vector3f gyro_mean{};
-	Vector3f gyro_all[MAX_SENSOR_COUNT] {};
-	uint8_t gyro_count = 0;
-
-	for (int sensor_index = 0; sensor_index < MAX_SENSOR_COUNT; sensor_index++) {
-		if ((_gyro_device_id[sensor_index] != 0) && (_gyro.priority[sensor_index] > 0)) {
-			gyro_count++;
-			gyro_all[sensor_index] = Vector3f{_last_sensor_data[sensor_index].gyro_rad};
-			gyro_mean += gyro_all[sensor_index];
-		}
-	}
-
-	if (gyro_count > 0) {
-		gyro_mean /= gyro_count;
-
-		for (int sensor_index = 0; sensor_index < MAX_SENSOR_COUNT; sensor_index++) {
-			if ((_gyro_device_id[sensor_index] != 0) && (_gyro.priority[sensor_index] > 0)) {
-				_gyro_diff[sensor_index] = 0.95f * _gyro_diff[sensor_index] + 0.05f * (gyro_all[sensor_index] - gyro_mean);
-			}
-		}
-	}
-}

src/modules/sensors/voted_sensors_update.h

@@ -1,186 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2016-2022 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
-
-/**
- * @file voted_sensors_update.h
- *
- * @author Beat Kueng <beat-kueng@gmx.net>
- */
-
-#include "data_validator/DataValidator.hpp"
-#include "data_validator/DataValidatorGroup.hpp"
-
-#include <px4_platform_common/events.h>
-#include <px4_platform_common/module_params.h>
-#include <drivers/drv_hrt.h>
-#include <mathlib/mathlib.h>
-#include <matrix/math.hpp>
-#include <uORB/Publication.hpp>
-#include <uORB/Subscription.hpp>
-#include <uORB/SubscriptionMultiArray.hpp>
-#include <uORB/SubscriptionCallback.hpp>
-#include <uORB/topics/sensor_accel.h>
-#include <uORB/topics/sensor_gyro.h>
-#include <uORB/topics/sensor_combined.h>
-#include <uORB/topics/sensors_status_imu.h>
-#include <uORB/topics/sensor_selection.h>
-#include <uORB/topics/vehicle_imu.h>
-#include <uORB/topics/vehicle_imu_status.h>
-
-namespace sensors
-{
-
-static constexpr uint8_t MAX_SENSOR_COUNT = 4;
-
-/**
- ** class VotedSensorsUpdate
- *
- * Handling of sensor updates with voting
- */
-class VotedSensorsUpdate : public ModuleParams
-{
-public:
-	/**
-	 * @param parameters parameter values. These do not have to be initialized when constructing this object.
-	 * Only when calling init(), they have to be initialized.
-	 */
-	VotedSensorsUpdate(bool hil_enabled, uORB::SubscriptionCallbackWorkItem(&vehicle_imu_sub)[MAX_SENSOR_COUNT]);
-
-	/**
-	 * This tries to find new sensor instances. This is called from init(), then it can be called periodically.
-	 */
-	void initializeSensors();
-
-	void printStatus();
-
-	/**
-	 * call this whenever parameters got updated. Make sure to have initializeSensors() called at least
-	 * once before calling this.
-	 */
-	void parametersUpdate();
-
-	/**
-	 * read new sensor data
-	 */
-	void sensorsPoll(sensor_combined_s &raw);
-
-	/**
-	 * set the relative timestamps of each sensor timestamp, based on the last sensorsPoll,
-	 * so that the data can be published.
-	 */
-	void setRelativeTimestamps(sensor_combined_s &raw);
-
-private:
-
-	static constexpr uint8_t DEFAULT_PRIORITY = 50;
-
-	struct SensorData {
-		SensorData() = delete;
-		explicit SensorData(ORB_ID meta) : subscription{{meta, 0}, {meta, 1}, {meta, 2}, {meta, 3}} {}
-
-		uORB::Subscription subscription[MAX_SENSOR_COUNT]; /**< raw sensor data subscription */
-		DataValidatorGroup voter{1};
-		unsigned int last_failover_count{0};
-		int32_t priority[MAX_SENSOR_COUNT] {};
-		int32_t priority_configured[MAX_SENSOR_COUNT] {};
-		uint8_t last_best_vote{0}; /**< index of the latest best vote */
-		uint8_t subscription_count{0};
-		bool advertised[MAX_SENSOR_COUNT] {false, false, false};
-	};
-
-	void initSensorClass(SensorData &sensor_data, uint8_t sensor_count_max);
-
-	/**
-	 * Poll IMU for updated data.
-	 *
-	 * @param raw	Combined sensor data structure into which
-	 *		data should be returned.
-	 */
-	void imuPoll(sensor_combined_s &raw);
-
-	/**
-	 * Check & handle failover of a sensor
-	 * @return true if a switch occured (could be for a non-critical reason)
-	 */
-	bool checkFailover(SensorData &sensor, const char *sensor_name, events::px4::enums::sensor_type_t sensor_type);
-
-	/**
-	 * Calculates the magnitude in m/s/s of the largest difference between each accelerometer vector and the mean of all vectors
-	 */
-	void calcAccelInconsistency();
-
-	/**
-	 * Calculates the magnitude in rad/s of the largest difference between each gyro vector and the mean of all vectors
-	 */
-	void calcGyroInconsistency();
-
-	SensorData _accel{ORB_ID::sensor_accel};
-	SensorData _gyro{ORB_ID::sensor_gyro};
-
-	hrt_abstime _last_error_message{0};
-	orb_advert_t _mavlink_log_pub{nullptr};
-
-	uORB::Publication<sensor_selection_s> _sensor_selection_pub{ORB_ID(sensor_selection)};	/**< handle to the sensor selection uORB topic */
-	uORB::Publication<sensors_status_imu_s> _sensors_status_imu_pub{ORB_ID(sensors_status_imu)};
-
-	uORB::SubscriptionCallbackWorkItem(&_vehicle_imu_sub)[MAX_SENSOR_COUNT];
-	uORB::SubscriptionMultiArray<vehicle_imu_status_s, MAX_SENSOR_COUNT> _vehicle_imu_status_subs{ORB_ID::vehicle_imu_status};
-
-	uORB::Subscription _sensor_selection_sub{ORB_ID(sensor_selection)};
-
-	sensor_combined_s _last_sensor_data[MAX_SENSOR_COUNT] {};	/**< latest sensor data from all sensors instances */
-
-	const bool _hil_enabled{false};			/**< is hardware-in-the-loop mode enabled? */
-
-	bool _selection_changed{true};			/**< true when a sensor selection has changed and not been published */
-
-	matrix::Vector3f _accel_diff[MAX_SENSOR_COUNT] {};		/**< filtered accel differences between IMU units (m/s/s) */
-	matrix::Vector3f _gyro_diff[MAX_SENSOR_COUNT] {};			/**< filtered gyro differences between IMU uinits (rad/s) */
-
-	uint32_t _accel_device_id[MAX_SENSOR_COUNT] {};	/**< accel driver device id for each uorb instance */
-	uint32_t _gyro_device_id[MAX_SENSOR_COUNT] {};	/**< gyro driver device id for each uorb instance */
-
-	uint64_t _last_accel_timestamp[MAX_SENSOR_COUNT] {};	/**< latest full timestamp */
-
-	sensor_selection_s _selection {};		/**< struct containing the sensor selection to be published to the uORB */
-
-	bool _parameter_update{false};
-
-	DEFINE_PARAMETERS(
-		(ParamBool<px4::params::SENS_IMU_MODE>) _param_sens_imu_mode
-	)
-};
-
-} /* namespace sensors */

src/systemcmds/microbench/test_microbench_uorb.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *  Copyright (C) 2018-2021 PX4 Development Team. All rights reserved.
+ *  Copyright (C) 2018-2022 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
@@ -50,7 +50,7 @@
 #include <uORB/Subscription.hpp>
 #include <uORB/topics/sensor_accel.h>
 #include <uORB/topics/sensor_gyro.h>
-#include <uORB/topics/sensor_gyro_fifo.h>
+#include <uORB/topics/sensor_imu_fifo.h>
 #include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/failsafe_flags.h>
 
@@ -108,7 +108,7 @@ private:
 	failsafe_flags_s status;
 	vehicle_local_position_s lpos;
 	sensor_gyro_s gyro;
-	sensor_gyro_fifo_s gyro_fifo;
+	sensor_imu_fifo_s imu_fifo;
 };
 
 bool MicroBenchORB::run_tests()
@@ -139,7 +139,7 @@ void MicroBenchORB::reset()
 
 	gyro.timestamp = rand();
 
-	gyro_fifo.timestamp = rand();
+	imu_fifo.timestamp = rand();
 }
 
 ut_declare_test_c(test_microbench_uorb, MicroBenchORB)
@@ -149,7 +149,7 @@ bool MicroBenchORB::time_px4_uorb()
 	int fd_status = orb_subscribe(ORB_ID(failsafe_flags));
 	int fd_lpos = orb_subscribe(ORB_ID(vehicle_local_position));
 	int fd_gyro = orb_subscribe(ORB_ID(sensor_gyro));
-	int fd_gyro_fifo = orb_subscribe(ORB_ID(sensor_gyro_fifo));
+	int fd_imu_fifo = orb_subscribe(ORB_ID(sensor_imu_fifo));
 
 	int ret = 0;
 	bool updated = false;
@@ -169,8 +169,8 @@ bool MicroBenchORB::time_px4_uorb()
 
 	printf("\n");
 
-	PERF("orb_check sensor_gyro_fifo", ret = orb_check(fd_gyro_fifo, &updated), 100);
-	PERF("orb_copy sensor_gyro_fifo", ret = orb_copy(ORB_ID(sensor_gyro_fifo), fd_gyro_fifo, &gyro_fifo), 100);
+	PERF("orb_check sensor_imu_fifo", ret = orb_check(fd_imu_fifo, &updated), 100);
+	PERF("orb_copy sensor_imu_fifo", ret = orb_copy(ORB_ID(sensor_imu_fifo), fd_imu_fifo, &imu_fifo), 100);
 
 	printf("\n");
 
@@ -189,7 +189,7 @@ bool MicroBenchORB::time_px4_uorb()
 	orb_unsubscribe(fd_status);
 	orb_unsubscribe(fd_lpos);
 	orb_unsubscribe(fd_gyro);
-	orb_unsubscribe(fd_gyro_fifo);
+	orb_unsubscribe(fd_imu_fifo);
 
 	return true;
 }
@@ -238,9 +238,9 @@ bool MicroBenchORB::time_px4_uorb_direct()
 
 	{
 		printf("\n");
-		uORB::Subscription sens_gyro_fifo0{ORB_ID(sensor_gyro_fifo), 0};
-		PERF("uORB::Subscription orb_check sensor_gyro_fifo:0", ret = sens_gyro_fifo0.updated(), 100);
-		PERF("uORB::Subscription orb_copy sensor_gyro_fifo:0", ret = sens_gyro_fifo0.copy(&gyro_fifo), 100);
+		uORB::Subscription sens_imu_fifo0{ORB_ID(sensor_imu_fifo), 0};
+		PERF("uORB::Subscription orb_check sensor_imu_fifo:0", ret = sens_imu_fifo0.updated(), 100);
+		PERF("uORB::Subscription orb_copy sensor_imu_fifo:0", ret = sens_imu_fifo0.copy(&imu_fifo), 100);
 	}
 
 	return true;