sensor accel/gyro message cleanup

- split out integrated data into new standalone messages (sensor_accel_integrated and sensor_gyro_integrated)
 - publish sensor_gyro at full rate and remove sensor_gyro_control
 - limit sensor status publications to 10 Hz
 - remove unused accel/gyro raw ADC fields
 - add device IDs to sensor_bias and sensor_correction
    - vehicle_angular_velocity/vehicle_acceleration: check device ids before using bias and corrections

.ci/Jenkinsfile-hardware

@@ -698,9 +698,13 @@ void statusFTDI() {
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "listener adc_report"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "listener battery_status"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "listener sensor_accel"'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "listener sensor_accel_fifo"'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "listener sensor_accel_integrated"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "listener sensor_accel_status"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "listener sensor_baro"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "listener sensor_gyro"'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "listener sensor_gyro_fifo"'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "listener sensor_gyro_integrated"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "listener sensor_gyro_status"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "listener sensor_mag"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "listener servorail_status"'
@@ -743,9 +747,13 @@ void statusSEGGER() {
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "listener adc_report"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "listener battery_status"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "listener sensor_accel"'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "listener sensor_accel_fifo"'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "listener sensor_accel_integrated"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "listener sensor_accel_status"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "listener sensor_baro"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "listener sensor_gyro"'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "listener sensor_gyro_fifo"'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "listener sensor_gyro_integrated"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "listener sensor_gyro_status"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "listener sensor_mag"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "listener servorail_status"'

msg/CMakeLists.txt

@@ -60,8 +60,8 @@ set(msg_files
 	ekf_gps_position.msg
 	esc_report.msg
 	esc_status.msg
-	estimator_status.msg
 	estimator_innovations.msg
+	estimator_status.msg
 	follow_target.msg
 	geofence_result.msg
 	gps_dump.msg
@@ -105,14 +105,15 @@ set(msg_files
 	satellite_info.msg
 	sensor_accel.msg
 	sensor_accel_fifo.msg
+	sensor_accel_integrated.msg
 	sensor_accel_status.msg
 	sensor_baro.msg
 	sensor_bias.msg
 	sensor_combined.msg
 	sensor_correction.msg
 	sensor_gyro.msg
-	sensor_gyro_control.msg
 	sensor_gyro_fifo.msg
+	sensor_gyro_integrated.msg
 	sensor_gyro_status.msg
 	sensor_mag.msg
 	sensor_preflight.msg
@@ -156,7 +157,7 @@ set(msg_files
 	vtol_vehicle_status.msg
 	wheel_encoders.msg
 	wind_estimate.msg
-	)
+)
 
 set(deprecated_msgs
 	ekf2_innovations.msg	# 2019-11-22, Updated estimator interface and logging; replaced by 'estimator_innovations'.

msg/sensor_accel.msg

@@ -1,22 +1,10 @@
-uint64 timestamp	# time since system start (microseconds)
+uint64 timestamp          # time since system start (microseconds)
-uint32 device_id	# unique device ID for the sensor that does not change between power cycles
+uint64 timestamp_sample
 
-uint64 error_count
+uint32 device_id          # unique device ID for the sensor that does not change between power cycles
 
-float32 x		# acceleration in the NED X board axis in m/s^2
+float32 x                 # acceleration in the NED X board axis in m/s^2
-float32 y		# acceleration in the NED Y board axis in m/s^2
+float32 y                 # acceleration in the NED Y board axis in m/s^2
-float32 z		# acceleration in the NED Z board axis in m/s^2
+float32 z                 # acceleration in the NED Z board axis in m/s^2
 
-uint32 integral_dt	# integration time (microseconds)
+float32 temperature       # temperature in degrees celsius
-uint8 integral_samples	# number of samples integrated
-float32 x_integral	# delta velocity in the NED X board axis in m/s over the integration time frame (integral_dt)
-float32 y_integral	# delta velocity in the NED Y board axis in m/s over the integration time frame (integral_dt)
-float32 z_integral	# delta velocity in the NED Z board axis in m/s over the integration time frame (integral_dt)
-uint8 integral_clip_count	# total clip count per integration period on any axis
-
-float32 temperature	# temperature in degrees celsius
-
-float32 scaling		# scaling from raw to m/s^2
-int16 x_raw
-int16 y_raw
-int16 z_raw

msg/sensor_accel_fifo.msg

@@ -1,13 +1,13 @@
-uint64 timestamp	# time since system start (microseconds)
+uint64 timestamp          # time since system start (microseconds)
-uint32 device_id	# unique device ID for the sensor that does not change between power cycles
+uint64 timestamp_sample
 
-uint64 timestamp_sample	# time since system start (microseconds)
+uint32 device_id          # unique device ID for the sensor that does not change between power cycles
 
-float32 dt		# delta time between samples (microseconds)
+float32 dt                # delta time between samples (microseconds)
 float32 scale
 
-uint8 samples		# number of valid samples
+uint8 samples             # number of valid samples
 
-int16[16] x		# acceleration in the NED X board axis in m/s/s
+int16[16] x               # acceleration in the NED X board axis in m/s/s
-int16[16] y		# acceleration in the NED Y board axis in m/s/s
+int16[16] y               # acceleration in the NED Y board axis in m/s/s
-int16[16] z		# acceleration in the NED Z board axis in m/s/s
+int16[16] z               # acceleration in the NED Z board axis in m/s/s

msg/sensor_accel_integrated.msg

@@ -0,0 +1,13 @@
+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 temperature       # temperature in degrees celsius
+
+uint64 error_count
+
+float32[3] delta_velocity # delta velocity in the NED board axis in m/s over the integration time frame (dt)
+uint16 dt                 # integration time (microseconds)
+uint8 samples             # number of samples integrated
+uint8 clip_count          # total clip count per integration period on any axis

msg/sensor_accel_status.msg

@@ -1,5 +1,6 @@
-uint64 timestamp	# time since system start (microseconds)
+uint64 timestamp          # time since system start (microseconds)
-uint32 device_id	# unique device ID for the sensor that does not change between power cycles
+
+uint32 device_id          # unique device ID for the sensor that does not change between power cycles
 
 uint64 error_count
 
@@ -7,11 +8,11 @@ float32 temperature
 
 uint8 rotation
 
-uint64[3] clipping	# clipping per axis
+uint32[3] clipping        # clipping per axis
 
 uint16 measure_rate
 uint16 sample_rate
 
 float32 full_scale_range
 
-float32 vibration_metric	# high frequency vibration level in the IMU delta velocity data (m/s)
+float32 vibration_metric  # high frequency vibration level in the IMU delta velocity data (m/s)

msg/sensor_bias.msg

@@ -7,8 +7,11 @@ uint64 timestamp		# time since system start (microseconds)
 
 # In-run bias estimates (subtract from uncorrected data)
 
+uint32 gyro_device_id		# unique device ID for the sensor that does not change between power cycles
 float32[3] gyro_bias		# gyroscope in-run bias in body frame (rad/s)
 
+uint32 accel_device_id		# unique device ID for the sensor that does not change between power cycles
 float32[3] accel_bias		# accelerometer in-run bias in body frame (m/s^2)
 
+uint32 mag_device_id		# unique device ID for the sensor that does not change between power cycles
 float32[3] mag_bias		# magnetometer in-run bias in body frame (Gauss)

msg/sensor_correction.msg

@@ -55,3 +55,7 @@ uint8 selected_baro_instance	# barometric pressure uORB topic instance for the v
 uint8[3] gyro_mapping
 uint8[3] accel_mapping
 uint8[3] baro_mapping
+
+uint32[3] gyro_device_ids
+uint32[3] accel_device_ids
+uint32[3] baro_device_ids

msg/sensor_gyro.msg

@@ -1,22 +1,10 @@
-uint64 timestamp	# time since system start (microseconds)
+uint64 timestamp          # time since system start (microseconds)
-uint32 device_id	# unique device ID for the sensor that does not change between power cycles
+uint64 timestamp_sample
 
-uint64 error_count
+uint32 device_id          # unique device ID for the sensor that does not change between power cycles
 
-float32 x		# angular velocity in the NED X board axis in rad/s
+float32 x                 # angular velocity in the NED X board axis in rad/s
-float32 y		# angular velocity in the NED Y board axis in rad/s
+float32 y                 # angular velocity in the NED Y board axis in rad/s
-float32 z		# angular velocity in the NED Z board axis in rad/s
+float32 z                 # angular velocity in the NED Z board axis in rad/s
 
-uint32 integral_dt	# integration time (microseconds)
+float32 temperature       # temperature in degrees celsius
-uint8 integral_samples	# number of samples integrated
-float32 x_integral	# delta angle in the NED X board axis in rad/s over the integration time frame (integral_dt)
-float32 y_integral	# delta angle in the NED Y board axis in rad/s over the integration time frame (integral_dt)
-float32 z_integral	# delta angle in the NED Z board axis in rad/s over the integration time frame (integral_dt)
-uint8 integral_clip_count	# total clip count per integration period on any axis
-
-float32 temperature	# temperature in degrees celsius
-
-float32 scaling		# scaling from raw to rad/s
-int16 x_raw
-int16 y_raw
-int16 z_raw

msg/sensor_gyro_control.msg

@@ -1,11 +0,0 @@
-
-# WARNING: Not recommend for custom development.
-#  This topic is part of an incremental refactoring of the sensor pipeline and will likely disappear post PX4 release v1.10
-
-uint32 device_id	# unique device ID for the sensor that does not change between power cycles
-
-uint64 timestamp	# time since system start (microseconds)
-
-uint64 timestamp_sample	# time the raw data was sampled (microseconds)
-
-float32[3] xyz		# filtered angular velocity in the board axis in rad/s

msg/sensor_gyro_fifo.msg

@@ -1,13 +1,13 @@
-uint32 device_id	# unique device ID for the sensor that does not change between power cycles
+uint64 timestamp          # time since system start (microseconds)
+uint64 timestamp_sample
 
-uint64 timestamp	# time since system start (microseconds)
+uint32 device_id          # unique device ID for the sensor that does not change between power cycles
-uint64 timestamp_sample	# time since system start (microseconds)
 
-float32 dt		# delta time between samples (microseconds)
+float32 dt                # delta time between samples (microseconds)
 float32 scale
 
-uint8 samples		# number of valid samples
+uint8 samples             # number of valid samples
 
-int16[16] x		# angular velocity in the NED X board axis in rad/s
+int16[16] x               # angular velocity in the NED X board axis in rad/s
-int16[16] y		# angular velocity in the NED Y board axis in rad/s
+int16[16] y               # angular velocity in the NED Y board axis in rad/s
-int16[16] z		# angular velocity in the NED Z board axis in rad/s
+int16[16] z               # angular velocity in the NED Z board axis in rad/s

msg/sensor_gyro_integrated.msg

@@ -0,0 +1,13 @@
+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 temperature       # temperature in degrees celsius
+
+uint64 error_count
+
+float32[3] delta_angle    # delta angle in the NED board axis in rad/s over the integration time frame (dt)
+uint16 dt                 # integration time (microseconds)
+uint8 samples             # number of samples integrated
+uint8 clip_count          # total clip count per integration period on any axis

msg/sensor_gyro_status.msg

@@ -1,5 +1,6 @@
-uint64 timestamp	# time since system start (microseconds)
+uint64 timestamp          # time since system start (microseconds)
-uint32 device_id	# unique device ID for the sensor that does not change between power cycles
+
+uint32 device_id          # unique device ID for the sensor that does not change between power cycles
 
 uint64 error_count
 
@@ -7,13 +8,13 @@ float32 temperature
 
 uint8 rotation
 
-uint64[3] clipping	# clipping per axis
+uint32[3] clipping        # clipping per axis
 
 uint16 measure_rate
 uint16 sample_rate
 
 float32 full_scale_range
 
-float32 vibration_metric	# high frequency vibration level in the IMU delta angle data (rad)
+float32 vibration_metric  # high frequency vibration level in the IMU delta angle data (rad)
 
-float32 coning_vibration	# Level of coning vibration in the IMU delta angles (rad^2)
+float32 coning_vibration  # Level of coning vibration in the IMU delta angles (rad^2)

msg/tools/uorb_rtps_message_ids.yaml

@@ -256,8 +256,6 @@ rtps:
     id: 112
   - msg: vehicle_acceleration
     id: 113
-  - msg: sensor_gyro_control
-    id: 114
   - msg: airspeed_validated
     id: 115
   - msg: onboard_computer_status
@@ -273,6 +271,10 @@ rtps:
     id: 120
   - msg: sensor_gyro_status
     id: 121
+  - msg: sensor_accel_integrated
+    id: 122
+  - msg: sensor_gyro_integrated
+    id: 123
   ########## multi topics: begin ##########
   - msg: actuator_controls_0
     id: 150

src/lib/drivers/accelerometer/CMakeLists.txt

@@ -31,7 +31,10 @@
 #
 ############################################################################
 
-px4_add_library(drivers_accelerometer PX4Accelerometer.cpp)
+px4_add_library(drivers_accelerometer
+	PX4Accelerometer.cpp
+	PX4Accelerometer.hpp
+)
 target_link_libraries(drivers_accelerometer
 	PRIVATE
 		drivers__device

src/lib/drivers/accelerometer/PX4Accelerometer.cpp

@@ -44,6 +44,7 @@ PX4Accelerometer::PX4Accelerometer(uint32_t device_id, uint8_t priority, enum Ro
 	ModuleParams(nullptr),
 	_sensor_pub{ORB_ID(sensor_accel), priority},
 	_sensor_fifo_pub{ORB_ID(sensor_accel_fifo), priority},
+	_sensor_integrated_pub{ORB_ID(sensor_accel_integrated), priority},
 	_sensor_status_pub{ORB_ID(sensor_accel_status), priority},
 	_device_id{device_id},
 	_rotation{rotation}
@@ -62,8 +63,7 @@ PX4Accelerometer::~PX4Accelerometer()
 	}
 }
 
-int
+int PX4Accelerometer::ioctl(cdev::file_t *filp, int cmd, unsigned long arg)
-PX4Accelerometer::ioctl(cdev::file_t *filp, int cmd, unsigned long arg)
 {
 	switch (cmd) {
 	case ACCELIOCSSCALE: {
@@ -85,8 +85,7 @@ PX4Accelerometer::ioctl(cdev::file_t *filp, int cmd, unsigned long arg)
 	}
 }
 
-void
+void PX4Accelerometer::set_device_type(uint8_t devtype)
-PX4Accelerometer::set_device_type(uint8_t devtype)
 {
 	// current DeviceStructure
 	union device::Device::DeviceId device_id;
@@ -95,41 +94,36 @@ PX4Accelerometer::set_device_type(uint8_t devtype)
 	// update to new device type
 	device_id.devid_s.devtype = devtype;
 
-	// copy back to report
+	// copy back
 	_device_id = device_id.devid;
 }
 
-void
+void PX4Accelerometer::set_sample_rate(uint16_t rate)
-PX4Accelerometer::set_sample_rate(uint16_t rate)
 {
 	_sample_rate = rate;
 
 	ConfigureFilter(_filter.get_cutoff_freq());
 }
 
-void
+void PX4Accelerometer::set_update_rate(uint16_t rate)
-PX4Accelerometer::set_update_rate(uint16_t rate)
 {
 	const uint32_t update_interval = 1000000 / rate;
-
 	_integrator_reset_samples = 4000 / update_interval;
 }
 
-void
+void PX4Accelerometer::update(hrt_abstime timestamp_sample, float x, float y, float z)
-PX4Accelerometer::update(hrt_abstime timestamp, float x, float y, float z)
 {
 	// Apply rotation (before scaling)
 	rotate_3f(_rotation, x, y, z);
 
 	const Vector3f raw{x, y, z};
 
-	// Clipping
+	// Clipping (check unscaled raw values)
-	sensor_accel_status_s &status = _sensor_status_pub.get();
 	const float clip_limit = (_range / _scale) * 0.95f;
 
 	for (int i = 0; i < 3; i++) {
 		if (fabsf(raw(i)) > clip_limit) {
-			status.clipping[i]++;
+			_clipping[i]++;
 			_integrator_clipping++;
 		}
 	}
@@ -140,62 +134,61 @@ PX4Accelerometer::update(hrt_abstime timestamp, float x, float y, float z)
 	// Filtered values
 	const Vector3f val_filtered{_filter.apply(val_calibrated)};
 
+
 	// Integrated values
-	Vector3f integrated_value;
+	Vector3f delta_velocity;
 	uint32_t integral_dt = 0;
 
+	if (_integrator_samples == 0) {
+		_integrator_timestamp_sample = timestamp_sample;
+	}
+
 	_integrator_samples++;
 
-	if (_integrator.put(timestamp, val_calibrated, integrated_value, integral_dt)) {
+	if (_integrator.put(timestamp_sample, val_calibrated, delta_velocity, integral_dt)) {
 
-		sensor_accel_s report{};
+		// publish control data (filtered)
-		report.timestamp = timestamp;
+		{
+			sensor_accel_s report{};
+
+			report.timestamp_sample = timestamp_sample;
+			report.device_id = _device_id;
+			report.temperature = _temperature;
+			report.x = val_filtered(0);
+			report.y = val_filtered(1);
+			report.z = val_filtered(2);
+			report.timestamp = hrt_absolute_time();
+
+			_sensor_pub.publish(report);
+		}
+
+		// fill sensor_accel_integrated and publish
+		sensor_accel_integrated_s report{};
+
+		report.timestamp_sample = _integrator_timestamp_sample;
+		report.error_count = _error_count;
 		report.device_id = _device_id;
 		report.temperature = _temperature;
-		report.scaling = _scale;
+		delta_velocity.copyTo(report.delta_velocity);
-		report.error_count = _error_count;
+		report.dt = integral_dt;
+		report.samples = _integrator_samples;
+		report.clip_count = _integrator_clipping;
+		report.timestamp = hrt_absolute_time();
 
-		// Raw values (ADC units 0 - 65535)
+		_sensor_integrated_pub.publish(report);
-		report.x_raw = x;
-		report.y_raw = y;
-		report.z_raw = z;
 
-		report.x = val_filtered(0);
-		report.y = val_filtered(1);
-		report.z = val_filtered(2);
-
-		report.integral_dt = integral_dt;
-		report.integral_samples = _integrator_samples;
-		report.x_integral = integrated_value(0);
-		report.y_integral = integrated_value(1);
-		report.z_integral = integrated_value(2);
-		report.integral_clip_count = _integrator_clipping;
-
-		_sensor_pub.publish(report);
 
 		// reset integrator
 		ResetIntegrator();
 
 		// update vibration metrics
-		const Vector3f delta_velocity = integrated_value * (integral_dt * 1.e-6f);
 		UpdateVibrationMetrics(delta_velocity);
 	}
 
-	// publish status
+	PublishStatus();
-	status.device_id = _device_id;
-	status.error_count = _error_count;
-	status.full_scale_range = _range;
-	status.rotation = _rotation;
-	status.measure_rate = _update_rate;
-	status.sample_rate = _sample_rate;
-	status.temperature = _temperature;
-	status.vibration_metric = _vibration_metric;
-	status.timestamp = hrt_absolute_time();
-	_sensor_status_pub.publish(status);
 }
 
-void
+void PX4Accelerometer::updateFIFO(const FIFOSample &sample)
-PX4Accelerometer::updateFIFO(const FIFOSample &sample)
 {
 	// filtered data (control)
 	float x_filtered = _filterArrayX.apply(sample.x, sample.samples);
@@ -211,45 +204,31 @@ PX4Accelerometer::updateFIFO(const FIFOSample &sample)
 	const Vector3f val_calibrated{(((raw * _scale) - _calibration_offset).emult(_calibration_scale))};
 
 
-	// status
+	// clipping
-	{
+	const int16_t clip_limit = (_range / _scale) * 0.95f;
-		sensor_accel_status_s &status = _sensor_status_pub.get();
 
-		const int16_t clip_limit = (_range / _scale) * 0.95f;
+	// x clipping
-
+	for (int n = 0; n < sample.samples; n++) {
-		// x clipping
+		if (abs(sample.x[n]) > clip_limit) {
-		for (int n = 0; n < sample.samples; n++) {
+			_clipping[0]++;
-			if (abs(sample.x[n]) > clip_limit) {
+			_integrator_clipping++;
-				status.clipping[0]++;
-				_integrator_clipping++;
-			}
 		}
+	}
 
-		// y clipping
+	// y clipping
-		for (int n = 0; n < sample.samples; n++) {
+	for (int n = 0; n < sample.samples; n++) {
-			if (abs(sample.y[n]) > clip_limit) {
+		if (abs(sample.y[n]) > clip_limit) {
-				status.clipping[1]++;
+			_clipping[1]++;
-				_integrator_clipping++;
+			_integrator_clipping++;
-			}
 		}
+	}
 
-		// z clipping
+	// z clipping
-		for (int n = 0; n < sample.samples; n++) {
+	for (int n = 0; n < sample.samples; n++) {
-			if (abs(sample.z[n]) > clip_limit) {
+		if (abs(sample.z[n]) > clip_limit) {
-				status.clipping[2]++;
+			_clipping[2]++;
-				_integrator_clipping++;
+			_integrator_clipping++;
-			}
 		}
-
-		status.device_id = _device_id;
-		status.error_count = _error_count;
-		status.full_scale_range = _range;
-		status.rotation = _rotation;
-		status.measure_rate = _update_rate;
-		status.sample_rate = _sample_rate;
-		status.temperature = _temperature;
-		status.timestamp = hrt_absolute_time();
-		_sensor_status_pub.publish(status);
 	}
 
 
@@ -284,6 +263,22 @@ PX4Accelerometer::updateFIFO(const FIFOSample &sample)
 
 		if (_integrator_fifo_samples > 0 && (_integrator_samples >= _integrator_reset_samples)) {
 
+			// publish control data (filtered)
+			{
+				sensor_accel_s report{};
+
+				report.timestamp_sample = sample.timestamp_sample + ((sample.samples - 1) * sample.dt); // timestamp of last sample
+				report.device_id = _device_id;
+				report.temperature = _temperature;
+				report.x = val_calibrated(0);
+				report.y = val_calibrated(1);
+				report.z = val_calibrated(2);
+				report.timestamp = hrt_absolute_time();
+
+				_sensor_pub.publish(report);
+			}
+
+
 			const uint32_t integrator_dt_us = _integrator_fifo_samples * sample.dt; // time span in microseconds
 
 			// average integrated values to apply calibration
@@ -297,37 +292,25 @@ PX4Accelerometer::updateFIFO(const FIFOSample &sample)
 			const Vector3f raw_int{x_int_avg, y_int_avg, z_int_avg};
 
 			// Apply range scale and the calibrating offset/scale
-			Vector3f val_int_calibrated{(((raw_int * _scale) - _calibration_offset).emult(_calibration_scale))};
+			Vector3f delta_velocity{(((raw_int * _scale) - _calibration_offset).emult(_calibration_scale))};
-			val_int_calibrated *= (_integrator_fifo_samples * sample.dt * 1e-6f);	// restore
+			delta_velocity *= (_integrator_fifo_samples * sample.dt * 1e-6f);	// restore
 
-			// publish
+			// fill sensor_accel_integrated and publish
-			sensor_accel_s report{};
+			sensor_accel_integrated_s report{};
+
+			report.timestamp_sample = _integrator_timestamp_sample;
+			report.error_count = _error_count;
 			report.device_id = _device_id;
 			report.temperature = _temperature;
-			report.scaling = _scale;
+			delta_velocity.copyTo(report.delta_velocity);
-			report.error_count = _error_count;
+			report.dt = integrator_dt_us;
+			report.samples = _integrator_fifo_samples;
+			report.clip_count = _integrator_clipping;
 
-			// Raw values (ADC units 0 - 65535)
+			report.timestamp = hrt_absolute_time();
-			report.x_raw = sample.x[0];
+			_sensor_integrated_pub.publish(report);
-			report.y_raw = sample.y[0];
-			report.z_raw = sample.z[0];
-
-			report.x = val_calibrated(0);
-			report.y = val_calibrated(1);
-			report.z = val_calibrated(2);
-
-			report.integral_dt = integrator_dt_us;
-			report.integral_samples = _integrator_fifo_samples;
-			report.x_integral = val_int_calibrated(0);
-			report.y_integral = val_int_calibrated(1);
-			report.z_integral = val_int_calibrated(2);
-			report.integral_clip_count = _integrator_clipping;
-
-			report.timestamp = _integrator_timestamp_sample;
-			_sensor_pub.publish(report);
 
 			// update vibration metrics
-			const Vector3f delta_velocity = val_int_calibrated * (integrator_dt_us * 1.e-6f);
 			UpdateVibrationMetrics(delta_velocity);
 
 			// reset integrator
@@ -337,6 +320,7 @@ PX4Accelerometer::updateFIFO(const FIFOSample &sample)
 		_timestamp_sample_prev = sample.timestamp_sample;
 	}
 
+	// publish sensor fifo
 	sensor_accel_fifo_s fifo{};
 
 	fifo.device_id = _device_id;
@@ -351,10 +335,36 @@ PX4Accelerometer::updateFIFO(const FIFOSample &sample)
 
 	fifo.timestamp = hrt_absolute_time();
 	_sensor_fifo_pub.publish(fifo);
+
+
+	PublishStatus();
 }
 
-void
+void PX4Accelerometer::PublishStatus()
-PX4Accelerometer::ResetIntegrator()
+{
+	// publish sensor status
+	if (hrt_elapsed_time(&_status_last_publish) >= 100_ms) {
+		sensor_accel_status_s status{};
+
+		status.device_id = _device_id;
+		status.error_count = _error_count;
+		status.full_scale_range = _range;
+		status.rotation = _rotation;
+		status.measure_rate = _update_rate;
+		status.sample_rate = _sample_rate;
+		status.temperature = _temperature;
+		status.vibration_metric = _vibration_metric;
+		status.clipping[0] = _clipping[0];
+		status.clipping[1] = _clipping[1];
+		status.clipping[2] = _clipping[2];
+		status.timestamp = hrt_absolute_time();
+		_sensor_status_pub.publish(status);
+
+		_status_last_publish = status.timestamp;
+	}
+}
+
+void PX4Accelerometer::ResetIntegrator()
 {
 	_integrator_samples = 0;
 	_integrator_fifo_samples = 0;
@@ -367,8 +377,7 @@ PX4Accelerometer::ResetIntegrator()
 	_timestamp_sample_prev = 0;
 }
 
-void
+void PX4Accelerometer::ConfigureFilter(float cutoff_freq)
-PX4Accelerometer::ConfigureFilter(float cutoff_freq)
 {
 	_filter.set_cutoff_frequency(_sample_rate, cutoff_freq);
 
@@ -377,8 +386,7 @@ PX4Accelerometer::ConfigureFilter(float cutoff_freq)
 	_filterArrayZ.set_cutoff_frequency(_sample_rate, cutoff_freq);
 }
 
-void
+void PX4Accelerometer::UpdateVibrationMetrics(const Vector3f &delta_velocity)
-PX4Accelerometer::UpdateVibrationMetrics(const Vector3f &delta_velocity)
 {
 	// Accel high frequency vibe = filtered length of (delta_velocity - prev_delta_velocity)
 	const Vector3f delta_velocity_diff = delta_velocity - _delta_velocity_prev;
@@ -387,8 +395,7 @@ PX4Accelerometer::UpdateVibrationMetrics(const Vector3f &delta_velocity)
 	_delta_velocity_prev = delta_velocity;
 }
 
-void
+void PX4Accelerometer::print_status()
-PX4Accelerometer::print_status()
 {
 	PX4_INFO(ACCEL_BASE_DEVICE_PATH " device instance: %d", _class_device_instance);
 	PX4_INFO("sample rate: %d Hz", _sample_rate);
@@ -398,5 +405,4 @@ PX4Accelerometer::print_status()
 		 (double)_calibration_scale(2));
 	PX4_INFO("calibration offset: %.5f %.5f %.5f", (double)_calibration_offset(0), (double)_calibration_offset(1),
 		 (double)_calibration_offset(2));
-
 }

src/lib/drivers/accelerometer/PX4Accelerometer.hpp

@@ -45,11 +45,11 @@
 #include <uORB/PublicationMulti.hpp>
 #include <uORB/topics/sensor_accel.h>
 #include <uORB/topics/sensor_accel_fifo.h>
+#include <uORB/topics/sensor_accel_integrated.h>
 #include <uORB/topics/sensor_accel_status.h>
 
 class PX4Accelerometer : public cdev::CDev, public ModuleParams
 {
-
 public:
 	PX4Accelerometer(uint32_t device_id, uint8_t priority = ORB_PRIO_DEFAULT, enum Rotation rotation = ROTATION_NONE);
 	~PX4Accelerometer() override;
@@ -67,7 +67,7 @@ public:
 	void set_temperature(float temperature) { _temperature = temperature; }
 	void set_update_rate(uint16_t rate);
 
-	void update(hrt_abstime timestamp, float x, float y, float z);
+	void update(hrt_abstime timestamp_sample, float x, float y, float z);
 
 	void print_status();
 
@@ -89,12 +89,14 @@ public:
 private:
 
 	void ConfigureFilter(float cutoff_freq);
+	void PublishStatus();
 	void ResetIntegrator();
 	void UpdateVibrationMetrics(const matrix::Vector3f &delta_velocity);
 
-	uORB::PublicationMulti<sensor_accel_s>			_sensor_pub;		// legacy message
+	uORB::PublicationMulti<sensor_accel_s>            _sensor_pub;
-	uORB::PublicationMulti<sensor_accel_fifo_s>		_sensor_fifo_pub;
+	uORB::PublicationMulti<sensor_accel_fifo_s>       _sensor_fifo_pub;
-	uORB::PublicationMultiData<sensor_accel_status_s>	_sensor_status_pub;
+	uORB::PublicationMulti<sensor_accel_integrated_s> _sensor_integrated_pub;
+	uORB::PublicationMulti<sensor_accel_status_s>     _sensor_status_pub;
 
 	math::LowPassFilter2pVector3f _filter{1000, 100};
 
@@ -102,6 +104,8 @@ private:
 	math::LowPassFilter2pArray _filterArrayY{4000, 100};
 	math::LowPassFilter2pArray _filterArrayZ{4000, 100};
 
+	hrt_abstime	_status_last_publish{0};
+
 	Integrator		_integrator{4000, false};
 
 	matrix::Vector3f	_calibration_scale{1.0f, 1.0f, 1.0f};
@@ -112,9 +116,7 @@ private:
 
 	int			_class_device_instance{-1};
 
-
 	uint32_t		_device_id{0};
-
 	const enum Rotation	_rotation;
 
 	float			_range{16.0f * CONSTANTS_ONE_G};
@@ -123,6 +125,8 @@ private:
 
 	uint64_t		_error_count{0};
 
+	uint32_t		_clipping[3] {};
+
 	uint16_t		_sample_rate{1000};
 	uint16_t		_update_rate{1000};
 
@@ -138,5 +142,4 @@ private:
 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::IMU_ACCEL_CUTOFF>) _param_imu_accel_cutoff
 	)
-
 };

src/lib/drivers/gyroscope/CMakeLists.txt

@@ -31,5 +31,8 @@
 #
 ############################################################################
 
-px4_add_library(drivers_gyroscope PX4Gyroscope.cpp)
+px4_add_library(drivers_gyroscope
+	PX4Gyroscope.cpp
+	PX4Gyroscope.hpp
+)
 target_link_libraries(drivers_gyroscope PRIVATE drivers__device)

src/lib/drivers/gyroscope/PX4Gyroscope.cpp

@@ -43,8 +43,8 @@ PX4Gyroscope::PX4Gyroscope(uint32_t device_id, uint8_t priority, enum Rotation r
 	CDev(nullptr),
 	ModuleParams(nullptr),
 	_sensor_pub{ORB_ID(sensor_gyro), priority},
-	_sensor_control_pub{ORB_ID(sensor_gyro_control), priority},
 	_sensor_fifo_pub{ORB_ID(sensor_gyro_fifo), priority},
+	_sensor_integrated_pub{ORB_ID(sensor_gyro_integrated), priority},
 	_sensor_status_pub{ORB_ID(sensor_gyro_status), priority},
 	_device_id{device_id},
 	_rotation{rotation}
@@ -64,8 +64,7 @@ PX4Gyroscope::~PX4Gyroscope()
 	}
 }
 
-int
+int PX4Gyroscope::ioctl(cdev::file_t *filp, int cmd, unsigned long arg)
-PX4Gyroscope::ioctl(cdev::file_t *filp, int cmd, unsigned long arg)
 {
 	switch (cmd) {
 	case GYROIOCSSCALE: {
@@ -86,8 +85,7 @@ PX4Gyroscope::ioctl(cdev::file_t *filp, int cmd, unsigned long arg)
 	}
 }
 
-void
+void PX4Gyroscope::set_device_type(uint8_t devtype)
-PX4Gyroscope::set_device_type(uint8_t devtype)
 {
 	// current DeviceStructure
 	union device::Device::DeviceId device_id;
@@ -96,12 +94,11 @@ PX4Gyroscope::set_device_type(uint8_t devtype)
 	// update to new device type
 	device_id.devid_s.devtype = devtype;
 
-	// copy back to report
+	// copy back
 	_device_id = device_id.devid;
 }
 
-void
+void PX4Gyroscope::set_sample_rate(uint16_t rate)
-PX4Gyroscope::set_sample_rate(uint16_t rate)
 {
 	_sample_rate = rate;
 
@@ -109,29 +106,25 @@ PX4Gyroscope::set_sample_rate(uint16_t rate)
 	ConfigureNotchFilter(_notch_filter.getNotchFreq(), _notch_filter.getBandwidth());
 }
 
-void
+void PX4Gyroscope::set_update_rate(uint16_t rate)
-PX4Gyroscope::set_update_rate(uint16_t rate)
 {
 	const uint32_t update_interval = 1000000 / rate;
-
 	_integrator_reset_samples = 4000 / update_interval;
 }
 
-void
+void PX4Gyroscope::update(hrt_abstime timestamp_sample, float x, float y, float z)
-PX4Gyroscope::update(hrt_abstime timestamp, float x, float y, float z)
 {
 	// Apply rotation (before scaling)
 	rotate_3f(_rotation, x, y, z);
 
 	const Vector3f raw{x, y, z};
 
-	// Clipping
+	// Clipping (check unscaled raw values)
-	sensor_gyro_status_s &status = _sensor_status_pub.get();
 	const float clip_limit = (_range / _scale) * 0.95f;
 
 	for (int i = 0; i < 3; i++) {
 		if (fabsf(raw(i)) > clip_limit) {
-			status.clipping[i]++;
+			_clipping[i]++;
 			_integrator_clipping++;
 		}
 	}
@@ -143,86 +136,61 @@ PX4Gyroscope::update(hrt_abstime timestamp, float x, float y, float z)
 	Vector3f val_filtered{_notch_filter.apply(val_calibrated)};
 	val_filtered = _filter.apply(val_filtered);
 
+
 	// publish control data (filtered) immediately
-	bool publish_control = true;
+	{
-	sensor_gyro_control_s control{};
-
-	if (_param_imu_gyro_rate_max.get() > 0) {
-		const uint64_t interval = 1e6f / _param_imu_gyro_rate_max.get();
-
-		if (hrt_elapsed_time(&_control_last_publish) < interval) {
-			publish_control = false;
-		}
-	}
-
-	if (publish_control) {
-		control.timestamp_sample = timestamp;
-		control.device_id = _device_id;
-		val_filtered.copyTo(control.xyz);
-		control.timestamp = hrt_absolute_time();
-		_sensor_control_pub.publish(control);
-
-		_control_last_publish = control.timestamp_sample;
-	}
-
-
-	// Integrated values
-	Vector3f integrated_value;
-	uint32_t integral_dt = 0;
-
-	_integrator_samples++;
-
-	if (_integrator.put(timestamp, val_calibrated, integrated_value, integral_dt)) {
-
 		sensor_gyro_s report{};
-		report.timestamp = timestamp;
+
+		report.timestamp_sample = timestamp_sample;
 		report.device_id = _device_id;
 		report.temperature = _temperature;
-		report.scaling = _scale;
-		report.error_count = _error_count;
-
-		// Raw values (ADC units 0 - 65535)
-		report.x_raw = x;
-		report.y_raw = y;
-		report.z_raw = z;
-
 		report.x = val_filtered(0);
 		report.y = val_filtered(1);
 		report.z = val_filtered(2);
-
+		report.timestamp = hrt_absolute_time();
-		report.integral_dt = integral_dt;
-		report.integral_samples = _integrator_samples;
-		report.x_integral = integrated_value(0);
-		report.y_integral = integrated_value(1);
-		report.z_integral = integrated_value(2);
-		report.integral_clip_count = _integrator_clipping;
 
 		_sensor_pub.publish(report);
+	}
+
+	// Integrated values
+	Vector3f delta_angle;
+	uint32_t integral_dt = 0;
+
+	if (_integrator_samples == 0) {
+		_integrator_timestamp_sample = timestamp_sample;
+	}
+
+	_integrator_samples++;
+
+	if (_integrator.put(timestamp_sample, val_calibrated, delta_angle, integral_dt)) {
+
+		// fill sensor_gyro_integrated and publish
+		sensor_gyro_integrated_s report{};
+
+		report.timestamp_sample = _integrator_timestamp_sample;
+		report.error_count = _error_count;
+		report.device_id = _device_id;
+		report.temperature = _temperature;
+		delta_angle.copyTo(report.delta_angle);
+		report.dt = integral_dt;
+		report.samples = _integrator_samples;
+		report.clip_count = _integrator_clipping;
+		report.timestamp = hrt_absolute_time();
+
+		_sensor_integrated_pub.publish(report);
+
 
 		// reset integrator
 		ResetIntegrator();
 
 		// update vibration metrics
-		const Vector3f delta_angle = integrated_value * (integral_dt * 1.e-6f);
 		UpdateVibrationMetrics(delta_angle);
 	}
 
-	// publish status
+	PublishStatus();
-	status.device_id = _device_id;
-	status.error_count = _error_count;
-	status.full_scale_range = _range;
-	status.rotation = _rotation;
-	status.measure_rate = _update_rate;
-	status.sample_rate = _sample_rate;
-	status.temperature = _temperature;
-	status.vibration_metric = _vibration_metric;
-	status.coning_vibration = _coning_vibration;
-	status.timestamp = hrt_absolute_time();
-	_sensor_status_pub.publish(status);
 }
 
-void
+void PX4Gyroscope::updateFIFO(const FIFOSample &sample)
-PX4Gyroscope::updateFIFO(const FIFOSample &sample)
 {
 	// filtered data (control)
 	float x_filtered = _filterArrayX.apply(sample.x, sample.samples);
@@ -238,11 +206,9 @@ PX4Gyroscope::updateFIFO(const FIFOSample &sample)
 	const Vector3f val_calibrated{(raw * _scale) - _calibration_offset};
 
 
-	// control
+	// publish control data (filtered) immediately
 	{
-		// publish control data (filtered) immediately
 		bool publish_control = true;
-		sensor_gyro_control_s control{};
 
 		if (_param_imu_gyro_rate_max.get() > 0) {
 			const uint64_t interval = 1e6f / _param_imu_gyro_rate_max.get();
@@ -253,56 +219,48 @@ PX4Gyroscope::updateFIFO(const FIFOSample &sample)
 		}
 
 		if (publish_control) {
-			control.timestamp_sample = sample.timestamp_sample + ((sample.samples - 1) * sample.dt); // timestamp of last sample
+			sensor_gyro_s report{};
-			control.device_id = _device_id;
-			val_calibrated.copyTo(control.xyz);
-			control.timestamp = hrt_absolute_time();
-			_sensor_control_pub.publish(control);
 
-			_control_last_publish = control.timestamp_sample;
+			report.timestamp_sample = sample.timestamp_sample + ((sample.samples - 1) * sample.dt); // timestamp of last sample
+			report.device_id = _device_id;
+			report.temperature = _temperature;
+			report.x = val_calibrated(0);
+			report.y = val_calibrated(1);
+			report.z = val_calibrated(2);
+			report.timestamp = hrt_absolute_time();
+
+			_sensor_pub.publish(report);
+
+			_control_last_publish = report.timestamp_sample;
 		}
 	}
 
 
-	// status
+	// clipping
-	{
+	const int16_t clip_limit = (_range / _scale) * 0.95f;
-		sensor_gyro_status_s &status = _sensor_status_pub.get();
 
-		const int16_t clip_limit = (_range / _scale) * 0.95f;
+	// x clipping
-
+	for (int n = 0; n < sample.samples; n++) {
-		// x clipping
+		if (abs(sample.x[n]) > clip_limit) {
-		for (int n = 0; n < sample.samples; n++) {
+			_clipping[0]++;
-			if (abs(sample.x[n]) > clip_limit) {
+			_integrator_clipping++;
-				status.clipping[0]++;
-				_integrator_clipping++;
-			}
 		}
+	}
 
-		// y clipping
+	// y clipping
-		for (int n = 0; n < sample.samples; n++) {
+	for (int n = 0; n < sample.samples; n++) {
-			if (abs(sample.y[n]) > clip_limit) {
+		if (abs(sample.y[n]) > clip_limit) {
-				status.clipping[1]++;
+			_clipping[1]++;
-				_integrator_clipping++;
+			_integrator_clipping++;
-			}
 		}
+	}
 
-		// z clipping
+	// z clipping
-		for (int n = 0; n < sample.samples; n++) {
+	for (int n = 0; n < sample.samples; n++) {
-			if (abs(sample.z[n]) > clip_limit) {
+		if (abs(sample.z[n]) > clip_limit) {
-				status.clipping[2]++;
+			_clipping[2]++;
-				_integrator_clipping++;
+			_integrator_clipping++;
-			}
 		}
-
-		status.device_id = _device_id;
-		status.error_count = _error_count;
-		status.full_scale_range = _range;
-		status.rotation = _rotation;
-		status.measure_rate = _update_rate;
-		status.sample_rate = _sample_rate;
-		status.temperature = _temperature;
-		status.timestamp = hrt_absolute_time();
-		_sensor_status_pub.publish(status);
 	}
 
 
@@ -350,37 +308,25 @@ PX4Gyroscope::updateFIFO(const FIFOSample &sample)
 			const Vector3f raw_int{x_int_avg, y_int_avg, z_int_avg};
 
 			// Apply range scale and the calibrating offset/scale
-			Vector3f val_int_calibrated{(raw_int * _scale) - _calibration_offset};
+			Vector3f delta_angle{(raw_int * _scale) - _calibration_offset};
-			val_int_calibrated *= (_integrator_fifo_samples * sample.dt * 1e-6f);	// restore
+			delta_angle *= (_integrator_fifo_samples * sample.dt * 1e-6f);	// restore
 
-			// publish
+			// fill sensor_gyro_integrated and publish
-			sensor_gyro_s report{};
+			sensor_gyro_integrated_s report{};
+
+			report.timestamp_sample = _integrator_timestamp_sample;
+			report.error_count = _error_count;
 			report.device_id = _device_id;
 			report.temperature = _temperature;
-			report.scaling = _scale;
+			delta_angle.copyTo(report.delta_angle);
-			report.error_count = _error_count;
+			report.dt = integrator_dt_us;
+			report.samples = _integrator_fifo_samples;
+			report.clip_count = _integrator_clipping;
 
-			// Raw values (ADC units 0 - 65535)
+			report.timestamp = hrt_absolute_time();
-			report.x_raw = sample.x[0];
+			_sensor_integrated_pub.publish(report);
-			report.y_raw = sample.y[0];
-			report.z_raw = sample.z[0];
-
-			report.x = val_calibrated(0);
-			report.y = val_calibrated(1);
-			report.z = val_calibrated(2);
-
-			report.integral_dt = integrator_dt_us;
-			report.integral_samples = _integrator_fifo_samples;
-			report.x_integral = val_int_calibrated(0);
-			report.y_integral = val_int_calibrated(1);
-			report.z_integral = val_int_calibrated(2);
-			report.integral_clip_count = _integrator_clipping;
-
-			report.timestamp = _integrator_timestamp_sample;
-			_sensor_pub.publish(report);
 
 			// update vibration metrics
-			const Vector3f delta_angle = val_int_calibrated * (integrator_dt_us * 1.e-6f);
 			UpdateVibrationMetrics(delta_angle);
 
 			// reset integrator
@@ -390,6 +336,7 @@ PX4Gyroscope::updateFIFO(const FIFOSample &sample)
 		_timestamp_sample_prev = sample.timestamp_sample;
 	}
 
+	// publish sensor fifo
 	sensor_gyro_fifo_s fifo{};
 
 	fifo.device_id = _device_id;
@@ -404,10 +351,37 @@ PX4Gyroscope::updateFIFO(const FIFOSample &sample)
 
 	fifo.timestamp = hrt_absolute_time();
 	_sensor_fifo_pub.publish(fifo);
+
+
+	PublishStatus();
 }
 
-void
+void PX4Gyroscope::PublishStatus()
-PX4Gyroscope::ResetIntegrator()
+{
+	// publish sensor status
+	if (hrt_elapsed_time(&_status_last_publish) >= 100_ms) {
+		sensor_gyro_status_s status{};
+
+		status.device_id = _device_id;
+		status.error_count = _error_count;
+		status.full_scale_range = _range;
+		status.rotation = _rotation;
+		status.measure_rate = _update_rate;
+		status.sample_rate = _sample_rate;
+		status.temperature = _temperature;
+		status.vibration_metric = _vibration_metric;
+		status.coning_vibration = _coning_vibration;
+		status.clipping[0] = _clipping[0];
+		status.clipping[1] = _clipping[1];
+		status.clipping[2] = _clipping[2];
+		status.timestamp = hrt_absolute_time();
+		_sensor_status_pub.publish(status);
+
+		_status_last_publish = status.timestamp;
+	}
+}
+
+void PX4Gyroscope::ResetIntegrator()
 {
 	_integrator_samples = 0;
 	_integrator_fifo_samples = 0;
@@ -420,8 +394,7 @@ PX4Gyroscope::ResetIntegrator()
 	_timestamp_sample_prev = 0;
 }
 
-void
+void PX4Gyroscope::ConfigureFilter(float cutoff_freq)
-PX4Gyroscope::ConfigureFilter(float cutoff_freq)
 {
 	_filter.set_cutoff_frequency(_sample_rate, cutoff_freq);
 
@@ -430,14 +403,12 @@ PX4Gyroscope::ConfigureFilter(float cutoff_freq)
 	_filterArrayZ.set_cutoff_frequency(_sample_rate, cutoff_freq);
 }
 
-void
+void PX4Gyroscope::ConfigureNotchFilter(float notch_freq, float bandwidth)
-PX4Gyroscope::ConfigureNotchFilter(float notch_freq, float bandwidth)
 {
 	_notch_filter.setParameters(_sample_rate, notch_freq, bandwidth);
 }
 
-void
+void PX4Gyroscope::UpdateVibrationMetrics(const Vector3f &delta_angle)
-PX4Gyroscope::UpdateVibrationMetrics(const Vector3f &delta_angle)
 {
 	// Gyro high frequency vibe = filtered length of (delta_angle - prev_delta_angle)
 	const Vector3f delta_angle_diff = delta_angle - _delta_angle_prev;
@@ -450,8 +421,7 @@ PX4Gyroscope::UpdateVibrationMetrics(const Vector3f &delta_angle)
 	_delta_angle_prev = delta_angle;
 }
 
-void
+void PX4Gyroscope::print_status()
-PX4Gyroscope::print_status()
 {
 	PX4_INFO(GYRO_BASE_DEVICE_PATH " device instance: %d", _class_device_instance);
 	PX4_INFO("sample rate: %d Hz", _sample_rate);
@@ -461,5 +431,4 @@ PX4Gyroscope::print_status()
 
 	PX4_INFO("calibration offset: %.5f %.5f %.5f", (double)_calibration_offset(0), (double)_calibration_offset(1),
 		 (double)_calibration_offset(2));
-
 }

src/lib/drivers/gyroscope/PX4Gyroscope.hpp

@@ -44,13 +44,12 @@
 #include <px4_platform_common/module_params.h>
 #include <uORB/PublicationMulti.hpp>
 #include <uORB/topics/sensor_gyro.h>
-#include <uORB/topics/sensor_gyro_control.h>
 #include <uORB/topics/sensor_gyro_fifo.h>
+#include <uORB/topics/sensor_gyro_integrated.h>
 #include <uORB/topics/sensor_gyro_status.h>
 
 class PX4Gyroscope : public cdev::CDev, public ModuleParams
 {
-
 public:
 	PX4Gyroscope(uint32_t device_id, uint8_t priority = ORB_PRIO_DEFAULT, enum Rotation rotation = ROTATION_NONE);
 	~PX4Gyroscope() override;
@@ -68,7 +67,7 @@ public:
 	void set_temperature(float temperature) { _temperature = temperature; }
 	void set_update_rate(uint16_t rate);
 
-	void update(hrt_abstime timestamp, float x, float y, float z);
+	void update(hrt_abstime timestamp_sample, float x, float y, float z);
 
 	void print_status();
 
@@ -91,18 +90,20 @@ private:
 
 	void ConfigureFilter(float cutoff_freq);
 	void ConfigureNotchFilter(float notch_freq, float bandwidth);
+	void PublishStatus();
 	void ResetIntegrator();
 	void UpdateVibrationMetrics(const matrix::Vector3f &delta_angle);
 
-	uORB::PublicationMulti<sensor_gyro_s>			_sensor_pub;		// legacy message
+	uORB::PublicationMulti<sensor_gyro_s>            _sensor_pub;
-	uORB::PublicationMulti<sensor_gyro_control_s>		_sensor_control_pub;
+	uORB::PublicationMulti<sensor_gyro_fifo_s>       _sensor_fifo_pub;
-	uORB::PublicationMulti<sensor_gyro_fifo_s>		_sensor_fifo_pub;
+	uORB::PublicationMulti<sensor_gyro_integrated_s> _sensor_integrated_pub;
-	uORB::PublicationMultiData<sensor_gyro_status_s>	_sensor_status_pub;
+	uORB::PublicationMulti<sensor_gyro_status_s>     _sensor_status_pub;
 
 	math::LowPassFilter2pVector3f _filter{1000, 100};
 	math::NotchFilter<matrix::Vector3f> _notch_filter{};
 
 	hrt_abstime	_control_last_publish{0};
+	hrt_abstime	_status_last_publish{0};
 
 	math::LowPassFilter2pArray _filterArrayX{8000, 100};
 	math::LowPassFilter2pArray _filterArrayY{8000, 100};
@@ -118,9 +119,7 @@ private:
 
 	int			_class_device_instance{-1};
 
-
 	uint32_t		_device_id{0};
-
 	const enum Rotation	_rotation;
 
 	float			_range{math::radians(2000.0f)};
@@ -129,6 +128,8 @@ private:
 
 	uint64_t		_error_count{0};
 
+	uint32_t		_clipping[3] {};
+
 	uint16_t		_sample_rate{1000};
 	uint16_t		_update_rate{1000};
 
@@ -147,5 +148,4 @@ private:
 		(ParamFloat<px4::params::IMU_GYRO_NF_BW>) _param_imu_gyro_nf_bw,
 		(ParamInt<px4::params::IMU_GYRO_RATEMAX>) _param_imu_gyro_rate_max
 	)
-
 };

src/lib/drivers/magnetometer/PX4Magnetometer.cpp

@@ -54,8 +54,7 @@ PX4Magnetometer::~PX4Magnetometer()
 	}
 }
 
-int
+int PX4Magnetometer::ioctl(cdev::file_t *filp, int cmd, unsigned long arg)
-PX4Magnetometer::ioctl(cdev::file_t *filp, int cmd, unsigned long arg)
 {
 	switch (cmd) {
 	case MAGIOCSSCALE: {
@@ -91,8 +90,7 @@ PX4Magnetometer::ioctl(cdev::file_t *filp, int cmd, unsigned long arg)
 	}
 }
 
-void
+void PX4Magnetometer::set_device_type(uint8_t devtype)
-PX4Magnetometer::set_device_type(uint8_t devtype)
 {
 	// current DeviceStructure
 	union device::Device::DeviceId device_id;
@@ -105,19 +103,15 @@ PX4Magnetometer::set_device_type(uint8_t devtype)
 	_sensor_mag_pub.get().device_id = device_id.devid;
 }
 
-void
+void PX4Magnetometer::update(hrt_abstime timestamp_sample, float x, float y, float z)
-PX4Magnetometer::update(hrt_abstime timestamp, int16_t x, int16_t y, int16_t z)
 {
 	sensor_mag_s &report = _sensor_mag_pub.get();
-	report.timestamp = timestamp;
+	report.timestamp = timestamp_sample;
 
 	// Apply rotation (before scaling)
-	float xraw_f = x;
+	rotate_3f(_rotation, x, y, z);
-	float yraw_f = y;
-	float zraw_f = z;
-	rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);
 
-	const matrix::Vector3f raw_f{xraw_f, yraw_f, zraw_f};
+	const matrix::Vector3f raw_f{x, y, z};
 
 	// Apply range scale and the calibrating offset/scale
 	const matrix::Vector3f val_calibrated{(((raw_f.emult(_sensitivity) * report.scaling) - _calibration_offset).emult(_calibration_scale))};
@@ -134,8 +128,7 @@ PX4Magnetometer::update(hrt_abstime timestamp, int16_t x, int16_t y, int16_t z)
 	_sensor_mag_pub.update();
 }
 
-void
+void PX4Magnetometer::print_status()
-PX4Magnetometer::print_status()
 {
 	PX4_INFO(MAG_BASE_DEVICE_PATH " device instance: %d", _class_device_instance);
 
@@ -143,6 +136,4 @@ PX4Magnetometer::print_status()
 		 (double)_calibration_scale(2));
 	PX4_INFO("calibration offset: %.5f %.5f %.5f", (double)_calibration_offset(0), (double)_calibration_offset(1),
 		 (double)_calibration_offset(2));
-
-	print_message(_sensor_mag_pub.get());
 }

src/lib/drivers/magnetometer/PX4Magnetometer.hpp

@@ -45,7 +45,7 @@ class PX4Magnetometer : public cdev::CDev
 {
 
 public:
-	PX4Magnetometer(uint32_t device_id, uint8_t priority, enum Rotation rotation);
+	PX4Magnetometer(uint32_t device_id, uint8_t priority = ORB_PRIO_DEFAULT, enum Rotation rotation = ROTATION_NONE);
 	~PX4Magnetometer() override;
 
 	int	ioctl(cdev::file_t *filp, int cmd, unsigned long arg) override;
@@ -57,7 +57,7 @@ public:
 	void set_external(bool external) { _sensor_mag_pub.get().is_external = external; }
 	void set_sensitivity(float x, float y, float z) { _sensitivity = matrix::Vector3f{x, y, z}; }
 
-	void update(hrt_abstime timestamp, int16_t x, int16_t y, int16_t z);
+	void update(hrt_abstime timestamp_sample, float x, float y, float z);
 
 	void print_status();
 

src/modules/ekf2/ekf2_main.cpp

@@ -1472,6 +1472,10 @@ void Ekf2::Run()
 
 				bias.timestamp = now;
 
+				bias.gyro_device_id = _sensor_selection.gyro_device_id;
+				bias.accel_device_id = _sensor_selection.accel_device_id;
+				bias.mag_device_id = _sensor_selection.mag_device_id;
+
 				// In-run bias estimates
 				_ekf.get_gyro_bias(bias.gyro_bias);
 				_ekf.get_accel_bias(bias.accel_bias);

src/modules/logger/logged_topics.cpp

@@ -69,6 +69,7 @@ void LoggedTopics::add_default_topics()
 	add_topic("position_setpoint_triplet", 200);
 	add_topic("radio_status");
 	add_topic("rate_ctrl_status", 200);
+	add_topic("sensor_bias", 1000);
 	add_topic("sensor_combined", 100);
 	add_topic("sensor_correction", 1000);
 	add_topic("sensor_preflight", 200);

src/modules/mavlink/mavlink_messages.cpp

@@ -60,13 +60,14 @@
 #include <uORB/topics/actuator_outputs.h>
 #include <uORB/topics/airspeed_validated.h>
 #include <uORB/topics/battery_status.h>
-#include <uORB/topics/camera_trigger.h>
 #include <uORB/topics/camera_capture.h>
+#include <uORB/topics/camera_trigger.h>
+#include <uORB/topics/collision_report.h>
 #include <uORB/topics/cpuload.h>
+#include <uORB/topics/debug_array.h>
 #include <uORB/topics/debug_key_value.h>
 #include <uORB/topics/debug_value.h>
 #include <uORB/topics/debug_vect.h>
-#include <uORB/topics/debug_array.h>
 #include <uORB/topics/differential_pressure.h>
 #include <uORB/topics/distance_sensor.h>
 #include <uORB/topics/estimator_status.h>
@@ -75,18 +76,22 @@
 #include <uORB/topics/input_rc.h>
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/mavlink_log.h>
-#include <uORB/topics/vehicle_trajectory_waypoint.h>
+#include <uORB/topics/mount_orientation.h>
 #include <uORB/topics/obstacle_distance.h>
 #include <uORB/topics/optical_flow.h>
 #include <uORB/topics/orbit_status.h>
 #include <uORB/topics/position_controller_status.h>
 #include <uORB/topics/position_setpoint_triplet.h>
+#include <uORB/topics/sensor_accel_integrated.h>
 #include <uORB/topics/sensor_accel_status.h>
-#include <uORB/topics/sensor_combined.h>
 #include <uORB/topics/sensor_bias.h>
+#include <uORB/topics/sensor_combined.h>
+#include <uORB/topics/sensor_gyro_integrated.h>
+#include <uORB/topics/sensor_mag.h>
 #include <uORB/topics/tecs_status.h>
 #include <uORB/topics/telemetry_status.h>
 #include <uORB/topics/transponder_report.h>
+#include <uORB/topics/vehicle_air_data.h>
 #include <uORB/topics/vehicle_angular_velocity.h>
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
@@ -97,21 +102,16 @@
 #include <uORB/topics/vehicle_land_detected.h>
 #include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/vehicle_local_position_setpoint.h>
+#include <uORB/topics/vehicle_magnetometer.h>
 #include <uORB/topics/vehicle_odometry.h>
 #include <uORB/topics/vehicle_rates_setpoint.h>
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/vehicle_status_flags.h>
+#include <uORB/topics/vehicle_trajectory_waypoint.h>
 #include <uORB/topics/vtol_vehicle_status.h>
 #include <uORB/topics/wind_estimate.h>
-#include <uORB/topics/mount_orientation.h>
-#include <uORB/topics/collision_report.h>
-#include <uORB/topics/sensor_accel.h>
-#include <uORB/topics/sensor_gyro.h>
-#include <uORB/topics/sensor_mag.h>
-#include <uORB/topics/vehicle_air_data.h>
-#include <uORB/topics/vehicle_magnetometer.h>
-#include <uORB/uORB.h>
 
+using matrix::Vector3f;
 using matrix::wrap_2pi;
 
 static uint16_t cm_uint16_from_m_float(float m);
@@ -952,8 +952,8 @@ private:
 
 protected:
 	explicit MavlinkStreamScaledIMU(Mavlink *mavlink) : MavlinkStream(mavlink),
-		_raw_accel_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_accel), 0)),
+		_raw_accel_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_accel_integrated), 0)),
-		_raw_gyro_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_gyro), 0)),
+		_raw_gyro_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_gyro_integrated), 0)),
 		_raw_mag_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_mag), 0)),
 		_raw_accel_time(0),
 		_raw_gyro_time(0),
@@ -962,9 +962,9 @@ protected:
 
 	bool send(const hrt_abstime t) override
 	{
-		sensor_accel_s sensor_accel = {};
+		sensor_accel_integrated_s sensor_accel{};
-		sensor_gyro_s sensor_gyro = {};
+		sensor_gyro_integrated_s sensor_gyro{};
-		sensor_mag_s sensor_mag = {};
+		sensor_mag_s sensor_mag{};
 
 		bool updated = false;
 		updated |= _raw_accel_sub->update(&_raw_accel_time, &sensor_accel);
@@ -972,19 +972,27 @@ protected:
 		updated |= _raw_mag_sub->update(&_raw_mag_time, &sensor_mag);
 
 		if (updated) {
-
+			mavlink_scaled_imu_t msg{};
-			mavlink_scaled_imu_t msg = {};
 
 			msg.time_boot_ms = sensor_accel.timestamp / 1000;
-			msg.xacc = (int16_t)(sensor_accel.x_raw / CONSTANTS_ONE_G); 	// [milli g]
+
-			msg.yacc = (int16_t)(sensor_accel.y_raw / CONSTANTS_ONE_G); 	// [milli g]
+			// Accelerometer in mG
-			msg.zacc = (int16_t)(sensor_accel.z_raw / CONSTANTS_ONE_G); 	// [milli g]
+			const float accel_dt_inv = 1.e6f / (float)sensor_accel.dt;
-			msg.xgyro = sensor_gyro.x_raw;					// [milli rad/s]
+			const Vector3f accel = Vector3f{sensor_accel.delta_velocity} * accel_dt_inv * 1000.0f / CONSTANTS_ONE_G;
-			msg.ygyro = sensor_gyro.y_raw;					// [milli rad/s]
+
-			msg.zgyro = sensor_gyro.z_raw;					// [milli rad/s]
+			// Gyroscope in mrad/s
-			msg.xmag = sensor_mag.x_raw;					// [milli tesla]
+			const float gyro_dt_inv = 1.e6f / (float)sensor_gyro.dt;
-			msg.ymag = sensor_mag.y_raw;					// [milli tesla]
+			const Vector3f gyro = Vector3f{sensor_gyro.delta_angle} * gyro_dt_inv * 1000.0f;
-			msg.zmag = sensor_mag.z_raw;					// [milli tesla]
+
+			msg.xacc = (int16_t)accel(0);
+			msg.yacc = (int16_t)accel(1);
+			msg.zacc = (int16_t)accel(2);
+			msg.xgyro = gyro(0);
+			msg.ygyro = gyro(1);
+			msg.zgyro = gyro(2);
+			msg.xmag = sensor_mag.x * 1000.0f; // Gauss -> MilliGauss
+			msg.ymag = sensor_mag.y * 1000.0f; // Gauss -> MilliGauss
+			msg.zmag = sensor_mag.z * 1000.0f; // Gauss -> MilliGauss
 
 			mavlink_msg_scaled_imu_send_struct(_mavlink->get_channel(), &msg);
 
@@ -1044,8 +1052,8 @@ private:
 
 protected:
 	explicit MavlinkStreamScaledIMU2(Mavlink *mavlink) : MavlinkStream(mavlink),
-		_raw_accel_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_accel), 1)),
+		_raw_accel_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_accel_integrated), 1)),
-		_raw_gyro_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_gyro), 1)),
+		_raw_gyro_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_gyro_integrated), 1)),
 		_raw_mag_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_mag), 1)),
 		_raw_accel_time(0),
 		_raw_gyro_time(0),
@@ -1054,9 +1062,9 @@ protected:
 
 	bool send(const hrt_abstime t) override
 	{
-		sensor_accel_s sensor_accel = {};
+		sensor_accel_integrated_s sensor_accel{};
-		sensor_gyro_s sensor_gyro = {};
+		sensor_gyro_integrated_s sensor_gyro{};
-		sensor_mag_s sensor_mag = {};
+		sensor_mag_s sensor_mag{};
 
 		bool updated = false;
 		updated |= _raw_accel_sub->update(&_raw_accel_time, &sensor_accel);
@@ -1064,19 +1072,27 @@ protected:
 		updated |= _raw_mag_sub->update(&_raw_mag_time, &sensor_mag);
 
 		if (updated) {
-
+			mavlink_scaled_imu2_t msg{};
-			mavlink_scaled_imu2_t msg = {};
 
 			msg.time_boot_ms = sensor_accel.timestamp / 1000;
-			msg.xacc = (int16_t)(sensor_accel.x_raw / CONSTANTS_ONE_G); 	// [milli g]
+
-			msg.yacc = (int16_t)(sensor_accel.y_raw / CONSTANTS_ONE_G); 	// [milli g]
+			// Accelerometer in mG
-			msg.zacc = (int16_t)(sensor_accel.z_raw / CONSTANTS_ONE_G); 	// [milli g]
+			const float accel_dt_inv = 1.e6f / (float)sensor_accel.dt;
-			msg.xgyro = sensor_gyro.x_raw;					// [milli rad/s]
+			const Vector3f accel = Vector3f{sensor_accel.delta_velocity} * accel_dt_inv * 1000.0f / CONSTANTS_ONE_G;
-			msg.ygyro = sensor_gyro.y_raw;					// [milli rad/s]
+
-			msg.zgyro = sensor_gyro.z_raw;					// [milli rad/s]
+			// Gyroscope in mrad/s
-			msg.xmag = sensor_mag.x_raw;					// [milli tesla]
+			const float gyro_dt_inv = 1.e6f / (float)sensor_gyro.dt;
-			msg.ymag = sensor_mag.y_raw;					// [milli tesla]
+			const Vector3f gyro = Vector3f{sensor_gyro.delta_angle} * gyro_dt_inv * 1000.0f;
-			msg.zmag = sensor_mag.z_raw;					// [milli tesla]
+
+			msg.xacc = (int16_t)accel(0);
+			msg.yacc = (int16_t)accel(1);
+			msg.zacc = (int16_t)accel(2);
+			msg.xgyro = gyro(0);
+			msg.ygyro = gyro(1);
+			msg.zgyro = gyro(2);
+			msg.xmag = sensor_mag.x * 1000.0f; // Gauss -> MilliGauss
+			msg.ymag = sensor_mag.y * 1000.0f; // Gauss -> MilliGauss
+			msg.zmag = sensor_mag.z * 1000.0f; // Gauss -> MilliGauss
 
 			mavlink_msg_scaled_imu2_send_struct(_mavlink->get_channel(), &msg);
 
@@ -1135,8 +1151,8 @@ private:
 
 protected:
 	explicit MavlinkStreamScaledIMU3(Mavlink *mavlink) : MavlinkStream(mavlink),
-		_raw_accel_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_accel), 2)),
+		_raw_accel_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_accel_integrated), 2)),
-		_raw_gyro_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_gyro), 2)),
+		_raw_gyro_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_gyro_integrated), 2)),
 		_raw_mag_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_mag), 2)),
 		_raw_accel_time(0),
 		_raw_gyro_time(0),
@@ -1145,9 +1161,9 @@ protected:
 
 	bool send(const hrt_abstime t) override
 	{
-		sensor_accel_s sensor_accel = {};
+		sensor_accel_integrated_s sensor_accel{};
-		sensor_gyro_s sensor_gyro = {};
+		sensor_gyro_integrated_s sensor_gyro{};
-		sensor_mag_s sensor_mag = {};
+		sensor_mag_s sensor_mag{};
 
 		bool updated = false;
 		updated |= _raw_accel_sub->update(&_raw_accel_time, &sensor_accel);
@@ -1155,19 +1171,27 @@ protected:
 		updated |= _raw_mag_sub->update(&_raw_mag_time, &sensor_mag);
 
 		if (updated) {
-
+			mavlink_scaled_imu3_t msg{};
-			mavlink_scaled_imu3_t msg = {};
 
 			msg.time_boot_ms = sensor_accel.timestamp / 1000;
-			msg.xacc = (int16_t)(sensor_accel.x_raw / CONSTANTS_ONE_G); 	// [milli g]
+
-			msg.yacc = (int16_t)(sensor_accel.y_raw / CONSTANTS_ONE_G); 	// [milli g]
+			// Accelerometer in mG
-			msg.zacc = (int16_t)(sensor_accel.z_raw / CONSTANTS_ONE_G); 	// [milli g]
+			const float accel_dt_inv = 1.e6f / (float)sensor_accel.dt;
-			msg.xgyro = sensor_gyro.x_raw;					// [milli rad/s]
+			const Vector3f accel = Vector3f{sensor_accel.delta_velocity} * accel_dt_inv * 1000.0f / CONSTANTS_ONE_G;
-			msg.ygyro = sensor_gyro.y_raw;					// [milli rad/s]
+
-			msg.zgyro = sensor_gyro.z_raw;					// [milli rad/s]
+			// Gyroscope in mrad/s
-			msg.xmag = sensor_mag.x_raw;					// [milli tesla]
+			const float gyro_dt_inv = 1.e6f / (float)sensor_gyro.dt;
-			msg.ymag = sensor_mag.y_raw;					// [milli tesla]
+			const Vector3f gyro = Vector3f{sensor_gyro.delta_angle} * gyro_dt_inv * 1000.0f;
-			msg.zmag = sensor_mag.z_raw;					// [milli tesla]
+
+			msg.xacc = (int16_t)accel(0);
+			msg.yacc = (int16_t)accel(1);
+			msg.zacc = (int16_t)accel(2);
+			msg.xgyro = gyro(0);
+			msg.ygyro = gyro(1);
+			msg.zgyro = gyro(2);
+			msg.xmag = sensor_mag.x * 1000.0f; // Gauss -> MilliGauss
+			msg.ymag = sensor_mag.y * 1000.0f; // Gauss -> MilliGauss
+			msg.zmag = sensor_mag.z * 1000.0f; // Gauss -> MilliGauss
 
 			mavlink_msg_scaled_imu3_send_struct(_mavlink->get_channel(), &msg);
 

src/modules/mavlink/mavlink_receiver.cpp

@@ -2088,9 +2088,6 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
 		sensor_gyro_s gyro{};
 
 		gyro.timestamp = timestamp;
-		gyro.x_raw = imu.xgyro * 1000.0f;
-		gyro.y_raw = imu.ygyro * 1000.0f;
-		gyro.z_raw = imu.zgyro * 1000.0f;
 		gyro.x = imu.xgyro;
 		gyro.y = imu.ygyro;
 		gyro.z = imu.zgyro;
@@ -2104,9 +2101,6 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
 		sensor_accel_s accel{};
 
 		accel.timestamp = timestamp;
-		accel.x_raw = imu.xacc / (CONSTANTS_ONE_G / 1000.0f);
-		accel.y_raw = imu.yacc / (CONSTANTS_ONE_G / 1000.0f);
-		accel.z_raw = imu.zacc / (CONSTANTS_ONE_G / 1000.0f);
 		accel.x = imu.xacc;
 		accel.y = imu.yacc;
 		accel.z = imu.zacc;
@@ -2507,9 +2501,6 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
 		sensor_accel_s accel{};
 
 		accel.timestamp = timestamp;
-		accel.x_raw = hil_state.xacc / CONSTANTS_ONE_G * 1e3f;
-		accel.y_raw = hil_state.yacc / CONSTANTS_ONE_G * 1e3f;
-		accel.z_raw = hil_state.zacc / CONSTANTS_ONE_G * 1e3f;
 		accel.x = hil_state.xacc;
 		accel.y = hil_state.yacc;
 		accel.z = hil_state.zacc;
@@ -2523,9 +2514,6 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
 		sensor_gyro_s gyro{};
 
 		gyro.timestamp = timestamp;
-		gyro.x_raw = hil_state.rollspeed * 1e3f;
-		gyro.y_raw = hil_state.pitchspeed * 1e3f;
-		gyro.z_raw = hil_state.yawspeed * 1e3f;
 		gyro.x = hil_state.rollspeed;
 		gyro.y = hil_state.pitchspeed;
 		gyro.z = hil_state.yawspeed;

src/modules/sensors/vehicle_acceleration/CMakeLists.txt

@@ -33,5 +33,6 @@
 
 px4_add_library(vehicle_acceleration
 	VehicleAcceleration.cpp
+	VehicleAcceleration.hpp
 )
 target_link_libraries(vehicle_acceleration PRIVATE px4_work_queue)

src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp

@@ -49,26 +49,18 @@ VehicleAcceleration::~VehicleAcceleration()
 	Stop();
 }
 
-bool
+bool VehicleAcceleration::Start()
-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);
+	SensorCorrectionsUpdate(true);
 
 	// needed to change the active sensor if the primary stops updating
-	_sensor_selection_sub.registerCallback();
+	return _sensor_selection_sub.registerCallback() && SensorSelectionUpdate(true);
-
-	return SensorCorrectionsUpdate(true);
 }
 
-void
+void VehicleAcceleration::Stop()
-VehicleAcceleration::Stop()
 {
 	Deinit();
 
@@ -80,21 +72,23 @@ VehicleAcceleration::Stop()
 	_sensor_selection_sub.unregisterCallback();
 }
 
-void
+void VehicleAcceleration::SensorBiasUpdate(bool force)
-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
+			if (bias.accel_device_id == _selected_sensor_device_id) {
-			_bias = Vector3f{bias.accel_bias};
+				_bias = Vector3f{bias.accel_bias};
+
+			} else {
+				_bias.zero();
+			}
 		}
 	}
 }
 
-bool
+void VehicleAcceleration::SensorCorrectionsUpdate(bool force)
-VehicleAcceleration::SensorCorrectionsUpdate(bool force)
 {
 	// check if the selected sensor has updated
 	if (_sensor_correction_sub.updated() || force) {
@@ -102,49 +96,82 @@ VehicleAcceleration::SensorCorrectionsUpdate(bool force)
 		sensor_correction_s corrections{};
 		_sensor_correction_sub.copy(&corrections);
 
-		// TODO: should be checking device ID
+		// selected sensor has changed, find updated index
-		if (_selected_sensor == 0) {
+		if ((_corrections_selected_instance != corrections.selected_accel_instance) || force) {
+			_corrections_selected_instance = -1;
+
+			// find sensor_corrections index
+			for (int i = 0; i < MAX_SENSOR_COUNT; i++) {
+				if (corrections.accel_device_ids[i] == _selected_sensor_device_id) {
+					_corrections_selected_instance = i;
+				}
+			}
+		}
+
+		switch (_corrections_selected_instance) {
+		case 0:
 			_offset = Vector3f{corrections.accel_offset_0};
 			_scale = Vector3f{corrections.accel_scale_0};
-
+			break;
-		} else if (_selected_sensor == 1) {
+		case 1:
 			_offset = Vector3f{corrections.accel_offset_1};
 			_scale = Vector3f{corrections.accel_scale_1};
-
+			break;
-		} else if (_selected_sensor == 2) {
+		case 2:
 			_offset = Vector3f{corrections.accel_offset_2};
 			_scale = Vector3f{corrections.accel_scale_2};
-
+			break;
-		} else {
+		default:
 			_offset = Vector3f{0.0f, 0.0f, 0.0f};
 			_scale = Vector3f{1.0f, 1.0f, 1.0f};
 		}
+	}
+}
 
-		// update the latest sensor selection
+bool VehicleAcceleration::SensorSelectionUpdate(bool force)
-		if ((_selected_sensor != corrections.selected_accel_instance) || force) {
+{
-			if (corrections.selected_accel_instance < MAX_SENSOR_COUNT) {
+	if (_sensor_selection_sub.updated() || (_selected_sensor_device_id == 0) || force) {
-				// clear all registered callbacks
+		sensor_selection_s sensor_selection{};
-				for (auto &sub : _sensor_sub) {
+		_sensor_selection_sub.copy(&sensor_selection);
-					sub.unregisterCallback();
-				}
 
-				const int sensor_new = corrections.selected_accel_instance;
+		if (_selected_sensor_device_id != sensor_selection.accel_device_id) {
+			// clear all registered callbacks
+			for (auto &sub : _sensor_sub) {
+				sub.unregisterCallback();
+			}
 
-				if (_sensor_sub[sensor_new].registerCallback()) {
+			for (int i = 0; i < MAX_SENSOR_COUNT; i++) {
-					PX4_DEBUG("selected sensor changed %d -> %d", _selected_sensor, sensor_new);
+				sensor_accel_s report{};
-					_selected_sensor = sensor_new;
+				_sensor_sub[i].copy(&report);
 
-					return true;
+				if ((report.device_id != 0) && (report.device_id == sensor_selection.accel_device_id)) {
+					if (_sensor_sub[i].registerCallback()) {
+						PX4_DEBUG("selected sensor changed %d -> %d", _selected_sensor_sub_index, i);
+
+						// record selected sensor (array index)
+						_selected_sensor_sub_index = i;
+						_selected_sensor_device_id = sensor_selection.accel_device_id;
+
+						// clear bias and corrections
+						_bias.zero();
+						_offset = Vector3f{0.0f, 0.0f, 0.0f};
+						_scale = Vector3f{1.0f, 1.0f, 1.0f};
+
+						return true;
+					}
 				}
 			}
+
+			PX4_ERR("unable to find or subscribe to selected sensor (%d)", sensor_selection.accel_device_id);
+			_selected_sensor_device_id = 0;
+			_selected_sensor_sub_index = 0;
 		}
 	}
 
 	return false;
 }
 
-void
+void VehicleAcceleration::ParametersUpdate(bool force)
-VehicleAcceleration::ParametersUpdate(bool force)
 {
 	// Check if parameters have changed
 	if (_params_sub.updated() || force) {
@@ -167,20 +194,19 @@ VehicleAcceleration::ParametersUpdate(bool force)
 	}
 }
 
-void
+void VehicleAcceleration::Run()
-VehicleAcceleration::Run()
 {
 	// update corrections first to set _selected_sensor
-	bool sensor_select_update = SensorCorrectionsUpdate();
+	bool sensor_select_update = SensorSelectionUpdate();
+	SensorCorrectionsUpdate(sensor_select_update);
+	SensorBiasUpdate(sensor_select_update);
+	ParametersUpdate();
 
-	if (_sensor_sub[_selected_sensor].updated() || sensor_select_update) {
+	if (_sensor_sub[_selected_sensor_sub_index].updated() || sensor_select_update) {
 		sensor_accel_s sensor_data;
-		_sensor_sub[_selected_sensor].copy(&sensor_data);
+		_sensor_sub[_selected_sensor_sub_index].copy(&sensor_data);
 
-		ParametersUpdate();
+		// get the sensor data and correct for thermal errors (apply offsets and scale)
-		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
@@ -192,8 +218,10 @@ VehicleAcceleration::Run()
 		// correct for in-run bias errors
 		accel -= _bias;
 
-		vehicle_acceleration_s out{};
+		// publish
-		out.timestamp_sample = sensor_data.timestamp;
+		vehicle_acceleration_s out;
+
+		out.timestamp_sample = sensor_data.timestamp_sample;
 		accel.copyTo(out.xyz);
 		out.timestamp = hrt_absolute_time();
 
@@ -201,8 +229,10 @@ VehicleAcceleration::Run()
 	}
 }
 
-void
+void VehicleAcceleration::PrintStatus()
-VehicleAcceleration::PrintStatus()
 {
-	PX4_INFO("selected sensor: %d", _selected_sensor);
+	PX4_INFO("selected sensor: %d (%d)", _selected_sensor_device_id, _selected_sensor_sub_index);
+	PX4_INFO("bias: [%.3f %.3f %.3f]", (double)_bias(0), (double)_bias(1), (double)_bias(2));
+	PX4_INFO("offset: [%.3f %.3f %.3f]", (double)_offset(0), (double)_offset(1), (double)_offset(2));
+	PX4_INFO("scale: [%.3f %.3f %.3f]", (double)_scale(0), (double)_scale(1), (double)_scale(2));
 }

src/modules/sensors/vehicle_acceleration/VehicleAcceleration.hpp

@@ -56,7 +56,7 @@ class VehicleAcceleration : public ModuleParams, public px4::WorkItem
 public:
 
 	VehicleAcceleration();
-	virtual ~VehicleAcceleration();
+	~VehicleAcceleration() override;
 
 	void	Run() override;
 
@@ -66,10 +66,10 @@ public:
 	void	PrintStatus();
 
 private:
-
+	void ParametersUpdate(bool force = false);
-	void	ParametersUpdate(bool force = false);
+	void SensorBiasUpdate(bool force = false);
-	void	SensorBiasUpdate(bool force = false);
+	void SensorCorrectionsUpdate(bool force = false);
-	bool	SensorCorrectionsUpdate(bool force = false);
+	bool SensorSelectionUpdate(bool force = false);
 
 	static constexpr int MAX_SENSOR_COUNT = 3;
 
@@ -96,10 +96,11 @@ private:
 
 	matrix::Dcmf				_board_rotation;				/**< rotation matrix for the orientation that the board is mounted */
 
-	matrix::Vector3f			_offset;
+	matrix::Vector3f			_offset{0.f, 0.f, 0.f};
-	matrix::Vector3f			_scale;
+	matrix::Vector3f			_scale{1.f, 1.f, 1.f};
-	matrix::Vector3f			_bias;
+	matrix::Vector3f			_bias{0.f, 0.f, 0.f};
-
-	uint8_t					_selected_sensor{0};
 
+	uint32_t				_selected_sensor_device_id{0};
+	uint8_t					_selected_sensor_sub_index{0};
+	int8_t					_corrections_selected_instance{-1};
 };

src/modules/sensors/vehicle_angular_velocity/CMakeLists.txt

@@ -33,5 +33,6 @@
 
 px4_add_library(vehicle_angular_velocity
 	VehicleAngularVelocity.cpp
+	VehicleAngularVelocity.hpp
 )
 target_link_libraries(vehicle_angular_velocity PRIVATE px4_work_queue)

src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp

@@ -49,26 +49,18 @@ VehicleAngularVelocity::~VehicleAngularVelocity()
 	Stop();
 }
 
-bool
+bool VehicleAngularVelocity::Start()
-VehicleAngularVelocity::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);
+	SensorCorrectionsUpdate(true);
 
 	// needed to change the active sensor if the primary stops updating
-	_sensor_selection_sub.registerCallback();
+	return _sensor_selection_sub.registerCallback() && SensorSelectionUpdate(true);
-
-	return SensorCorrectionsUpdate(true);
 }
 
-void
+void VehicleAngularVelocity::Stop()
-VehicleAngularVelocity::Stop()
 {
 	Deinit();
 
@@ -80,110 +72,106 @@ VehicleAngularVelocity::Stop()
 	_sensor_selection_sub.unregisterCallback();
 }
 
-void
+void VehicleAngularVelocity::SensorBiasUpdate(bool force)
-VehicleAngularVelocity::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
+			if (bias.gyro_device_id == _selected_sensor_device_id) {
-			_bias = Vector3f{bias.gyro_bias};
+				_bias = Vector3f{bias.gyro_bias};
+
+			} else {
+				_bias.zero();
+			}
 		}
 	}
 }
 
-bool
+void VehicleAngularVelocity::SensorCorrectionsUpdate(bool force)
-VehicleAngularVelocity::SensorCorrectionsUpdate(bool force)
 {
-	if (_sensor_selection_sub.updated() || force) {
-		sensor_selection_s sensor_selection;
-
-		if (_sensor_selection_sub.copy(&sensor_selection)) {
-			if (_selected_sensor_device_id != sensor_selection.gyro_device_id) {
-				_selected_sensor_device_id = sensor_selection.gyro_device_id;
-				force = true;
-			}
-		}
-	}
-
 	// 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
+		// selected sensor has changed, find updated index
-		if (_selected_sensor == 0) {
+		if ((_corrections_selected_instance != corrections.selected_gyro_instance) || force) {
+			_corrections_selected_instance = -1;
+
+			// find sensor_corrections index
+			for (int i = 0; i < MAX_SENSOR_COUNT; i++) {
+				if (corrections.gyro_device_ids[i] == _selected_sensor_device_id) {
+					_corrections_selected_instance = i;
+				}
+			}
+		}
+
+		switch (_corrections_selected_instance) {
+		case 0:
 			_offset = Vector3f{corrections.gyro_offset_0};
 			_scale = Vector3f{corrections.gyro_scale_0};
-
+			break;
-		} else if (_selected_sensor == 1) {
+		case 1:
 			_offset = Vector3f{corrections.gyro_offset_1};
 			_scale = Vector3f{corrections.gyro_scale_1};
-
+			break;
-		} else if (_selected_sensor == 2) {
+		case 2:
 			_offset = Vector3f{corrections.gyro_offset_2};
 			_scale = Vector3f{corrections.gyro_scale_2};
-
+			break;
-		} else {
+		default:
 			_offset = Vector3f{0.0f, 0.0f, 0.0f};
 			_scale = Vector3f{1.0f, 1.0f, 1.0f};
 		}
+	}
+}
 
-		// update the latest sensor selection
+bool VehicleAngularVelocity::SensorSelectionUpdate(bool force)
-		if ((_selected_sensor != corrections.selected_gyro_instance) || force) {
+{
-			if (corrections.selected_gyro_instance < MAX_SENSOR_COUNT) {
+	if (_sensor_selection_sub.updated() || (_selected_sensor_device_id == 0) || force) {
-				// clear all registered callbacks
+		sensor_selection_s sensor_selection{};
-				for (auto &sub : _sensor_control_sub) {
+		_sensor_selection_sub.copy(&sensor_selection);
-					sub.unregisterCallback();
-				}
 
-				for (auto &sub : _sensor_sub) {
+		if (_selected_sensor_device_id != sensor_selection.gyro_device_id) {
-					sub.unregisterCallback();
+			// clear all registered callbacks
-				}
+			for (auto &sub : _sensor_sub) {
+				sub.unregisterCallback();
+			}
 
-				const int sensor_new = corrections.selected_gyro_instance;
+			for (int i = 0; i < MAX_SENSOR_COUNT; i++) {
+				sensor_gyro_s report{};
+				_sensor_sub[i].copy(&report);
 
-				// subscribe to sensor_gyro_control if available
+				if ((report.device_id != 0) && (report.device_id == sensor_selection.gyro_device_id)) {
-				//  currently not all drivers (eg df_*) provide sensor_gyro_control
+					if (_sensor_sub[i].registerCallback()) {
-				for (int i = 0; i < MAX_SENSOR_COUNT; i++) {
+						PX4_DEBUG("selected sensor changed %d -> %d", _selected_sensor_sub_index, i);
-					sensor_gyro_control_s report{};
-					_sensor_control_sub[i].copy(&report);
 
-					if ((report.device_id != 0) && (report.device_id == _selected_sensor_device_id)) {
+						// record selected sensor (array index)
-						if (_sensor_control_sub[i].registerCallback()) {
+						_selected_sensor_sub_index = i;
-							PX4_DEBUG("selected sensor (control) changed %d -> %d", _selected_sensor, i);
+						_selected_sensor_device_id = sensor_selection.gyro_device_id;
-							_selected_sensor_control = i;
 
-							_sensor_control_available = true;
+						// clear bias and corrections
+						_bias.zero();
+						_offset = Vector3f{0.0f, 0.0f, 0.0f};
+						_scale = Vector3f{1.0f, 1.0f, 1.0f};
 
-							// record selected sensor (sensor_gyro orb index)
+						return true;
-							_selected_sensor = sensor_new;
-
-							return true;
-						}
 					}
 				}
-
-				// otherwise fallback to using sensor_gyro (legacy that will be removed)
-				_sensor_control_available = false;
-
-				if (_sensor_sub[sensor_new].registerCallback()) {
-					PX4_DEBUG("selected sensor changed %d -> %d", _selected_sensor, sensor_new);
-					_selected_sensor = sensor_new;
-
-					return true;
-				}
 			}
+
+			PX4_ERR("unable to find or subscribe to selected sensor (%d)", sensor_selection.gyro_device_id);
+			_selected_sensor_device_id = 0;
+			_selected_sensor_sub_index = 0;
 		}
 	}
 
 	return false;
 }
 
-void
+void VehicleAngularVelocity::ParametersUpdate(bool force)
-VehicleAngularVelocity::ParametersUpdate(bool force)
 {
 	// Check if parameters have changed
 	if (_params_sub.updated() || force) {
@@ -206,78 +194,45 @@ VehicleAngularVelocity::ParametersUpdate(bool force)
 	}
 }
 
-void
+void VehicleAngularVelocity::Run()
-VehicleAngularVelocity::Run()
 {
 	// update corrections first to set _selected_sensor
-	bool sensor_select_update = SensorCorrectionsUpdate();
+	bool sensor_select_update = SensorSelectionUpdate();
+	SensorCorrectionsUpdate(sensor_select_update);
+	SensorBiasUpdate(sensor_select_update);
+	ParametersUpdate();
 
-	if (_sensor_control_available) {
+	if (_sensor_sub[_selected_sensor_sub_index].updated() || sensor_select_update) {
-		//  using sensor_gyro_control is preferred, but currently not all drivers (eg df_*) provide sensor_gyro_control
+		sensor_gyro_s sensor_data;
-		if (_sensor_control_sub[_selected_sensor].updated() || sensor_select_update) {
+		_sensor_sub[_selected_sensor_sub_index].copy(&sensor_data);
-			sensor_gyro_control_s sensor_data;
-			_sensor_control_sub[_selected_sensor].copy(&sensor_data);
 
-			ParametersUpdate();
+		// get the sensor data and correct for thermal errors (apply offsets and scale)
-			SensorBiasUpdate();
+		const Vector3f val{sensor_data.x, sensor_data.y, sensor_data.z};
 
-			// get the sensor data and correct for thermal errors (apply offsets and scale)
+		// apply offsets and scale
-			Vector3f rates{(Vector3f{sensor_data.xyz} - _offset).emult(_scale)};
+		Vector3f rates{(val - _offset).emult(_scale)};
 
-			// rotate corrected measurements from sensor to body frame
+		// rotate corrected measurements from sensor to body frame
-			rates = _board_rotation * rates;
+		rates = _board_rotation * rates;
 
-			// correct for in-run bias errors
+		// correct for in-run bias errors
-			rates -= _bias;
+		rates -= _bias;
 
-			vehicle_angular_velocity_s angular_velocity;
+		// publish
-			angular_velocity.timestamp_sample = sensor_data.timestamp_sample;
+		vehicle_angular_velocity_s out;
-			rates.copyTo(angular_velocity.xyz);
-			angular_velocity.timestamp = hrt_absolute_time();
 
-			_vehicle_angular_velocity_pub.publish(angular_velocity);
+		out.timestamp_sample = sensor_data.timestamp_sample;
-		}
+		rates.copyTo(out.xyz);
+		out.timestamp = hrt_absolute_time();
 
-	} else {
+		_vehicle_angular_velocity_pub.publish(out);
-		// otherwise fallback to using sensor_gyro (legacy that will be removed)
-		if (_sensor_sub[_selected_sensor].updated() || sensor_select_update) {
-			sensor_gyro_s sensor_data;
-			_sensor_sub[_selected_sensor].copy(&sensor_data);
-
-			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 rates{(val - _offset).emult(_scale)};
-
-			// rotate corrected measurements from sensor to body frame
-			rates = _board_rotation * rates;
-
-			// correct for in-run bias errors
-			rates -= _bias;
-
-			vehicle_angular_velocity_s angular_velocity;
-			angular_velocity.timestamp_sample = sensor_data.timestamp;
-			rates.copyTo(angular_velocity.xyz);
-			angular_velocity.timestamp = hrt_absolute_time();
-
-			_vehicle_angular_velocity_pub.publish(angular_velocity);
-		}
 	}
 }
 
-void
+void VehicleAngularVelocity::PrintStatus()
-VehicleAngularVelocity::PrintStatus()
 {
-	PX4_INFO("selected sensor: %d", _selected_sensor);
+	PX4_INFO("selected sensor: %d (%d)", _selected_sensor_device_id, _selected_sensor_sub_index);
-
+	PX4_INFO("bias: [%.3f %.3f %.3f]", (double)_bias(0), (double)_bias(1), (double)_bias(2));
-	if (_selected_sensor_device_id != 0) {
+	PX4_INFO("offset: [%.3f %.3f %.3f]", (double)_offset(0), (double)_offset(1), (double)_offset(2));
-		PX4_INFO("using sensor_gyro_control: %d (%d)", _selected_sensor_device_id, _selected_sensor_control);
+	PX4_INFO("scale: [%.3f %.3f %.3f]", (double)_scale(0), (double)_scale(1), (double)_scale(2));
-
-	} else {
-		PX4_WARN("sensor_gyro_control unavailable for selected sensor: %d (%d)", _selected_sensor_device_id,  _selected_sensor);
-	}
 }

src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp

@@ -49,7 +49,6 @@
 #include <uORB/topics/sensor_selection.h>
 
 #include <uORB/topics/sensor_gyro.h>
-#include <uORB/topics/sensor_gyro_control.h>
 #include <uORB/topics/vehicle_angular_velocity.h>
 
 class VehicleAngularVelocity : public ModuleParams, public px4::WorkItem
@@ -57,7 +56,7 @@ class VehicleAngularVelocity : public ModuleParams, public px4::WorkItem
 public:
 
 	VehicleAngularVelocity();
-	virtual ~VehicleAngularVelocity();
+	~VehicleAngularVelocity() override;
 
 	void	Run() override;
 
@@ -67,10 +66,10 @@ public:
 	void	PrintStatus();
 
 private:
-
+	void ParametersUpdate(bool force = false);
-	void	ParametersUpdate(bool force = false);
+	void SensorBiasUpdate(bool force = false);
-	void	SensorBiasUpdate(bool force = false);
+	void SensorCorrectionsUpdate(bool force = false);
-	bool	SensorCorrectionsUpdate(bool force = false);
+	bool SensorSelectionUpdate(bool force = false);
 
 	static constexpr int MAX_SENSOR_COUNT = 3;
 
@@ -89,28 +88,19 @@ 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_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}
 	};
 
-	uORB::SubscriptionCallbackWorkItem	_sensor_control_sub[MAX_SENSOR_COUNT] {			/**< sensor control data subscription */
-		{this, ORB_ID(sensor_gyro_control), 0},
-		{this, ORB_ID(sensor_gyro_control), 1},
-		{this, ORB_ID(sensor_gyro_control), 2}
-	};
-
 	matrix::Dcmf				_board_rotation;				/**< rotation matrix for the orientation that the board is mounted */
 
-	matrix::Vector3f			_offset;
+	matrix::Vector3f			_bias{0.f, 0.f, 0.f};
-	matrix::Vector3f			_scale;
+	matrix::Vector3f			_offset{0.f, 0.f, 0.f};
-	matrix::Vector3f			_bias;
+	matrix::Vector3f			_scale{1.f, 1.f, 1.f};
 
 	uint32_t				_selected_sensor_device_id{0};
-	uint8_t					_selected_sensor{0};
+	uint8_t					_selected_sensor_sub_index{0};
-	uint8_t					_selected_sensor_control{0};
+	int8_t					_corrections_selected_instance{-1};
-	bool					_sensor_control_available{false};
-
 };

src/modules/sensors/voted_sensors_update.cpp

@@ -92,9 +92,9 @@ int VotedSensorsUpdate::init(sensor_combined_s &raw)
 
 void VotedSensorsUpdate::initializeSensors()
 {
-	initSensorClass(ORB_ID(sensor_gyro), _gyro, GYRO_COUNT_MAX);
+	initSensorClass(ORB_ID(sensor_gyro_integrated), _gyro, GYRO_COUNT_MAX);
 	initSensorClass(ORB_ID(sensor_mag), _mag, MAG_COUNT_MAX);
-	initSensorClass(ORB_ID(sensor_accel), _accel, ACCEL_COUNT_MAX);
+	initSensorClass(ORB_ID(sensor_accel_integrated), _accel, ACCEL_COUNT_MAX);
 	initSensorClass(ORB_ID(sensor_baro), _baro, BARO_COUNT_MAX);
 }
 
@@ -142,7 +142,7 @@ void VotedSensorsUpdate::parametersUpdate()
 	/* gyro */
 	for (uint8_t topic_instance = 0; topic_instance < _gyro.subscription_count; ++topic_instance) {
 
-		uORB::SubscriptionData<sensor_gyro_s> report{ORB_ID(sensor_gyro), topic_instance};
+		uORB::SubscriptionData<sensor_gyro_integrated_s> report{ORB_ID(sensor_gyro_integrated), topic_instance};
 
 		int temp = _temperature_compensation.set_sensor_id_gyro(report.get().device_id, topic_instance);
 
@@ -151,10 +151,11 @@ void VotedSensorsUpdate::parametersUpdate()
 				topic_instance);
 
 			_corrections.gyro_mapping[topic_instance] = 0;
+			_corrections.gyro_device_ids[topic_instance] = 0;
 
 		} else {
 			_corrections.gyro_mapping[topic_instance] = temp;
-
+			_corrections.gyro_device_ids[topic_instance] = report.get().device_id;
 		}
 	}
 
@@ -162,7 +163,7 @@ void VotedSensorsUpdate::parametersUpdate()
 	/* accel */
 	for (uint8_t topic_instance = 0; topic_instance < _accel.subscription_count; ++topic_instance) {
 
-		uORB::SubscriptionData<sensor_accel_s> report{ORB_ID(sensor_accel), topic_instance};
+		uORB::SubscriptionData<sensor_accel_integrated_s> report{ORB_ID(sensor_accel_integrated), topic_instance};
 
 		int temp = _temperature_compensation.set_sensor_id_accel(report.get().device_id, topic_instance);
 
@@ -171,10 +172,11 @@ void VotedSensorsUpdate::parametersUpdate()
 				topic_instance);
 
 			_corrections.accel_mapping[topic_instance] = 0;
+			_corrections.accel_device_ids[topic_instance] = 0;
 
 		} else {
 			_corrections.accel_mapping[topic_instance] = temp;
-
+			_corrections.accel_device_ids[topic_instance] = report.get().device_id;
 		}
 	}
 
@@ -191,10 +193,11 @@ void VotedSensorsUpdate::parametersUpdate()
 				topic_instance);
 
 			_corrections.baro_mapping[topic_instance] = 0;
+			_corrections.baro_device_ids[topic_instance] = 0;
 
 		} else {
 			_corrections.baro_mapping[topic_instance] = temp;
-
+			_corrections.baro_device_ids[topic_instance] = report.get().device_id;
 		}
 	}
 
@@ -565,9 +568,9 @@ void VotedSensorsUpdate::accelPoll(struct sensor_combined_s &raw)
 		orb_check(_accel.subscription[uorb_index], &accel_updated);
 
 		if (accel_updated) {
-			sensor_accel_s accel_report;
+			sensor_accel_integrated_s accel_report;
 
-			int ret = orb_copy(ORB_ID(sensor_accel), _accel.subscription[uorb_index], &accel_report);
+			int ret = orb_copy(ORB_ID(sensor_accel_integrated), _accel.subscription[uorb_index], &accel_report);
 
 			if (ret != PX4_OK || accel_report.timestamp == 0) {
 				continue; //ignore invalid data
@@ -586,40 +589,17 @@ void VotedSensorsUpdate::accelPoll(struct sensor_combined_s &raw)
 
 			_accel_device_id[uorb_index] = accel_report.device_id;
 
-			Vector3f accel_data;
+			/*
+			 * Correct the raw sensor data for scale factor errors
+			 * and offsets due to temperature variation. It is assumed that any filtering of input
+			 * data required is performed in the sensor driver, preferably before downsampling.
+			*/
 
-			if (accel_report.integral_dt != 0) {
+			// convert the delta velocities to an equivalent acceleration before application of corrections
-				/*
+			const float dt_inv = 1.e6f / (float)accel_report.dt;
-				 * Using data that has been integrated in the driver before downsampling is preferred
+			Vector3f accel_data = Vector3f{accel_report.delta_velocity} * dt_inv;
-				 * becasue it reduces aliasing errors. Correct the raw sensor data for scale factor errors
-				 * and offsets due to temperature variation. It is assumed that any filtering of input
-				 * data required is performed in the sensor driver, preferably before downsampling.
-				*/
 
-				// convert the delta velocities to an equivalent acceleration before application of corrections
+			_last_sensor_data[uorb_index].accelerometer_integral_dt = accel_report.dt;
-				float dt_inv = 1.e6f / accel_report.integral_dt;
-				accel_data = Vector3f(accel_report.x_integral * dt_inv,
-						      accel_report.y_integral * dt_inv,
-						      accel_report.z_integral * dt_inv);
-
-				_last_sensor_data[uorb_index].accelerometer_integral_dt = accel_report.integral_dt;
-
-			} else {
-				// using the value instead of the integral (the integral is the prefered choice)
-
-				// Correct each sensor for temperature effects
-				// Filtering and/or downsampling of temperature should be performed in the driver layer
-				accel_data = Vector3f(accel_report.x, accel_report.y, accel_report.z);
-
-				// handle the cse where this is our first output
-				if (_last_accel_timestamp[uorb_index] == 0) {
-					_last_accel_timestamp[uorb_index] = accel_report.timestamp - 1000;
-				}
-
-				// approximate the  delta time using the difference in accel data time stamps
-				_last_sensor_data[uorb_index].accelerometer_integral_dt =
-					(accel_report.timestamp - _last_accel_timestamp[uorb_index]);
-			}
 
 			// handle temperature compensation
 			if (_temperature_compensation.apply_corrections_accel(uorb_index, accel_data, accel_report.temperature,
@@ -672,9 +652,9 @@ void VotedSensorsUpdate::gyroPoll(struct sensor_combined_s &raw)
 		orb_check(_gyro.subscription[uorb_index], &gyro_updated);
 
 		if (gyro_updated) {
-			sensor_gyro_s gyro_report;
+			sensor_gyro_integrated_s gyro_report;
 
-			int ret = orb_copy(ORB_ID(sensor_gyro), _gyro.subscription[uorb_index], &gyro_report);
+			int ret = orb_copy(ORB_ID(sensor_gyro_integrated), _gyro.subscription[uorb_index], &gyro_report);
 
 			if (ret != PX4_OK || gyro_report.timestamp == 0) {
 				continue; //ignore invalid data
@@ -693,40 +673,17 @@ void VotedSensorsUpdate::gyroPoll(struct sensor_combined_s &raw)
 
 			_gyro_device_id[uorb_index] = gyro_report.device_id;
 
-			Vector3f gyro_rate;
+			/*
+			 * Correct the raw sensor data for scale factor errors
+			 * and offsets due to temperature variation. It is assumed that any filtering of input
+			 * data required is performed in the sensor driver, preferably before downsampling.
+			*/
 
-			if (gyro_report.integral_dt != 0) {
+			// convert the delta angles to an equivalent angular rate before application of corrections
-				/*
+			const float dt_inv = 1.e6f / (float)gyro_report.dt;
-				 * Using data that has been integrated in the driver before downsampling is preferred
+			Vector3f gyro_rate = Vector3f{gyro_report.delta_angle} * dt_inv;
-				 * becasue it reduces aliasing errors. Correct the raw sensor data for scale factor errors
-				 * and offsets due to temperature variation. It is assumed that any filtering of input
-				 * data required is performed in the sensor driver, preferably before downsampling.
-				*/
 
-				// convert the delta angles to an equivalent angular rate before application of corrections
+			_last_sensor_data[uorb_index].gyro_integral_dt = gyro_report.dt;
-				float dt_inv = 1.e6f / gyro_report.integral_dt;
-				gyro_rate = Vector3f(gyro_report.x_integral * dt_inv,
-						     gyro_report.y_integral * dt_inv,
-						     gyro_report.z_integral * dt_inv);
-
-				_last_sensor_data[uorb_index].gyro_integral_dt = gyro_report.integral_dt;
-
-			} else {
-				//using the value instead of the integral (the integral is the prefered choice)
-
-				// Correct each sensor for temperature effects
-				// Filtering and/or downsampling of temperature should be performed in the driver layer
-				gyro_rate = Vector3f(gyro_report.x, gyro_report.y, gyro_report.z);
-
-				// handle the case where this is our first output
-				if (_last_sensor_data[uorb_index].timestamp == 0) {
-					_last_sensor_data[uorb_index].timestamp = gyro_report.timestamp - 1000;
-				}
-
-				// approximate the delta time using the difference in gyro data time stamps
-				_last_sensor_data[uorb_index].gyro_integral_dt =
-					(gyro_report.timestamp - _last_sensor_data[uorb_index].timestamp);
-			}
 
 			// handle temperature compensation
 			if (_temperature_compensation.apply_corrections_gyro(uorb_index, gyro_rate, gyro_report.temperature,

src/modules/sensors/voted_sensors_update.h

@@ -54,9 +54,11 @@
 
 #include <uORB/Publication.hpp>
 #include <uORB/PublicationQueued.hpp>
+#include <uORB/topics/sensor_accel_integrated.h>
 #include <uORB/topics/sensor_combined.h>
 #include <uORB/topics/sensor_preflight.h>
 #include <uORB/topics/sensor_correction.h>
+#include <uORB/topics/sensor_gyro_integrated.h>
 #include <uORB/topics/sensor_selection.h>
 #include <uORB/topics/vehicle_air_data.h>
 #include <uORB/topics/vehicle_magnetometer.h>