sensors: create vehicle_acceleration module (#12597)

src/modules/sensors/CMakeLists.txt

@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2015-2017 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2015-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
@@ -31,6 +31,7 @@
 #
 ############################################################################
 
+add_subdirectory(vehicle_acceleration)
 add_subdirectory(vehicle_angular_velocity)
 
 px4_add_module(
@@ -53,5 +54,6 @@ px4_add_module(
 		git_ecl
 		ecl_validation
 		mathlib
+		sensors__vehicle_acceleration
 		sensors__vehicle_angular_velocity
 	)

src/modules/sensors/sensors.cpp

@@ -92,6 +92,7 @@
 #include "rc_update.h"
 #include "voted_sensors_update.h"
 
+#include "vehicle_acceleration/VehicleAcceleration.hpp"
 #include "vehicle_angular_velocity/VehicleAngularVelocity.hpp"
 
 using namespace DriverFramework;
@@ -204,7 +205,8 @@ private:
 	VotedSensorsUpdate _voted_sensors_update;
 
 
-	VehicleAngularVelocity	_angular_velocity;
+	VehicleAcceleration	_vehicle_acceleration;
+	VehicleAngularVelocity	_vehicle_angular_velocity;
 
 
 	/**
@@ -259,12 +261,14 @@ Sensors::Sensors(bool hil_enabled) :
 
 #endif /* BOARD_NUMBER_BRICKS > 0 */
 
-	_angular_velocity.Start();
+	_vehicle_acceleration.Start();
+	_vehicle_angular_velocity.Start();
 }
 
 Sensors::~Sensors()
 {
-	_angular_velocity.Stop();
+	_vehicle_acceleration.Stop();
+	_vehicle_angular_velocity.Stop();
 }
 
 int
@@ -736,7 +740,8 @@ int Sensors::print_status()
 	PX4_INFO("Airspeed status:");
 	_airspeed_validator.print();
 
-	_angular_velocity.PrintStatus();
+	_vehicle_acceleration.PrintStatus();
+	_vehicle_angular_velocity.PrintStatus();
 
 	return 0;
 }

src/modules/sensors/vehicle_acceleration/CMakeLists.txt

@@ -0,0 +1,37 @@
+############################################################################
+#
+#   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(sensors__vehicle_acceleration
+	VehicleAcceleration.cpp
+)
+target_link_libraries(sensors__vehicle_acceleration PRIVATE px4_work_queue)

src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp

@@ -0,0 +1,225 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+#include "VehicleAcceleration.hpp"
+
+#include <px4_log.h>
+
+using namespace matrix;
+using namespace time_literals;
+
+VehicleAcceleration::VehicleAcceleration() :
+	ModuleParams(nullptr),
+	WorkItem(px4::wq_configurations::att_pos_ctrl),
+	_cycle_perf(perf_alloc(PC_ELAPSED, "vehicle_acceleration: cycle time")),
+	_interval_perf(perf_alloc(PC_INTERVAL, "vehicle_acceleration: interval")),
+	_sensor_latency_perf(perf_alloc(PC_ELAPSED, "vehicle_acceleration: sensor latency"))
+{
+}
+
+VehicleAcceleration::~VehicleAcceleration()
+{
+	Stop();
+
+	perf_free(_cycle_perf);
+	perf_free(_interval_perf);
+	perf_free(_sensor_latency_perf);
+}
+
+bool
+VehicleAcceleration::Start()
+{
+	// initialize thermal corrections as we might not immediately get a topic update (only non-zero values)
+	_scale = Vector3f{1.0f, 1.0f, 1.0f};
+	_offset.zero();
+	_bias.zero();
+
+	// force initial updates
+	ParametersUpdate(true);
+	SensorBiasUpdate(true);
+
+	// needed to change the active sensor if the primary stops updating
+	_sensor_selection_sub.register_callback();
+
+	return SensorCorrectionsUpdate(true);
+}
+
+void
+VehicleAcceleration::Stop()
+{
+	Deinit();
+
+	// clear all registered callbacks
+	for (auto sub : _sensor_sub) {
+		sub.unregister_callback();
+	}
+
+	_sensor_selection_sub.unregister_callback();
+}
+
+void
+VehicleAcceleration::SensorBiasUpdate(bool force)
+{
+	if (_sensor_bias_sub.updated() || force) {
+		sensor_bias_s bias;
+
+		if (_sensor_bias_sub.copy(&bias)) {
+			// TODO: should be checking device ID
+			_bias = Vector3f{bias.accel_bias};
+		}
+	}
+}
+
+bool
+VehicleAcceleration::SensorCorrectionsUpdate(bool force)
+{
+	// check if the selected sensor has updated
+	if (_sensor_correction_sub.updated() || force) {
+
+		sensor_correction_s corrections{};
+		_sensor_correction_sub.copy(&corrections);
+
+		// TODO: should be checking device ID
+		if (_selected_sensor == 0) {
+			_offset = Vector3f{corrections.accel_offset_0};
+			_scale = Vector3f{corrections.accel_scale_0};
+
+		} else if (_selected_sensor == 1) {
+			_offset = Vector3f{corrections.accel_offset_1};
+			_scale = Vector3f{corrections.accel_scale_1};
+
+		} else if (_selected_sensor == 2) {
+			_offset = Vector3f{corrections.accel_offset_2};
+			_scale = Vector3f{corrections.accel_scale_2};
+
+		} else {
+			_offset = Vector3f{0.0f, 0.0f, 0.0f};
+			_scale = Vector3f{1.0f, 1.0f, 1.0f};
+		}
+
+		// update the latest sensor selection
+		if ((_selected_sensor != corrections.selected_accel_instance) || force) {
+			if (corrections.selected_accel_instance < MAX_SENSOR_COUNT) {
+				// clear all registered callbacks
+				for (auto sub : _sensor_sub) {
+					sub.unregister_callback();
+				}
+
+				const int sensor_new = corrections.selected_accel_instance;
+
+				if (_sensor_sub[sensor_new].register_callback()) {
+					PX4_DEBUG("selected sensor changed %d -> %d", _selected_sensor, sensor_new);
+					_selected_sensor = sensor_new;
+
+					return true;
+				}
+			}
+		}
+	}
+
+	return false;
+}
+
+void
+VehicleAcceleration::ParametersUpdate(bool force)
+{
+	// Check if parameters have changed
+	if (_params_sub.updated() || force) {
+		// clear update
+		parameter_update_s param_update;
+		_params_sub.copy(&param_update);
+
+		updateParams();
+
+		// get transformation matrix from sensor/board to body frame
+		const matrix::Dcmf board_rotation = get_rot_matrix((enum Rotation)_param_sens_board_rot.get());
+
+		// fine tune the rotation
+		const Dcmf board_rotation_offset(Eulerf(
+				math::radians(_param_sens_board_x_off.get()),
+				math::radians(_param_sens_board_y_off.get()),
+				math::radians(_param_sens_board_z_off.get())));
+
+		_board_rotation = board_rotation_offset * board_rotation;
+	}
+}
+
+void
+VehicleAcceleration::Run()
+{
+	perf_begin(_cycle_perf);
+	perf_count(_interval_perf);
+
+	// update corrections first to set _selected_sensor
+	SensorCorrectionsUpdate();
+
+	sensor_accel_s sensor_data;
+
+	if (_sensor_sub[_selected_sensor].update(&sensor_data)) {
+		perf_set_elapsed(_sensor_latency_perf, hrt_elapsed_time(&sensor_data.timestamp));
+
+		ParametersUpdate();
+		SensorBiasUpdate();
+
+		// get the sensor data and correct for thermal errors
+		const Vector3f val{sensor_data.x, sensor_data.y, sensor_data.z};
+
+		// apply offsets and scale
+		Vector3f accel{(val - _offset).emult(_scale)};
+
+		// rotate corrected measurements from sensor to body frame
+		accel = _board_rotation * accel;
+
+		// correct for in-run bias errors
+		accel -= _bias;
+
+		vehicle_acceleration_s out{};
+		out.timestamp_sample = sensor_data.timestamp;
+		accel.copyTo(out.xyz);
+		out.timestamp = hrt_absolute_time();
+
+		_vehicle_acceleration_pub.publish(out);
+	}
+
+	perf_end(_cycle_perf);
+}
+
+void
+VehicleAcceleration::PrintStatus()
+{
+	PX4_INFO("selected sensor: %d", _selected_sensor);
+
+	perf_print_counter(_cycle_perf);
+	perf_print_counter(_interval_perf);
+	perf_print_counter(_sensor_latency_perf);
+}

src/modules/sensors/vehicle_acceleration/VehicleAcceleration.hpp

@@ -0,0 +1,110 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include <lib/conversion/rotation.h>
+#include <lib/mathlib/math/Limits.hpp>
+#include <lib/matrix/matrix/math.hpp>
+#include <lib/perf/perf_counter.h>
+#include <px4_config.h>
+#include <px4_log.h>
+#include <px4_module_params.h>
+#include <px4_work_queue/WorkItem.hpp>
+#include <uORB/Publication.hpp>
+#include <uORB/Subscription.hpp>
+#include <uORB/SubscriptionCallback.hpp>
+#include <uORB/topics/parameter_update.h>
+#include <uORB/topics/sensor_bias.h>
+#include <uORB/topics/sensor_correction.h>
+#include <uORB/topics/sensor_selection.h>
+
+#include <uORB/topics/sensor_accel.h>
+#include <uORB/topics/vehicle_acceleration.h>
+
+class VehicleAcceleration : public ModuleParams, public px4::WorkItem
+{
+public:
+
+	VehicleAcceleration();
+	virtual ~VehicleAcceleration();
+
+	void	Run() override;
+
+	bool	Start();
+	void	Stop();
+
+	void	PrintStatus();
+
+private:
+
+	void	ParametersUpdate(bool force = false);
+	void	SensorBiasUpdate(bool force = false);
+	bool	SensorCorrectionsUpdate(bool force = false);
+
+	static constexpr int MAX_SENSOR_COUNT = 3;
+
+	DEFINE_PARAMETERS(
+		(ParamInt<px4::params::SENS_BOARD_ROT>) _param_sens_board_rot,
+
+		(ParamFloat<px4::params::SENS_BOARD_X_OFF>) _param_sens_board_x_off,
+		(ParamFloat<px4::params::SENS_BOARD_Y_OFF>) _param_sens_board_y_off,
+		(ParamFloat<px4::params::SENS_BOARD_Z_OFF>) _param_sens_board_z_off
+	)
+
+	uORB::Publication<vehicle_acceleration_s>	_vehicle_acceleration_pub{ORB_ID(vehicle_acceleration)};
+
+	uORB::Subscription			_params_sub{ORB_ID(parameter_update)};			/**< parameter updates subscription */
+	uORB::Subscription			_sensor_bias_sub{ORB_ID(sensor_bias)};			/**< sensor in-run bias correction subscription */
+	uORB::Subscription			_sensor_correction_sub{ORB_ID(sensor_correction)};	/**< sensor thermal correction subscription */
+
+	uORB::SubscriptionCallbackWorkItem	_sensor_selection_sub{this, ORB_ID(sensor_selection)};	/**< selected primary sensor subscription */
+	uORB::SubscriptionCallbackWorkItem	_sensor_sub[MAX_SENSOR_COUNT] {				/**< sensor data subscription */
+		{this, ORB_ID(sensor_accel), 0},
+		{this, ORB_ID(sensor_accel), 1},
+		{this, ORB_ID(sensor_accel), 2}
+	};
+
+	matrix::Dcmf				_board_rotation;				/**< rotation matrix for the orientation that the board is mounted */
+
+	matrix::Vector3f			_offset;
+	matrix::Vector3f			_scale;
+	matrix::Vector3f			_bias;
+
+	perf_counter_t				_cycle_perf;
+	perf_counter_t				_interval_perf;
+	perf_counter_t				_sensor_latency_perf;
+
+	uint8_t					_selected_sensor{0};
+
+};

src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp

@@ -35,15 +35,15 @@
 
 #include <px4_log.h>
 
-using namespace time_literals;
 using namespace matrix;
+using namespace time_literals;
 
 VehicleAngularVelocity::VehicleAngularVelocity() :
 	ModuleParams(nullptr),
 	WorkItem(px4::wq_configurations::rate_ctrl),
 	_cycle_perf(perf_alloc(PC_ELAPSED, "vehicle_angular_velocity: cycle time")),
 	_interval_perf(perf_alloc(PC_INTERVAL, "vehicle_angular_velocity: interval")),
-	_sensor_gyro_latency_perf(perf_alloc(PC_ELAPSED, "vehicle_angular_velocity: sensor gyro latency"))
+	_sensor_latency_perf(perf_alloc(PC_ELAPSED, "vehicle_angular_velocity: sensor latency"))
 {
 }
 
@@ -53,7 +53,7 @@ VehicleAngularVelocity::~VehicleAngularVelocity()
 
 	perf_free(_cycle_perf);
 	perf_free(_interval_perf);
-	perf_free(_sensor_gyro_latency_perf);
+	perf_free(_sensor_latency_perf);
 }
 
 bool
@@ -80,7 +80,7 @@ VehicleAngularVelocity::Stop()
 	Deinit();
 
 	// clear all registered callbacks
-	for (auto sub : _sensor_gyro_sub) {
+	for (auto sub : _sensor_sub) {
 		sub.unregister_callback();
 	}
 
@@ -95,9 +95,7 @@ VehicleAngularVelocity::SensorBiasUpdate(bool force)
 
 		if (_sensor_bias_sub.copy(&bias)) {
 			// TODO: should be checking device ID
-			_bias(0) = bias.gyro_x_bias;
-			_bias(1) = bias.gyro_y_bias;
-			_bias(2) = bias.gyro_z_bias;
+			_bias = Vector3f{bias.gyro_bias};
 		}
 	}
 }
@@ -105,22 +103,22 @@ VehicleAngularVelocity::SensorBiasUpdate(bool force)
 bool
 VehicleAngularVelocity::SensorCorrectionsUpdate(bool force)
 {
-	// check if the selected gyro has updated
+	// check if the selected sensor has updated
 	if (_sensor_correction_sub.updated() || force) {
 
 		sensor_correction_s corrections{};
 		_sensor_correction_sub.copy(&corrections);
 
 		// TODO: should be checking device ID
-		if (_selected_gyro == 0) {
+		if (_selected_sensor == 0) {
 			_offset = Vector3f{corrections.gyro_offset_0};
 			_scale = Vector3f{corrections.gyro_scale_0};
 
-		} else if (_selected_gyro == 1) {
+		} else if (_selected_sensor == 1) {
 			_offset = Vector3f{corrections.gyro_offset_1};
 			_scale = Vector3f{corrections.gyro_scale_1};
 
-		} else if (_selected_gyro == 2) {
+		} else if (_selected_sensor == 2) {
 			_offset = Vector3f{corrections.gyro_offset_2};
 			_scale = Vector3f{corrections.gyro_scale_2};
 
@@ -129,19 +127,19 @@ VehicleAngularVelocity::SensorCorrectionsUpdate(bool force)
 			_scale = Vector3f{1.0f, 1.0f, 1.0f};
 		}
 
-		// update the latest gyro selection
-		if (_selected_gyro != corrections.selected_gyro_instance) {
-			if (corrections.selected_gyro_instance < MAX_GYRO_COUNT) {
+		// update the latest sensor selection
+		if ((_selected_sensor != corrections.selected_gyro_instance) || force) {
+			if (corrections.selected_gyro_instance < MAX_SENSOR_COUNT) {
 				// clear all registered callbacks
-				for (auto sub : _sensor_gyro_sub) {
+				for (auto sub : _sensor_sub) {
 					sub.unregister_callback();
 				}
 
-				const int gyro_new = corrections.selected_gyro_instance;
+				const int sensor_new = corrections.selected_gyro_instance;
 
-				if (_sensor_gyro_sub[gyro_new].register_callback()) {
-					PX4_DEBUG("selected gyro changed %d -> %d", _selected_gyro, gyro_new);
-					_selected_gyro = gyro_new;
+				if (_sensor_sub[sensor_new].register_callback()) {
+					PX4_DEBUG("selected sensor changed %d -> %d", _selected_sensor, sensor_new);
+					_selected_sensor = sensor_new;
 
 					return true;
 				}
@@ -182,22 +180,22 @@ VehicleAngularVelocity::Run()
 	perf_begin(_cycle_perf);
 	perf_count(_interval_perf);
 
-	// update corrections first to set _selected_gyro
+	// update corrections first to set _selected_sensor
 	SensorCorrectionsUpdate();
 
-	sensor_gyro_s sensor_gyro;
+	sensor_gyro_s sensor_data;
 
-	if (_sensor_gyro_sub[_selected_gyro].update(&sensor_gyro)) {
-		perf_set_elapsed(_sensor_gyro_latency_perf, hrt_elapsed_time(&sensor_gyro.timestamp));
+	if (_sensor_sub[_selected_sensor].update(&sensor_data)) {
+		perf_set_elapsed(_sensor_latency_perf, hrt_elapsed_time(&sensor_data.timestamp));
 
 		ParametersUpdate();
 		SensorBiasUpdate();
 
-		// get the raw gyro data and correct for thermal errors
-		const Vector3f gyro{sensor_gyro.x, sensor_gyro.y, sensor_gyro.z};
+		// get the sensor data and correct for thermal errors
+		const Vector3f val{sensor_data.x, sensor_data.y, sensor_data.z};
 
 		// apply offsets and scale
-		Vector3f rates{(gyro - _offset).emult(_scale)};
+		Vector3f rates{(val - _offset).emult(_scale)};
 
 		// rotate corrected measurements from sensor to body frame
 		rates = _board_rotation * rates;
@@ -206,7 +204,7 @@ VehicleAngularVelocity::Run()
 		rates -= _bias;
 
 		vehicle_angular_velocity_s angular_velocity;
-		angular_velocity.timestamp_sample = sensor_gyro.timestamp;
+		angular_velocity.timestamp_sample = sensor_data.timestamp;
 		rates.copyTo(angular_velocity.xyz);
 		angular_velocity.timestamp = hrt_absolute_time();
 
@@ -219,9 +217,9 @@ VehicleAngularVelocity::Run()
 void
 VehicleAngularVelocity::PrintStatus()
 {
-	PX4_INFO("selected gyro: %d", _selected_gyro);
+	PX4_INFO("selected sensor: %d", _selected_sensor);
 
 	perf_print_counter(_cycle_perf);
 	perf_print_counter(_interval_perf);
-	perf_print_counter(_sensor_gyro_latency_perf);
+	perf_print_counter(_sensor_latency_perf);
 }

src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp

@@ -33,15 +33,13 @@
 
 #pragma once
 
+#include <lib/conversion/rotation.h>
+#include <lib/mathlib/math/Limits.hpp>
+#include <lib/matrix/matrix/math.hpp>
 #include <lib/perf/perf_counter.h>
 #include <px4_config.h>
-#include <px4_getopt.h>
 #include <px4_log.h>
 #include <px4_module_params.h>
-#include <lib/matrix/matrix/math.hpp>
-#include <lib/mathlib/math/Limits.hpp>
-#include <lib/mathlib/math/Functions.hpp>
-#include <lib/conversion/rotation.h>
 #include <px4_work_queue/WorkItem.hpp>
 #include <uORB/Publication.hpp>
 #include <uORB/Subscription.hpp>
@@ -49,11 +47,10 @@
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/sensor_bias.h>
 #include <uORB/topics/sensor_correction.h>
-#include <uORB/topics/sensor_gyro.h>
 #include <uORB/topics/sensor_selection.h>
-#include <uORB/topics/vehicle_angular_velocity.h>
 
-#define MAX_GYRO_COUNT 3
+#include <uORB/topics/sensor_gyro.h>
+#include <uORB/topics/vehicle_angular_velocity.h>
 
 class VehicleAngularVelocity : public ModuleParams, public px4::WorkItem
 {
@@ -75,6 +72,7 @@ private:
 	void	SensorBiasUpdate(bool force = false);
 	bool	SensorCorrectionsUpdate(bool force = false);
 
+	static constexpr int MAX_SENSOR_COUNT = 3;
 
 	DEFINE_PARAMETERS(
 		(ParamInt<px4::params::SENS_BOARD_ROT>) _param_sens_board_rot,
@@ -91,7 +89,7 @@ private:
 	uORB::Subscription			_sensor_correction_sub{ORB_ID(sensor_correction)};	/**< sensor thermal correction subscription */
 
 	uORB::SubscriptionCallbackWorkItem	_sensor_selection_sub{this, ORB_ID(sensor_selection)};	/**< selected primary sensor subscription */
-	uORB::SubscriptionCallbackWorkItem	_sensor_gyro_sub[MAX_GYRO_COUNT] {			/**< gyro data subscription */
+	uORB::SubscriptionCallbackWorkItem	_sensor_sub[MAX_SENSOR_COUNT] {				/**< sensor data subscription */
 		{this, ORB_ID(sensor_gyro), 0},
 		{this, ORB_ID(sensor_gyro), 1},
 		{this, ORB_ID(sensor_gyro), 2}
@@ -105,8 +103,8 @@ private:
 
 	perf_counter_t				_cycle_perf;
 	perf_counter_t				_interval_perf;
-	perf_counter_t				_sensor_gyro_latency_perf;
+	perf_counter_t				_sensor_latency_perf;
 
-	int					_selected_gyro{-1};
+	uint8_t					_selected_sensor{0};
 
 };