sensors: create vehicle_acceleration module (#12597)

msg/CMakeLists.txt

@@ -122,6 +122,7 @@ set(msg_files
 	uavcan_parameter_value.msg
 	ulog_stream.msg
 	ulog_stream_ack.msg
+	vehicle_acceleration.msg
 	vehicle_air_data.msg
 	vehicle_angular_velocity.msg
 	vehicle_attitude.msg

msg/sensor_bias.msg

@@ -5,20 +5,10 @@
 
 uint64 timestamp		# time since system start (microseconds)
 
-float32 accel_x		# Bias corrected acceleration in body X axis (in m/s^2)
-float32 accel_y		# Bias corrected acceleration in body Y axis (in m/s^2)
-float32 accel_z		# Bias corrected acceleration in body Z axis (in m/s^2)
-
 # In-run bias estimates (subtract from uncorrected data)
 
-float32 gyro_x_bias		# X gyroscope in-run bias in body frame (rad/s, x forward)
-float32 gyro_y_bias		# Y gyroscope in-run bias in body frame (rad/s, y right)
-float32 gyro_z_bias		# Z gyroscope in-run bias in body frame (rad/s, z down)
+float32[3] gyro_bias		# gyroscope in-run bias in body frame (rad/s)
 
-float32 accel_x_bias	# X accelerometer in-run bias in body frame (m/s^2, x forward)
-float32 accel_y_bias	# Y accelerometer in-run bias in body frame (m/s^2, y right)
-float32 accel_z_bias	# Z accelerometer in-run bias in body frame (m/s^2, z down)
+float32[3] accel_bias		# accelerometer in-run bias in body frame (m/s^2)
 
-float32 mag_x_bias		# X magnetometer in-run bias in body frame (Gauss, x forward)
-float32 mag_y_bias		# Y magnetometer in-run bias in body frame (Gauss, y right)
-float32 mag_z_bias		# Z magnetometer in-run bias in body frame (Gauss, z down)
+float32[3] mag_bias		# magnetometer in-run bias in body frame (Gauss)

msg/tools/uorb_rtps_message_ids.yaml

@@ -256,6 +256,8 @@ rtps:
     id: 111
   - msg: vehicle_angular_velocity
     id: 112
+  - msg: vehicle_acceleration
+    id: 113
   # multi topics
   - msg: actuator_controls_0
     id: 120

msg/vehicle_acceleration.msg

@@ -0,0 +1,6 @@
+
+uint64 timestamp		# time since system start (microseconds)
+
+uint64 timestamp_sample		# the timestamp of the raw data (microseconds)
+
+float32[3] xyz			# Bias corrected acceleration (including gravity) in body axis (in m/s^2)

src/modules/commander/Commander.cpp

@@ -3901,8 +3901,8 @@ void Commander::airspeed_use_check()
 	_airspeed_sub.update();
 	const airspeed_s &airspeed = _airspeed_sub.get();
 
-	_sensor_bias_sub.update();
-	const sensor_bias_s &sensors = _sensor_bias_sub.get();
+	vehicle_acceleration_s accel{};
+	_vehicle_acceleration_sub.copy(&accel);
 
 	bool is_fixed_wing = status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING;
 
@@ -3988,7 +3988,7 @@ void Commander::airspeed_use_check()
 			float max_lift_ratio = fmaxf(airspeed.indicated_airspeed_m_s, 0.7f) / fmaxf(_airspeed_stall.get(), 1.0f);
 			max_lift_ratio *= max_lift_ratio;
 
-			_load_factor_ratio = 0.95f * _load_factor_ratio + 0.05f * (fabsf(sensors.accel_z) / CONSTANTS_ONE_G) / max_lift_ratio;
+			_load_factor_ratio = 0.95f * _load_factor_ratio + 0.05f * (fabsf(accel.xyz[2]) / CONSTANTS_ONE_G) / max_lift_ratio;
 			_load_factor_ratio = math::constrain(_load_factor_ratio, 0.25f, 2.0f);
 			load_factor_ratio_fail = (_load_factor_ratio > 1.1f);
 			status.load_factor_ratio = _load_factor_ratio;

src/modules/commander/Commander.hpp

@@ -60,8 +60,8 @@
 #include <uORB/topics/iridiumsbd_status.h>
 #include <uORB/topics/mission_result.h>
 #include <uORB/topics/offboard_control_mode.h>
-#include <uORB/topics/sensor_bias.h>
 #include <uORB/topics/telemetry_status.h>
+#include <uORB/topics/vehicle_acceleration.h>
 #include <uORB/topics/vehicle_command.h>
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/vehicle_local_position.h>
@@ -250,11 +250,12 @@ private:
 	bool _print_avoidance_msg_once{false};
 
 	// Subscriptions
+	uORB::Subscription					_vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)};
+
 	uORB::SubscriptionData<airspeed_s>			_airspeed_sub{ORB_ID(airspeed)};
 	uORB::SubscriptionData<estimator_status_s>		_estimator_status_sub{ORB_ID(estimator_status)};
 	uORB::SubscriptionData<mission_result_s>		_mission_result_sub{ORB_ID(mission_result)};
 	uORB::SubscriptionData<offboard_control_mode_s>		_offboard_control_mode_sub{ORB_ID(offboard_control_mode)};
-	uORB::SubscriptionData<sensor_bias_s>			_sensor_bias_sub{ORB_ID(sensor_bias)};
 	uORB::SubscriptionData<vehicle_global_position_s>	_global_position_sub{ORB_ID(vehicle_global_position)};
 	uORB::SubscriptionData<vehicle_local_position_s>	_local_position_sub{ORB_ID(vehicle_local_position)};
 

src/modules/ekf2/ekf2_main.cpp

@@ -1482,31 +1482,17 @@ void Ekf2::run()
 
 			{
 				// publish all corrected sensor readings and bias estimates after mag calibration is updated above
-				sensor_bias_s bias;
+				sensor_bias_s bias{};
 
 				bias.timestamp = now;
 
 				// In-run bias estimates
-				float gyro_bias[3];
-				_ekf.get_gyro_bias(gyro_bias);
-				bias.gyro_x_bias = gyro_bias[0];
-				bias.gyro_y_bias = gyro_bias[1];
-				bias.gyro_z_bias = gyro_bias[2];
+				_ekf.get_gyro_bias(bias.gyro_bias);
+				_ekf.get_accel_bias(bias.accel_bias);
 
-				float accel_bias[3];
-				_ekf.get_accel_bias(accel_bias);
-				bias.accel_x_bias = accel_bias[0];
-				bias.accel_y_bias = accel_bias[1];
-				bias.accel_z_bias = accel_bias[2];
-
-				bias.mag_x_bias = _last_valid_mag_cal[0];
-				bias.mag_y_bias = _last_valid_mag_cal[1];
-				bias.mag_z_bias = _last_valid_mag_cal[2];
-
-				// TODO: remove from sensor_bias?
-				bias.accel_x = sensors.accelerometer_m_s2[0] - accel_bias[0];
-				bias.accel_y = sensors.accelerometer_m_s2[1] - accel_bias[1];
-				bias.accel_z = sensors.accelerometer_m_s2[2] - accel_bias[2];
+				bias.mag_bias[0] = _last_valid_mag_cal[0];
+				bias.mag_bias[1] = _last_valid_mag_cal[1];
+				bias.mag_bias[2] = _last_valid_mag_cal[2];
 
 				_sensor_bias_pub.publish(bias);
 			}

src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp

@@ -37,8 +37,6 @@ extern "C" __EXPORT int fw_pos_control_l1_main(int argc, char *argv[]);
 
 FixedwingPositionControl::FixedwingPositionControl() :
 	ModuleParams(nullptr),
-	_sub_airspeed(ORB_ID(airspeed)),
-	_sub_sensors(ORB_ID(sensor_bias)),
 	_loop_perf(perf_alloc(PC_ELAPSED, "fw_l1_control")),
 	_launchDetector(this),
 	_runway_takeoff(this)
@@ -346,9 +344,9 @@ FixedwingPositionControl::airspeed_poll()
 {
 	bool airspeed_valid = _airspeed_valid;
 
-	if (!_parameters.airspeed_disabled && _sub_airspeed.update()) {
+	if (!_parameters.airspeed_disabled && _airspeed_sub.update()) {
 
-		const airspeed_s &as = _sub_airspeed.get();
+		const airspeed_s &as = _airspeed_sub.get();
 
 		if (PX4_ISFINITE(as.indicated_airspeed_m_s)
 		    && PX4_ISFINITE(as.true_airspeed_m_s)
@@ -1292,7 +1290,7 @@ FixedwingPositionControl::control_takeoff(const Vector2f &curr_pos, const Vector
 				}
 
 				/* Detect launch using body X (forward) acceleration */
-				_launchDetector.update(_sub_sensors.get().accel_x);
+				_launchDetector.update(_vehicle_acceleration_sub.get().xyz[0]);
 
 				/* update our copy of the launch detection state */
 				_launch_detection_state = _launchDetector.getLaunchDetected();
@@ -1750,7 +1748,6 @@ FixedwingPositionControl::run()
 			_alt_reset_counter = _global_pos.alt_reset_counter;
 			_pos_reset_counter = _global_pos.lat_lon_reset_counter;
 
-			_sub_sensors.update();
 			airspeed_poll();
 			_manual_control_sub.update(&_manual);
 			_pos_sp_triplet_sub.update(&_pos_sp_triplet);
@@ -1759,6 +1756,7 @@ FixedwingPositionControl::run()
 			vehicle_control_mode_poll();
 			_vehicle_land_detected_sub.update(&_vehicle_land_detected);
 			vehicle_status_poll();
+			_vehicle_acceleration_sub.update();
 			_vehicle_rates_sub.update();
 
 			Vector2f curr_pos((float)_global_pos.lat, (float)_global_pos.lon);
@@ -1931,7 +1929,7 @@ FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float airspee
 	float pitch_for_tecs = _pitch - _parameters.pitchsp_offset_rad;
 
 	/* filter speed and altitude for controller */
-	Vector3f accel_body(_sub_sensors.get().accel_x, _sub_sensors.get().accel_y, _sub_sensors.get().accel_z);
+	Vector3f accel_body(_vehicle_acceleration_sub.get().xyz);
 
 	// tailsitters use the multicopter frame as reference, in fixed wing
 	// we need to use the fixed wing frame

src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp

@@ -74,8 +74,8 @@
 #include <uORB/topics/position_controller_status.h>
 #include <uORB/topics/position_setpoint_triplet.h>
 #include <uORB/topics/sensor_baro.h>
-#include <uORB/topics/sensor_bias.h>
 #include <uORB/topics/tecs_status.h>
+#include <uORB/topics/vehicle_acceleration.h>
 #include <uORB/topics/vehicle_angular_velocity.h>
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
@@ -184,8 +184,8 @@ private:
 	vehicle_land_detected_s		_vehicle_land_detected {};	///< vehicle land detected */
 	vehicle_status_s		_vehicle_status {};		///< vehicle status */
 
-	SubscriptionData<airspeed_s> _sub_airspeed;
-	SubscriptionData<sensor_bias_s> _sub_sensors;
+	SubscriptionData<airspeed_s>			_airspeed_sub{ORB_ID(airspeed)};
+	SubscriptionData<vehicle_acceleration_s>	_vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)};
 
 	perf_counter_t	_loop_perf;				///< loop performance counter */
 

src/modules/land_detector/FixedwingLandDetector.cpp

@@ -45,6 +45,7 @@
 
 #include <px4_config.h>
 #include <px4_defines.h>
+#include <matrix/math.hpp>
 
 namespace land_detector
 {
@@ -59,7 +60,7 @@ FixedwingLandDetector::FixedwingLandDetector()
 void FixedwingLandDetector::_update_topics()
 {
 	_airspeed_sub.update(&_airspeed);
-	_sensor_bias_sub.update(&_sensor_bias);
+	_vehicle_acceleration_sub.update(&_vehicle_acceleration);
 	_vehicle_local_position_sub.update(&_vehicle_local_position);
 }
 
@@ -110,8 +111,8 @@ bool FixedwingLandDetector::_get_landed_state()
 
 		// A leaking lowpass prevents biases from building up, but
 		// gives a mostly correct response for short impulses.
-		const float acc_hor = sqrtf(_sensor_bias.accel_x * _sensor_bias.accel_x +
-					    _sensor_bias.accel_y * _sensor_bias.accel_y);
+		const matrix::Vector3f accel{_vehicle_acceleration.xyz};
+		const float acc_hor = sqrtf(accel(0) * accel(0) + accel(1) * accel(1));
 
 		_accel_horz_lp = _accel_horz_lp * 0.8f + acc_hor * 0.18f;
 

src/modules/land_detector/FixedwingLandDetector.h

@@ -45,7 +45,7 @@
 #include <drivers/drv_hrt.h>
 #include <uORB/Subscription.hpp>
 #include <uORB/topics/airspeed.h>
-#include <uORB/topics/sensor_bias.h>
+#include <uORB/topics/vehicle_acceleration.h>
 #include <uORB/topics/vehicle_local_position.h>
 
 #include "LandDetector.h"
@@ -75,11 +75,11 @@ private:
 
 	uORB::Subscription _airspeed_sub{ORB_ID(airspeed)};
 	uORB::Subscription _param_update_sub{ORB_ID(parameter_update)};
-	uORB::Subscription _sensor_bias_sub{ORB_ID(sensor_bias)};
+	uORB::Subscription _vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)};
 	uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
 
 	airspeed_s _airspeed{};
-	sensor_bias_s _sensor_bias{};
+	vehicle_acceleration_s _vehicle_acceleration{};
 	vehicle_local_position_s _vehicle_local_position{};
 
 	float _accel_horz_lp{0.0f};

src/modules/land_detector/MulticopterLandDetector.cpp

@@ -63,6 +63,7 @@
 
 #include <cmath>
 #include <mathlib/mathlib.h>
+#include <matrix/math.hpp>
 
 #include "MulticopterLandDetector.h"
 
@@ -94,7 +95,7 @@ void MulticopterLandDetector::_update_topics()
 {
 	_actuator_controls_sub.update(&_actuator_controls);
 	_battery_sub.update(&_battery_status);
-	_sensor_bias_sub.update(&_sensor_bias);
+	_vehicle_acceleration_sub.update(&_vehicle_acceleration);
 	_vehicle_angular_velocity_sub.update(&_vehicle_angular_velocity);
 	_vehicle_attitude_sub.update(&_vehicle_attitude);
 	_vehicle_control_mode_sub.update(&_control_mode);
@@ -126,17 +127,15 @@ bool MulticopterLandDetector::_get_freefall_state()
 		return false;
 	}
 
-	if (_sensor_bias.timestamp == 0) {
+	if (_vehicle_acceleration.timestamp == 0) {
 		// _sensors is not valid yet, we have to assume we're not falling.
 		return false;
 	}
 
-	float acc_norm = _sensor_bias.accel_x * _sensor_bias.accel_x
-			 + _sensor_bias.accel_y * _sensor_bias.accel_y
-			 + _sensor_bias.accel_z * _sensor_bias.accel_z;
-	acc_norm = sqrtf(acc_norm);	//norm of specific force. Should be close to 9.8 m/s^2 when landed.
+	// norm of specific force. Should be close to 9.8 m/s^2 when landed.
+	const matrix::Vector3f accel{_vehicle_acceleration.xyz};
 
-	return (acc_norm < _params.freefall_acc_threshold);	//true if we are currently falling
+	return (accel.norm() < _params.freefall_acc_threshold);	// true if we are currently falling
 }
 
 bool MulticopterLandDetector::_get_ground_contact_state()
@@ -216,7 +215,7 @@ bool MulticopterLandDetector::_get_maybe_landed_state()
 	// Next look if all rotation angles are not moving.
 	float maxRotationScaled = _params.maxRotation_rad_s * landThresholdFactor;
 
-	bool rotating = (fabsf(_vehicle_angular_velocity.xyz[0])  > maxRotationScaled) ||
+	bool rotating = (fabsf(_vehicle_angular_velocity.xyz[0]) > maxRotationScaled) ||
 			(fabsf(_vehicle_angular_velocity.xyz[1]) > maxRotationScaled) ||
 			(fabsf(_vehicle_angular_velocity.xyz[2]) > maxRotationScaled);
 

src/modules/land_detector/MulticopterLandDetector.h

@@ -52,7 +52,7 @@
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/battery_status.h>
 #include <uORB/topics/parameter_update.h>
-#include <uORB/topics/sensor_bias.h>
+#include <uORB/topics/vehicle_acceleration.h>
 #include <uORB/topics/vehicle_angular_velocity.h>
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_control_mode.h>
@@ -126,7 +126,7 @@ private:
 
 	uORB::Subscription _actuator_controls_sub{ORB_ID(actuator_controls_0)};
 	uORB::Subscription _battery_sub{ORB_ID(battery_status)};
-	uORB::Subscription _sensor_bias_sub{ORB_ID(sensor_bias)};
+	uORB::Subscription _vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)};
 	uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)};
 	uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
 	uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
@@ -136,7 +136,7 @@ private:
 	actuator_controls_s               _actuator_controls {};
 	battery_status_s                  _battery_status {};
 	vehicle_control_mode_s            _control_mode {};
-	sensor_bias_s                     _sensor_bias {};
+	vehicle_acceleration_s           _vehicle_acceleration{};
 	vehicle_attitude_s                _vehicle_attitude {};
 	vehicle_angular_velocity_s        _vehicle_angular_velocity{};
 	vehicle_local_position_s          _vehicle_local_position {};

src/modules/landing_target_estimator/LandingTargetEstimator.cpp

@@ -80,14 +80,11 @@ void LandingTargetEstimator::update()
 			float dt = (hrt_absolute_time() - _last_predict) / SEC2USEC;
 
 			// predict target position with the help of accel data
-			matrix::Vector3f a;
+			matrix::Vector3f a{_vehicle_acceleration.xyz};
 
-			if (_vehicleAttitude_valid && _sensorBias_valid) {
+			if (_vehicleAttitude_valid && _vehicle_acceleration_valid) {
 				matrix::Quaternion<float> q_att(&_vehicleAttitude.q[0]);
 				_R_att = matrix::Dcm<float>(q_att);
-				a(0) = _sensorBias.accel_x;
-				a(1) = _sensorBias.accel_y;
-				a(2) = _sensorBias.accel_z;
 				a = _R_att * a;
 
 			} else {
@@ -244,7 +241,7 @@ void LandingTargetEstimator::_update_topics()
 {
 	_vehicleLocalPosition_valid = _vehicleLocalPositionSub.update(&_vehicleLocalPosition);
 	_vehicleAttitude_valid = _attitudeSub.update(&_vehicleAttitude);
-	_sensorBias_valid = _sensorBiasSub.update(&_sensorBias);
+	_vehicle_acceleration_valid = _vehicle_acceleration_sub.update(&_vehicle_acceleration);
 
 	_new_irlockReport = _irlockReportSub.update(&_irlockReport);
 }

src/modules/landing_target_estimator/LandingTargetEstimator.h

@@ -47,9 +47,9 @@
 #include <parameters/param.h>
 #include <uORB/Publication.hpp>
 #include <uORB/Subscription.hpp>
-#include <uORB/topics/vehicle_local_position.h>
+#include <uORB/topics/vehicle_acceleration.h>
 #include <uORB/topics/vehicle_attitude.h>
-#include <uORB/topics/sensor_bias.h>
+#include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/irlock_report.h>
 #include <uORB/topics/landing_target_pose.h>
 #include <uORB/topics/landing_target_innovations.h>
@@ -130,18 +130,18 @@ private:
 
 	uORB::Subscription _vehicleLocalPositionSub{ORB_ID(vehicle_local_position)};
 	uORB::Subscription _attitudeSub{ORB_ID(vehicle_attitude)};
-	uORB::Subscription _sensorBiasSub{ORB_ID(sensor_bias)};
+	uORB::Subscription _vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)};
 	uORB::Subscription _irlockReportSub{ORB_ID(irlock_report)};
 
 	vehicle_local_position_s	_vehicleLocalPosition{};
-	vehicle_attitude_s	_vehicleAttitude{};
-	sensor_bias_s		_sensorBias{};
-	irlock_report_s		_irlockReport{};
+	vehicle_attitude_s		_vehicleAttitude{};
+	vehicle_acceleration_s		_vehicle_acceleration{};
+	irlock_report_s			_irlockReport{};
 
 	// keep track of which topics we have received
 	bool _vehicleLocalPosition_valid{false};
 	bool _vehicleAttitude_valid{false};
-	bool _sensorBias_valid{false};
+	bool _vehicle_acceleration_valid{false};
 	bool _new_irlockReport{false};
 	bool _estimator_initialized{false};
 	// keep track of whether last measurement was rejected

src/modules/mavlink/mavlink_messages.cpp

@@ -848,15 +848,15 @@ protected:
 			mavlink_highres_imu_t msg = {};
 
 			msg.time_usec = sensor.timestamp;
-			msg.xacc = sensor.accelerometer_m_s2[0] - bias.accel_x_bias;
-			msg.yacc = sensor.accelerometer_m_s2[1] - bias.accel_y_bias;
-			msg.zacc = sensor.accelerometer_m_s2[2] - bias.accel_z_bias;
-			msg.xgyro = sensor.gyro_rad[0] - bias.gyro_x_bias;
-			msg.ygyro = sensor.gyro_rad[1] - bias.gyro_y_bias;
-			msg.zgyro = sensor.gyro_rad[2] - bias.gyro_z_bias;
-			msg.xmag = magnetometer.magnetometer_ga[0] - bias.mag_x_bias;
-			msg.ymag = magnetometer.magnetometer_ga[1] - bias.mag_y_bias;
-			msg.zmag = magnetometer.magnetometer_ga[2] - bias.mag_z_bias;
+			msg.xacc = sensor.accelerometer_m_s2[0] - bias.accel_bias[0];
+			msg.yacc = sensor.accelerometer_m_s2[1] - bias.accel_bias[1];
+			msg.zacc = sensor.accelerometer_m_s2[2] - bias.accel_bias[2];
+			msg.xgyro = sensor.gyro_rad[0] - bias.gyro_bias[0];
+			msg.ygyro = sensor.gyro_rad[1] - bias.gyro_bias[1];
+			msg.zgyro = sensor.gyro_rad[2] - bias.gyro_bias[2];
+			msg.xmag = magnetometer.magnetometer_ga[0] - bias.mag_bias[0];
+			msg.ymag = magnetometer.magnetometer_ga[1] - bias.mag_bias[1];
+			msg.zmag = magnetometer.magnetometer_ga[2] - bias.mag_bias[2];
 			msg.abs_pressure = air_data.baro_pressure_pa;
 			msg.diff_pressure = differential_pressure.differential_pressure_raw_pa;
 			msg.pressure_alt = air_data.baro_alt_meter;

src/modules/rover_pos_control/RoverPositionControl.cpp

@@ -61,10 +61,8 @@ extern "C" __EXPORT int rover_pos_control_main(int argc, char *argv[]);
 RoverPositionControl::RoverPositionControl() :
 	ModuleParams(nullptr),
 	/* performance counters */
-	_sub_sensors(ORB_ID(sensor_bias)),
 	_loop_perf(perf_alloc(PC_ELAPSED, "rover position control")) // TODO : do we even need these perf counters
 {
-
 }
 
 RoverPositionControl::~RoverPositionControl()
@@ -197,7 +195,7 @@ RoverPositionControl::control_position(const matrix::Vector2f &current_position,
 			const Vector3f vel = R_to_body * Vector3f(ground_speed(0), ground_speed(1), ground_speed(2));
 
 			const float x_vel = vel(0);
-			const float x_acc = _sub_sensors.get().accel_x;
+			const float x_acc = _vehicle_acceleration_sub.get().xyz[0];
 
 			// Compute airspeed control out and just scale it as a constant
 			mission_throttle = _param_throttle_speed_scaler.get()
@@ -314,7 +312,7 @@ RoverPositionControl::run()
 		vehicle_control_mode_poll();
 		//manual_control_setpoint_poll();
 
-		_sub_sensors.update();
+		_vehicle_acceleration_sub.update();
 
 		/* update parameters from storage */
 		parameters_update(_params_sub);

src/modules/rover_pos_control/RoverPositionControl.hpp

@@ -58,8 +58,8 @@
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/position_controller_status.h>
 #include <uORB/topics/position_setpoint_triplet.h>
-#include <uORB/topics/sensor_bias.h>
 #include <uORB/topics/sensor_combined.h>
+#include <uORB/topics/vehicle_acceleration.h>
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
 #include <uORB/topics/vehicle_control_mode.h>
@@ -116,7 +116,7 @@ private:
 	vehicle_attitude_s              _vehicle_att{};
 	sensor_combined_s				_sensor_combined{};
 
-	SubscriptionData<sensor_bias_s>	_sub_sensors;
+	SubscriptionData<vehicle_acceleration_s>		_vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)};
 
 	perf_counter_t	_loop_perf;			/**< loop performance counter */
 

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};
 
 };