move IMU integration out of drivers to sensors hub to handle accel/gyro sync

- IMU integration move from drivers (PX4Accelerometer/PX4Gyroscope) to sensors/vehicle_imu 
 - sensors: voted_sensors_update now consumes vehicle_imu
 - delete sensor_accel_integrated, sensor_gyro_integrated
 - merge sensor_accel_status/sensor_gyro_status into vehicle_imu_status
 - sensors status output minor improvements (ordering, whitespace, show selected sensor device id and instance)

.ci/Jenkinsfile-hardware

@@ -770,19 +770,16 @@ void statusFTDI() {
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "listener logger_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_combined"'
   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"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "listener system_power"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "listener vehicle_attitude"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "listener vehicle_imu"'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "listener vehicle_imu_status"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "listener vehicle_local_position"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "logger status"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ls /"'
@@ -832,19 +829,16 @@ void statusSEGGER() {
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "listener logger_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_combined"'
   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"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "listener system_power"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "listener vehicle_attitude"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "listener vehicle_imu"'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "listener vehicle_imu_status"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "listener vehicle_local_position"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "logger status"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "ls /"'

ROMFS/px4fmu_common/init.d-posix/rcS

@@ -123,8 +123,8 @@ then
 	param set CAL_ACC0_ID 1311244
 	param set CAL_ACC_PRIME 1311244
 
-	param set CAL_GYRO0_ID 2294028
+	param set CAL_GYRO0_ID 1311244
-	param set CAL_GYRO_PRIME 2294028
+	param set CAL_GYRO_PRIME 1311244
 
 	param set CAL_MAG0_ID 197388
 	param set CAL_MAG_PRIME 197388

msg/CMakeLists.txt

@@ -112,15 +112,11 @@ 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_combined.msg
 	sensor_correction.msg
 	sensor_gyro.msg
 	sensor_gyro_fifo.msg
-	sensor_gyro_integrated.msg
-	sensor_gyro_status.msg
 	sensor_mag.msg
 	sensor_preflight.msg
 	sensor_selection.msg
@@ -154,6 +150,7 @@ set(msg_files
 	vehicle_global_position.msg
 	vehicle_gps_position.msg
 	vehicle_imu.msg
+	vehicle_imu_status.msg
 	vehicle_land_detected.msg
 	vehicle_local_position.msg
 	vehicle_local_position_setpoint.msg

msg/sensor_accel.msg

@@ -9,4 +9,8 @@ float32 z                 # acceleration in the NED Z board axis in m/s^2
 
 float32 temperature       # temperature in degrees celsius
 
-uint8 ORB_QUEUE_LENGTH = 4
+uint32 error_count
+
+uint8[3] clip_counter     # clip count per axis in the sample period
+
+uint8 ORB_QUEUE_LENGTH = 8

msg/sensor_accel_integrated.msg

@@ -1,12 +0,0 @@
-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
-
-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[3] clip_counter     # clip count per axis over the integration period

msg/sensor_accel_status.msg

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

msg/sensor_gyro.msg

@@ -9,4 +9,6 @@ float32 z                 # angular velocity in the NED Z board axis in rad/s
 
 float32 temperature       # temperature in degrees celsius
 
-uint8 ORB_QUEUE_LENGTH = 4
+uint32 error_count
+
+uint8 ORB_QUEUE_LENGTH = 8

msg/sensor_gyro_integrated.msg

@@ -1,12 +0,0 @@
-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
-
-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[3] clip_counter     # clip count per axis over the integration period

msg/sensor_gyro_status.msg

@@ -1,19 +0,0 @@
-uint64 timestamp          # time since system start (microseconds)
-
-uint32 device_id          # unique device ID for the sensor that does not change between power cycles
-
-uint64 error_count
-
-float32 temperature
-
-uint8 rotation
-
-uint32[3] clipping        # clipping per axis
-
-uint16 measure_rate_hz
-
-float32 full_scale_range
-
-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)

msg/tools/px_generate_uorb_topic_helper.py

@@ -305,6 +305,18 @@ def print_field(field):
         print("char device_id_buffer[80];")
         print("device::Device::device_id_print_buffer(device_id_buffer, sizeof(device_id_buffer), message.device_id);")
         print("PX4_INFO_RAW(\"\\tdevice_id: %d (%s) \\n\", message.device_id, device_id_buffer);")
+    elif field.name == 'accel_device_id':
+        print("char accel_device_id_buffer[80];")
+        print("device::Device::device_id_print_buffer(accel_device_id_buffer, sizeof(accel_device_id_buffer), message.accel_device_id);")
+        print("PX4_INFO_RAW(\"\\taccel_device_id: %d (%s) \\n\", message.accel_device_id, accel_device_id_buffer);")
+    elif field.name == 'gyro_device_id':
+        print("char gyro_device_id_buffer[80];")
+        print("device::Device::device_id_print_buffer(gyro_device_id_buffer, sizeof(gyro_device_id_buffer), message.gyro_device_id);")
+        print("PX4_INFO_RAW(\"\\tgyro_device_id: %d (%s) \\n\", message.gyro_device_id, gyro_device_id_buffer);")
+    elif field.name == 'baro_device_id':
+        print("char baro_device_id_buffer[80];")
+        print("device::Device::device_id_print_buffer(baro_device_id_buffer, sizeof(baro_device_id_buffer), message.baro_device_id);")
+        print("PX4_INFO_RAW(\"\\tbaro_device_id: %d (%s) \\n\", message.baro_device_id, baro_device_id_buffer);")
     elif is_array and 'char' in field.type:
         print(("PX4_INFO_RAW(\"\\t" + field.name + ": \\\"%." + str(array_length) + "s\\\" \\n\", message." + field.name + ");"))
     else:

msg/tools/uorb_rtps_message_ids.yaml

@@ -267,40 +267,34 @@ rtps:
     id: 117
   - msg: sensor_accel_fifo
     id: 118
-  - msg: sensor_accel_status
-    id: 119
   - msg: sensor_gyro_fifo
-    id: 120
+    id: 119
-  - msg: sensor_gyro_status
-    id: 121
-  - msg: sensor_accel_integrated
-    id: 122
-  - msg: sensor_gyro_integrated
-    id: 123
   - msg: vehicle_imu
-    id: 124
+    id: 120
+  - msg: vehicle_imu_status
+    id: 121
   - msg: vehicle_angular_acceleration
-    id: 125
+    id: 122
   - msg: logger_status
-    id: 126
+    id: 123
   - msg: rpm
-    id: 127
+    id: 124
   - msg: hover_thrust_estimate
-    id: 128
+    id: 125
   - msg: trajectory_bezier
-    id: 129
+    id: 126
   - msg: vehicle_trajectory_bezier
-    id: 130
+    id: 127
   - msg: timesync_status
-    id: 131
+    id: 128
   - msg: orb_test
-    id: 132
+    id: 129
   - msg: orb_test_medium
-    id: 133
+    id: 130
   - msg: orb_test_large
-    id: 134
+    id: 131
   - msg: yaw_estimator_status
-    id: 135
+    id: 132
   ########## multi topics: begin ##########
   - msg: actuator_controls_0
     id: 150

msg/vehicle_imu.msg

@@ -8,8 +8,8 @@ uint32 gyro_device_id           # Gyroscope unique device ID for the sensor that
 
 float32[3] delta_angle          # delta angle in the NED board axis in rad/s over the integration time frame (dt)
 float32[3] delta_velocity       # delta velocity in the NED board axis in m/s over the integration time frame (dt)
-
+uint16 delta_angle_dt           # integration period in us
-uint16 dt                       # integration period in us
+uint16 delta_velocity_dt        # integration period in us
 
 uint8 CLIPPING_X = 1
 uint8 CLIPPING_Y = 2

msg/vehicle_imu_status.msg

@@ -0,0 +1,16 @@
+uint64 timestamp          # time since system start (microseconds)
+
+uint32 accel_device_id    # unique device ID for the sensor that does not change between power cycles
+uint32 gyro_device_id    # unique device ID for the sensor that does not change between power cycles
+
+uint32[3] accel_clipping        # clipping per axis
+
+uint32 accel_error_count
+uint32 gyro_error_count
+
+uint16 accel_rate_hz
+uint16 gyro_rate_hz
+
+float32 accel_vibration_metric  # high frequency vibration level in the IMU delta velocity data (m/s)
+float32 gyro_vibration_metric  # high frequency vibration level in the IMU delta velocity data (m/s)
+float32 gyro_coning_vibration  # Level of coning vibration in the IMU delta angles (rad^2)

src/drivers/drv_sensor.h

@@ -68,13 +68,12 @@
 #define DRV_IMU_DEVTYPE_LSM303D  0x11
 #define DRV_ACC_DEVTYPE_BMA180   0x12
 #define DRV_ACC_DEVTYPE_MPU6000_LEGACY  0x13
-#define DRV_ACC_DEVTYPE_ACCELSIM 0x14
+#define DRV_IMU_DEVTYPE_SIM 0x14
 #define DRV_ACC_DEVTYPE_MPU9250_LEGACY  0x16
 #define DRV_IMU_DEVTYPE_BMI160   0x17
 
 #define DRV_IMU_DEVTYPE_MPU6000  0x21
 #define DRV_GYR_DEVTYPE_L3GD20   0x22
-#define DRV_GYR_DEVTYPE_GYROSIM  0x23
 #define DRV_IMU_DEVTYPE_MPU9250  0x24
 #define DRV_IMU_DEVTYPE_ICM20649 0x25
 #define DRV_IMU_DEVTYPE_ICM42688P 0x26

src/drivers/imu/adis16448/ADIS16448.cpp

@@ -115,9 +115,6 @@ bool ADIS16448::reset()
 		return false;
 	}
 
-	_px4_accel.set_update_rate(_sample_rate);
-	_px4_gyro.set_update_rate(_sample_rate);
-
 	// Set gyroscope scale to default value.
 	//if (!set_gyro_dyn_range(GYRO_INITIAL_SENSITIVITY)) {
 	//	return false;

src/drivers/imu/adis16477/ADIS16477.cpp

@@ -69,10 +69,7 @@ ADIS16477::ADIS16477(I2CSPIBusOption bus_option, int bus, int32_t device, enum R
 #endif // GPIO_SPI1_RESET_ADIS16477
 
 	_px4_accel.set_scale(1.25f * CONSTANTS_ONE_G / 1000.0f); // accel 1.25 mg/LSB
-	_px4_accel.set_update_rate(ADIS16477_DEFAULT_RATE);
-
 	_px4_gyro.set_scale(math::radians(0.025f)); // gyro 0.025 °/sec/LSB
-	_px4_gyro.set_update_rate(ADIS16477_DEFAULT_RATE);
 }
 
 ADIS16477::~ADIS16477()

src/drivers/imu/adis16497/ADIS16497.cpp

@@ -84,9 +84,6 @@ ADIS16497::ADIS16497(I2CSPIBusOption bus_option, int bus, int32_t device, enum R
 	// Configure hardware reset line
 	px4_arch_configgpio(GPIO_SPI1_RESET_ADIS16497);
 #endif // GPIO_SPI1_RESET_ADIS16497
-
-	_px4_accel.set_update_rate(ADIS16497_DEFAULT_RATE);
-	_px4_gyro.set_update_rate(ADIS16497_DEFAULT_RATE);
 }
 
 ADIS16497::~ADIS16497()

src/drivers/imu/bma180/bma180.cpp

@@ -182,7 +182,6 @@ BMA180::BMA180(I2CSPIBusOption bus_option, int bus, int32_t device, enum Rotatio
 	_px4_accel(get_device_id(), ORB_PRIO_MAX, rotation),
 	_sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": read"))
 {
-	_px4_accel.set_update_rate(1000);
 }
 
 BMA180::~BMA180()

src/drivers/imu/bmi055/BMI055_accel.cpp

@@ -56,7 +56,6 @@ BMI055_accel::BMI055_accel(I2CSPIBusOption bus_option, int bus, const char *path
 	_duplicates(perf_alloc(PC_COUNT, "bmi055_accel_duplicates")),
 	_got_duplicate(false)
 {
-	_px4_accel.set_update_rate(BMI055_ACCEL_DEFAULT_RATE);
 }
 
 BMI055_accel::~BMI055_accel()

src/drivers/imu/bmi055/BMI055_gyro.cpp

@@ -58,7 +58,6 @@ BMI055_gyro::BMI055_gyro(I2CSPIBusOption bus_option, int bus, const char *path_g
 	_bad_registers(perf_alloc(PC_COUNT, "bmi055_gyro_bad_registers")),
 	_duplicates(perf_alloc(PC_COUNT, "bmi055_gyro_duplicates"))
 {
-	_px4_gyro.set_update_rate(BMI055_GYRO_DEFAULT_RATE);
 }
 
 BMI055_gyro::~BMI055_gyro()

src/drivers/imu/bmi088/BMI088_accel.cpp

@@ -65,7 +65,6 @@ BMI088_accel::BMI088_accel(I2CSPIBusOption bus_option, int bus, const char *path
 	_duplicates(perf_alloc(PC_COUNT, "bmi088_accel_duplicates")),
 	_got_duplicate(false)
 {
-	_px4_accel.set_update_rate(BMI088_ACCEL_DEFAULT_RATE);
 }
 
 BMI088_accel::~BMI088_accel()

src/drivers/imu/bmi088/BMI088_gyro.cpp

@@ -66,7 +66,6 @@ BMI088_gyro::BMI088_gyro(I2CSPIBusOption bus_option, int bus, const char *path_g
 	_bad_registers(perf_alloc(PC_COUNT, "bmi088_gyro_bad_registers")),
 	_duplicates(perf_alloc(PC_COUNT, "bmi088_gyro_duplicates"))
 {
-	_px4_gyro.set_update_rate(BMI088_GYRO_DEFAULT_RATE);
 }
 
 BMI088_gyro::~BMI088_gyro()

src/drivers/imu/bmi160/bmi160.cpp

@@ -64,7 +64,6 @@ BMI160::BMI160(I2CSPIBusOption bus_option, int bus, int32_t device, enum Rotatio
 	_reset_retries(perf_alloc(PC_COUNT, MODULE_NAME":reset retries")),
 	_duplicates(perf_alloc(PC_COUNT, MODULE_NAME": duplicates"))
 {
-	_px4_gyro.set_update_rate(BMI160_GYRO_DEFAULT_RATE);
 }
 
 BMI160::~BMI160()

src/drivers/imu/fxas21002c/FXAS21002C.cpp

@@ -75,7 +75,6 @@ FXAS21002C::FXAS21002C(device::Device *interface, I2CSPIBusOption bus_option, in
 	_bad_registers(perf_alloc(PC_COUNT, MODULE_NAME": bad register")),
 	_duplicates(perf_alloc(PC_COUNT, MODULE_NAME": duplicate reading"))
 {
-	_px4_gyro.set_update_rate(FXAS21002C_DEFAULT_RATE);
 }
 
 FXAS21002C::~FXAS21002C()

src/drivers/imu/fxos8701cq/FXOS8701CQ.cpp

@@ -66,8 +66,6 @@ FXOS8701CQ::FXOS8701CQ(device::Device *interface, I2CSPIBusOption bus_option, in
 	_bad_registers(perf_alloc(PC_COUNT, MODULE_NAME": bad reg")),
 	_accel_duplicates(perf_alloc(PC_COUNT, MODULE_NAME": acc dupe"))
 {
-	_px4_accel.set_update_rate(FXOS8701C_ACCEL_DEFAULT_RATE);
-
 #if !defined(BOARD_HAS_NOISY_FXOS8700_MAG)
 	_px4_mag.set_scale(0.001f);
 #endif

src/drivers/imu/invensense/icm20602/ICM20602.cpp

@@ -316,9 +316,6 @@ void ICM20602::ConfigureSampleRate(int sample_rate)
 
 	_fifo_accel_samples = math::min(_fifo_empty_interval_us / (1e6f / ACCEL_RATE), (float)FIFO_MAX_SAMPLES);
 
-	_px4_accel.set_update_rate(1e6f / _fifo_empty_interval_us);
-	_px4_gyro.set_update_rate(1e6f / _fifo_empty_interval_us);
-
 	ConfigureFIFOWatermark(_fifo_gyro_samples);
 }
 

src/drivers/imu/invensense/icm20608g/ICM20608G.cpp

@@ -328,9 +328,6 @@ void ICM20608G::ConfigureSampleRate(int sample_rate)
 	_fifo_empty_interval_us = _fifo_gyro_samples * (1e6f / GYRO_RATE);
 
 	_fifo_accel_samples = math::min(_fifo_empty_interval_us / (1e6f / ACCEL_RATE), (float)FIFO_MAX_SAMPLES);
-
-	_px4_accel.set_update_rate(1e6f / _fifo_empty_interval_us);
-	_px4_gyro.set_update_rate(1e6f / _fifo_empty_interval_us);
 }
 
 bool ICM20608G::Configure()

src/drivers/imu/invensense/icm20649/ICM20649.cpp

@@ -330,9 +330,6 @@ void ICM20649::ConfigureSampleRate(int sample_rate)
 	_fifo_empty_interval_us = _fifo_gyro_samples * (1e6f / GYRO_RATE);
 
 	_fifo_accel_samples = roundf(math::min(_fifo_empty_interval_us / (1e6f / ACCEL_RATE), (float)FIFO_MAX_SAMPLES));
-
-	_px4_accel.set_update_rate(1e6f / _fifo_empty_interval_us);
-	_px4_gyro.set_update_rate(1e6f / _fifo_empty_interval_us);
 }
 
 void ICM20649::SelectRegisterBank(enum REG_BANK_SEL_BIT bank)

src/drivers/imu/invensense/icm20689/ICM20689.cpp

@@ -328,9 +328,6 @@ void ICM20689::ConfigureSampleRate(int sample_rate)
 	_fifo_empty_interval_us = _fifo_gyro_samples * (1e6f / GYRO_RATE);
 
 	_fifo_accel_samples = math::min(_fifo_empty_interval_us / (1e6f / ACCEL_RATE), (float)FIFO_MAX_SAMPLES);
-
-	_px4_accel.set_update_rate(1e6f / _fifo_empty_interval_us);
-	_px4_gyro.set_update_rate(1e6f / _fifo_empty_interval_us);
 }
 
 bool ICM20689::Configure()

src/drivers/imu/invensense/icm20948/ICM20948.cpp

@@ -364,9 +364,6 @@ void ICM20948::ConfigureSampleRate(int sample_rate)
 	_fifo_empty_interval_us = _fifo_gyro_samples * (1e6f / GYRO_RATE);
 
 	_fifo_accel_samples = roundf(math::min(_fifo_empty_interval_us / (1e6f / ACCEL_RATE), (float)FIFO_MAX_SAMPLES));
-
-	_px4_accel.set_update_rate(1e6f / _fifo_empty_interval_us);
-	_px4_gyro.set_update_rate(1e6f / _fifo_empty_interval_us);
 }
 
 void ICM20948::SelectRegisterBank(enum REG_BANK_SEL_BIT bank)

src/drivers/imu/invensense/icm40609d/ICM40609D.cpp

@@ -322,9 +322,6 @@ void ICM40609D::ConfigureSampleRate(int sample_rate)
 
 	_fifo_accel_samples = math::min(_fifo_empty_interval_us / (1e6f / ACCEL_RATE), (float)FIFO_MAX_SAMPLES);
 
-	_px4_accel.set_update_rate(1e6f / _fifo_empty_interval_us);
-	_px4_gyro.set_update_rate(1e6f / _fifo_empty_interval_us);
-
 	ConfigureFIFOWatermark(_fifo_gyro_samples);
 }
 

src/drivers/imu/invensense/icm42688p/ICM42688P.cpp

@@ -324,9 +324,6 @@ void ICM42688P::ConfigureSampleRate(int sample_rate)
 
 	_fifo_accel_samples = math::min(_fifo_empty_interval_us / (1e6f / ACCEL_RATE), (float)FIFO_MAX_SAMPLES);
 
-	_px4_accel.set_update_rate(1e6f / _fifo_empty_interval_us);
-	_px4_gyro.set_update_rate(1e6f / _fifo_empty_interval_us);
-
 	ConfigureFIFOWatermark(_fifo_gyro_samples);
 }
 

src/drivers/imu/invensense/mpu6000/MPU6000.cpp

@@ -323,9 +323,6 @@ void MPU6000::ConfigureSampleRate(int sample_rate)
 	_fifo_empty_interval_us = _fifo_gyro_samples * (1e6f / GYRO_RATE);
 
 	_fifo_accel_samples = math::min(_fifo_empty_interval_us / (1e6f / ACCEL_RATE), (float)FIFO_MAX_SAMPLES);
-
-	_px4_accel.set_update_rate(1e6f / _fifo_empty_interval_us);
-	_px4_gyro.set_update_rate(1e6f / _fifo_empty_interval_us);
 }
 
 bool MPU6000::Configure()

src/drivers/imu/invensense/mpu6500/MPU6500.cpp

@@ -323,9 +323,6 @@ void MPU6500::ConfigureSampleRate(int sample_rate)
 	_fifo_empty_interval_us = _fifo_gyro_samples * (1e6f / GYRO_RATE);
 
 	_fifo_accel_samples = math::min(_fifo_empty_interval_us / (1e6f / ACCEL_RATE), (float)FIFO_MAX_SAMPLES);
-
-	_px4_accel.set_update_rate(1e6f / _fifo_empty_interval_us);
-	_px4_gyro.set_update_rate(1e6f / _fifo_empty_interval_us);
 }
 
 bool MPU6500::Configure()

src/drivers/imu/invensense/mpu9250/MPU9250.cpp

@@ -356,9 +356,6 @@ void MPU9250::ConfigureSampleRate(int sample_rate)
 	_fifo_empty_interval_us = _fifo_gyro_samples * (1e6f / GYRO_RATE);
 
 	_fifo_accel_samples = math::min(_fifo_empty_interval_us / (1e6f / ACCEL_RATE), (float)FIFO_MAX_SAMPLES);
-
-	_px4_accel.set_update_rate(1e6f / _fifo_empty_interval_us);
-	_px4_gyro.set_update_rate(1e6f / _fifo_empty_interval_us);
 }
 
 bool MPU9250::Configure()

src/drivers/imu/l3gd20/L3GD20.cpp

@@ -45,7 +45,6 @@ L3GD20::L3GD20(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotati
 	_bad_registers(perf_alloc(PC_COUNT, MODULE_NAME": bad_reg")),
 	_duplicates(perf_alloc(PC_COUNT, MODULE_NAME": dupe"))
 {
-	_px4_gyro.set_update_rate(L3GD20_DEFAULT_RATE);
 }
 
 L3GD20::~L3GD20()

src/drivers/imu/lsm303d/LSM303D.cpp

@@ -127,7 +127,6 @@ LSM303D::reset()
 
 	accel_set_range(LSM303D_ACCEL_DEFAULT_RANGE_G);
 	accel_set_samplerate(LSM303D_ACCEL_DEFAULT_RATE);
-	_px4_accel.set_update_rate(LSM303D_ACCEL_DEFAULT_RATE);
 
 	// we setup the anti-alias on-chip filter as 50Hz. We believe
 	// this operates in the analog domain, and is critical for

src/drivers/imu/mpu6000/MPU6000.cpp

@@ -160,8 +160,6 @@ int MPU6000::reset()
 
 	// SAMPLE RATE
 	_set_sample_rate(1000);
-	_px4_accel.set_update_rate(1000);
-	_px4_gyro.set_update_rate(1000);
 	px4_usleep(1000);
 
 	_set_dlpf_filter(MPU6000_DEFAULT_ONCHIP_FILTER_FREQ);

src/drivers/imu/mpu9250/mpu9250.cpp

@@ -189,8 +189,6 @@ MPU9250::reset_mpu()
 
 	// SAMPLE RATE
 	_set_sample_rate(_sample_rate);
-	_px4_accel.set_update_rate(_sample_rate);
-	_px4_gyro.set_update_rate(_sample_rate);
 
 	_set_dlpf_filter(MPU9250_DEFAULT_ONCHIP_FILTER_FREQ);
 

src/drivers/imu/st/ism330dlc/ISM330DLC.cpp

@@ -47,8 +47,6 @@ ISM330DLC::ISM330DLC(I2CSPIBusOption bus_option, int bus, uint32_t device, enum
 	_px4_accel(get_device_id(), ORB_PRIO_DEFAULT, rotation),
 	_px4_gyro(get_device_id(), ORB_PRIO_DEFAULT, rotation)
 {
-	_px4_accel.set_update_rate(1000000 / FIFO_INTERVAL);
-	_px4_gyro.set_update_rate(1000000 / FIFO_INTERVAL);
 }
 
 ISM330DLC::~ISM330DLC()

src/drivers/imu/st/lsm9ds1/LSM9DS1.cpp

@@ -45,8 +45,6 @@ LSM9DS1::LSM9DS1(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rota
 	_px4_accel(get_device_id(), ORB_PRIO_DEFAULT, rotation),
 	_px4_gyro(get_device_id(), ORB_PRIO_DEFAULT, rotation)
 {
-	_px4_accel.set_update_rate(1000000 / _fifo_interval);
-	_px4_gyro.set_update_rate(1000000 / _fifo_interval);
 }
 
 LSM9DS1::~LSM9DS1()

src/lib/drivers/accelerometer/PX4Accelerometer.cpp

@@ -68,21 +68,14 @@ PX4Accelerometer::PX4Accelerometer(uint32_t device_id, ORB_PRIO priority, enum R
 	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}
 {
 	// register class and advertise immediately to keep instance numbering in sync
 	_class_device_instance = register_class_devname(ACCEL_BASE_DEVICE_PATH);
 	_sensor_pub.advertise();
-	_sensor_integrated_pub.advertise();
-	_sensor_status_pub.advertise();
 
 	updateParams();
-
-	// set reasonable default, driver should be setting real value
-	set_update_rate(800);
 }
 
 PX4Accelerometer::~PX4Accelerometer()
@@ -93,8 +86,6 @@ PX4Accelerometer::~PX4Accelerometer()
 
 	_sensor_pub.unadvertise();
 	_sensor_fifo_pub.unadvertise();
-	_sensor_integrated_pub.unadvertise();
-	_sensor_status_pub.unadvertise();
 }
 
 int PX4Accelerometer::ioctl(cdev::file_t *filp, int cmd, unsigned long arg)
@@ -132,25 +123,6 @@ void PX4Accelerometer::set_device_type(uint8_t devtype)
 	_device_id = device_id.devid;
 }
 
-void PX4Accelerometer::set_update_rate(uint16_t rate)
-{
-	_update_rate = math::constrain((int)rate, 50, 32000);
-
-	// constrain IMU integration time 1-20 milliseconds (50-1000 Hz)
-	int32_t imu_integration_rate_hz = math::constrain(_param_imu_integ_rate.get(), 50, 1000);
-
-	if (imu_integration_rate_hz != _param_imu_integ_rate.get()) {
-		_param_imu_integ_rate.set(imu_integration_rate_hz);
-		_param_imu_integ_rate.commit_no_notification();
-	}
-
-	const float update_interval_us = 1e6f / _update_rate;
-	const float imu_integration_interval_us = 1e6f / (float)imu_integration_rate_hz;
-
-	_integrator_reset_samples = roundf(imu_integration_interval_us / update_interval_us);
-	_integrator.set_autoreset_interval(_integrator_reset_samples * update_interval_us);
-}
-
 void PX4Accelerometer::update(hrt_abstime timestamp_sample, float x, float y, float z)
 {
 	// Apply rotation (before scaling)
@@ -158,67 +130,32 @@ void PX4Accelerometer::update(hrt_abstime timestamp_sample, float x, float y, fl
 
 	const Vector3f raw{x, y, z};
 
-	// Clipping (check unscaled raw values)
+	// clipping
-	for (int i = 0; i < 3; i++) {
+	float clip_count_x = (fabsf(raw(0)) > _clip_limit);
-		if (fabsf(raw(i)) > _clip_limit) {
+	float clip_count_y = (fabsf(raw(1)) > _clip_limit);
-			_clipping_total[i]++;
+	float clip_count_z = (fabsf(raw(2)) > _clip_limit);
-			_integrator_clipping(i)++;
+
-		}
+	rotate_3f(_rotation, clip_count_x, clip_count_y, clip_count_z);
-	}
 
 	// Apply range scale and the calibrating offset/scale
 	const Vector3f val_calibrated{(((raw * _scale) - _calibration_offset).emult(_calibration_scale))};
 
-	// publish raw data immediately
+	// publish
-	{
+	sensor_accel_s report;
-		sensor_accel_s report;
 
-		report.timestamp_sample = timestamp_sample;
+	report.timestamp_sample = timestamp_sample;
-		report.device_id = _device_id;
+	report.device_id = _device_id;
-		report.temperature = _temperature;
+	report.temperature = _temperature;
-		report.x = val_calibrated(0);
+	report.error_count = _error_count;
-		report.y = val_calibrated(1);
+	report.x = val_calibrated(0);
-		report.z = val_calibrated(2);
+	report.y = val_calibrated(1);
-		report.timestamp = hrt_absolute_time();
+	report.z = val_calibrated(2);
+	report.clip_counter[0] = fabsf(roundf(clip_count_x));
+	report.clip_counter[1] = fabsf(roundf(clip_count_y));
+	report.clip_counter[2] = fabsf(roundf(clip_count_z));
+	report.timestamp = hrt_absolute_time();
 
-		_sensor_pub.publish(report);
+	_sensor_pub.publish(report);
-	}
-
-	// Integrated values
-	Vector3f delta_velocity;
-	uint32_t integral_dt = 0;
-
-	_integrator_samples++;
-
-	if (_integrator.put(timestamp_sample, val_calibrated, delta_velocity, integral_dt)) {
-
-		// fill sensor_accel_integrated and publish
-		sensor_accel_integrated_s report;
-
-		report.timestamp_sample = timestamp_sample;
-		report.error_count = _error_count;
-		report.device_id = _device_id;
-		delta_velocity.copyTo(report.delta_velocity);
-		report.dt = integral_dt;
-		report.samples = _integrator_samples;
-
-		for (int i = 0; i < 3; i++) {
-			report.clip_counter[i] = fabsf(roundf(_integrator_clipping(i)));
-		}
-
-		report.timestamp = hrt_absolute_time();
-
-		_sensor_integrated_pub.publish(report);
-
-
-		// reset integrator
-		ResetIntegrator();
-
-		// update vibration metrics
-		UpdateVibrationMetrics(delta_velocity);
-	}
-
-	PublishStatus();
 }
 
 void PX4Accelerometer::updateFIFO(const FIFOSample &sample)
@@ -226,110 +163,57 @@ void PX4Accelerometer::updateFIFO(const FIFOSample &sample)
 	const uint8_t N = sample.samples;
 	const float dt = sample.dt;
 
-	// publish raw data immediately
 	{
-		// average
+		// trapezoidal integration (equally spaced, scaled by dt later)
-		float x = (float)sum(sample.x, N) / (float)N;
+		Vector3f integral{
-		float y = (float)sum(sample.y, N) / (float)N;
+			(0.5f * (_last_sample[0] + sample.x[N - 1]) + sum(sample.x, N - 1)),
-		float z = (float)sum(sample.z, N) / (float)N;
+			(0.5f * (_last_sample[1] + sample.y[N - 1]) + sum(sample.y, N - 1)),
+			(0.5f * (_last_sample[2] + sample.z[N - 1]) + sum(sample.z, N - 1)),
+		};
+
+		_last_sample[0] = sample.x[N - 1];
+		_last_sample[1] = sample.y[N - 1];
+		_last_sample[2] = sample.z[N - 1];
+
+		// clipping
+		float clip_count_x = clipping(sample.x, _clip_limit, N);
+		float clip_count_y = clipping(sample.y, _clip_limit, N);
+		float clip_count_z = clipping(sample.z, _clip_limit, N);
+
+		rotate_3f(_rotation, clip_count_x, clip_count_y, clip_count_z);
+
 
 		// Apply rotation (before scaling)
-		rotate_3f(_rotation, x, y, z);
+		rotate_3f(_rotation, integral(0), integral(1), integral(2));
 
-		// Apply range scale and the calibrating offset/scale
+		// average
+		const float x = integral(0) / (float)N;
+		const float y = integral(1) / (float)N;
+		const float z = integral(2) / (float)N;
+
+		// Apply range scale and the calibration offset/scale
 		const Vector3f val_calibrated{((Vector3f{x, y, z} * _scale) - _calibration_offset).emult(_calibration_scale)};
 
+		// publish
 		sensor_accel_s report;
 
 		report.timestamp_sample = sample.timestamp_sample;
 		report.device_id = _device_id;
 		report.temperature = _temperature;
+		report.error_count = _error_count;
 		report.x = val_calibrated(0);
 		report.y = val_calibrated(1);
 		report.z = val_calibrated(2);
+		report.clip_counter[0] = fabsf(roundf(clip_count_x));
+		report.clip_counter[1] = fabsf(roundf(clip_count_y));
+		report.clip_counter[2] = fabsf(roundf(clip_count_z));
 		report.timestamp = hrt_absolute_time();
 
 		_sensor_pub.publish(report);
 	}
 
 
-	// clipping
+	// publish fifo
-	unsigned clip_count_x = clipping(sample.x, _clip_limit, N);
-	unsigned clip_count_y = clipping(sample.y, _clip_limit, N);
-	unsigned clip_count_z = clipping(sample.z, _clip_limit, N);
-
-	_clipping_total[0] += clip_count_x;
-	_clipping_total[1] += clip_count_y;
-	_clipping_total[2] += clip_count_z;
-
-	_integrator_clipping(0) += clip_count_x;
-	_integrator_clipping(1) += clip_count_y;
-	_integrator_clipping(2) += clip_count_z;
-
-	// integrated data (INS)
-	{
-		// reset integrator if previous sample was too long ago
-		if ((sample.timestamp_sample > _timestamp_sample_prev)
-		    && ((sample.timestamp_sample - _timestamp_sample_prev) > (N * dt * 2.0f))) {
-
-			ResetIntegrator();
-		}
-
-		// integrate
-		_integrator_samples += 1;
-		_integrator_fifo_samples += N;
-
-		// trapezoidal integration (equally spaced, scaled by dt later)
-		_integration_raw(0) += (0.5f * (_last_sample[0] + sample.x[N - 1]) + sum(sample.x, N - 1));
-		_integration_raw(1) += (0.5f * (_last_sample[1] + sample.y[N - 1]) + sum(sample.y, N - 1));
-		_integration_raw(2) += (0.5f * (_last_sample[2] + sample.z[N - 1]) + sum(sample.z, N - 1));
-		_last_sample[0] = sample.x[N - 1];
-		_last_sample[1] = sample.y[N - 1];
-		_last_sample[2] = sample.z[N - 1];
-
-
-		if (_integrator_fifo_samples > 0 && (_integrator_samples >= _integrator_reset_samples)) {
-
-			// Apply rotation (before scaling)
-			rotate_3f(_rotation, _integration_raw(0), _integration_raw(1), _integration_raw(2));
-
-			// scale calibration offset to number of samples
-			const Vector3f offset{_calibration_offset * _integrator_fifo_samples};
-
-			// Apply calibration and scale to seconds
-			const Vector3f delta_velocity{((_integration_raw * _scale) - offset).emult(_calibration_scale) * 1e-6f * dt};
-
-			// fill sensor_accel_integrated and publish
-			sensor_accel_integrated_s report;
-
-			report.timestamp_sample = sample.timestamp_sample;
-			report.error_count = _error_count;
-			report.device_id = _device_id;
-			delta_velocity.copyTo(report.delta_velocity);
-			report.dt = _integrator_fifo_samples * dt; // time span in microseconds
-			report.samples = _integrator_fifo_samples;
-
-			rotate_3f(_rotation, _integrator_clipping(0), _integrator_clipping(1), _integrator_clipping(2));
-			const Vector3f clipping{_integrator_clipping};
-
-			for (int i = 0; i < 3; i++) {
-				report.clip_counter[i] = fabsf(roundf(clipping(i)));
-			}
-
-			report.timestamp = hrt_absolute_time();
-			_sensor_integrated_pub.publish(report);
-
-			// update vibration metrics
-			UpdateVibrationMetrics(delta_velocity);
-
-			// reset integrator
-			ResetIntegrator();
-		}
-
-		_timestamp_sample_prev = sample.timestamp_sample;
-	}
-
-	// publish sensor fifo
 	sensor_accel_fifo_s fifo{};
 
 	fifo.device_id = _device_id;
@@ -344,42 +228,6 @@ void PX4Accelerometer::updateFIFO(const FIFOSample &sample)
 
 	fifo.timestamp = hrt_absolute_time();
 	_sensor_fifo_pub.publish(fifo);
-
-
-	PublishStatus();
-}
-
-void PX4Accelerometer::PublishStatus()
-{
-	// 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_hz = _update_rate;
-		status.temperature = _temperature;
-		status.vibration_metric = _vibration_metric;
-		status.clipping[0] = _clipping_total[0];
-		status.clipping[1] = _clipping_total[1];
-		status.clipping[2] = _clipping_total[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;
-	_integration_raw.zero();
-	_integrator_clipping.zero();
-
-	_timestamp_sample_prev = 0;
 }
 
 void PX4Accelerometer::UpdateClipLimit()
@@ -388,15 +236,6 @@ void PX4Accelerometer::UpdateClipLimit()
 	_clip_limit = fmaxf((_range / _scale) * 0.999f, INT16_MAX);
 }
 
-void 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;
-	_vibration_metric = 0.99f * _vibration_metric + 0.01f * delta_velocity_diff.norm();
-
-	_delta_velocity_prev = delta_velocity;
-}
-
 void PX4Accelerometer::print_status()
 {
 #if !defined(CONSTRAINED_FLASH)

src/lib/drivers/accelerometer/PX4Accelerometer.hpp

@@ -37,15 +37,12 @@
 #include <drivers/drv_hrt.h>
 #include <lib/cdev/CDev.hpp>
 #include <lib/conversion/rotation.h>
-#include <lib/drivers/device/integrator.h>
 #include <lib/ecl/geo/geo.h>
 #include <px4_platform_common/module_params.h>
 #include <uORB/PublicationMulti.hpp>
 #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
 {
@@ -64,7 +61,6 @@ public:
 	void set_range(float range) { _range = range; UpdateClipLimit(); }
 	void set_scale(float scale) { _scale = scale; UpdateClipLimit(); }
 	void set_temperature(float temperature) { _temperature = temperature; }
-	void set_update_rate(uint16_t rate);
 
 	void update(hrt_abstime timestamp_sample, float x, float y, float z);
 
@@ -86,27 +82,14 @@ public:
 	void updateFIFO(const FIFOSample &sample);
 
 private:
-
-	void PublishStatus();
-	void ResetIntegrator();
 	void UpdateClipLimit();
-	void UpdateVibrationMetrics(const matrix::Vector3f &delta_velocity);
 
 	uORB::PublicationQueuedMulti<sensor_accel_s>      _sensor_pub;
 	uORB::PublicationMulti<sensor_accel_fifo_s>       _sensor_fifo_pub;
-	uORB::PublicationMulti<sensor_accel_integrated_s> _sensor_integrated_pub;
-	uORB::PublicationMulti<sensor_accel_status_s>     _sensor_status_pub;
-
-	hrt_abstime	_status_last_publish{0};
-
-	Integrator		_integrator{5000, false}; // 200 Hz default
 
 	matrix::Vector3f	_calibration_scale{1.f, 1.f, 1.f};
 	matrix::Vector3f	_calibration_offset{0.f, 0.f, 0.f};
 
-	matrix::Vector3f _delta_velocity_prev{0.f, 0.f, 0.f};	// delta velocity from the previous IMU measurement
-	float _vibration_metric{0.f};	// high frequency vibration level in the IMU delta velocity data (m/s)
-
 	int			_class_device_instance{-1};
 
 	uint32_t		_device_id{0};
@@ -116,24 +99,9 @@ private:
 	float			_scale{1.f};
 	float			_temperature{0.f};
 
-	int16_t			_clip_limit{(int16_t)(_range / _scale)};
+	float			_clip_limit{_range / _scale};
 
-	uint64_t		_error_count{0};
+	uint32_t		_error_count{0};
 
-	uint32_t		_clipping_total[3] {};
-
-	uint16_t		_update_rate{1000};
-
-	// integrator
-	hrt_abstime		_timestamp_sample_prev{0};
-	matrix::Vector3f	_integration_raw{};
-	matrix::Vector3f	_integrator_clipping{};
 	int16_t			_last_sample[3] {};
-	uint8_t			_integrator_reset_samples{4};
-	uint8_t			_integrator_samples{0};
-	uint8_t			_integrator_fifo_samples{0};
-
-	DEFINE_PARAMETERS(
-		(ParamInt<px4::params::IMU_INTEG_RATE>) _param_imu_integ_rate
-	)
 };

src/lib/drivers/device/CMakeLists.txt

@@ -56,7 +56,6 @@ endif()
 px4_add_library(drivers__device
 	CDev.cpp
 	ringbuffer.cpp
-	integrator.cpp
 	${SRCS_PLATFORM}
 	)
 

src/lib/drivers/gyroscope/PX4Gyroscope.cpp

@@ -50,39 +50,19 @@ static inline int32_t sum(const int16_t samples[16], uint8_t len)
 	return sum;
 }
 
-static constexpr unsigned clipping(const int16_t samples[16], int16_t clip_limit, uint8_t len)
-{
-	unsigned clip_count = 0;
-
-	for (int n = 0; n < len; n++) {
-		if (abs(samples[n]) >= clip_limit) {
-			clip_count++;
-		}
-	}
-
-	return clip_count;
-}
-
 PX4Gyroscope::PX4Gyroscope(uint32_t device_id, ORB_PRIO priority, enum Rotation rotation) :
 	CDev(nullptr),
 	ModuleParams(nullptr),
 	_sensor_pub{ORB_ID(sensor_gyro), 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}
 {
 	// register class and advertise immediately to keep instance numbering in sync
 	_class_device_instance = register_class_devname(GYRO_BASE_DEVICE_PATH);
 	_sensor_pub.advertise();
-	_sensor_integrated_pub.advertise();
-	_sensor_status_pub.advertise();
 
 	updateParams();
-
-	// set reasonable default, driver should be setting real value
-	set_update_rate(800);
 }
 
 PX4Gyroscope::~PX4Gyroscope()
@@ -93,8 +73,6 @@ PX4Gyroscope::~PX4Gyroscope()
 
 	_sensor_pub.unadvertise();
 	_sensor_fifo_pub.unadvertise();
-	_sensor_integrated_pub.unadvertise();
-	_sensor_status_pub.unadvertise();
 }
 
 int PX4Gyroscope::ioctl(cdev::file_t *filp, int cmd, unsigned long arg)
@@ -131,25 +109,6 @@ void PX4Gyroscope::set_device_type(uint8_t devtype)
 	_device_id = device_id.devid;
 }
 
-void PX4Gyroscope::set_update_rate(uint16_t rate)
-{
-	_update_rate = math::constrain((int)rate, 50, 32000);
-
-	// constrain IMU integration time 1-20 milliseconds (50-1000 Hz)
-	int32_t imu_integration_rate_hz = math::constrain(_param_imu_integ_rate.get(), 50, 1000);
-
-	if (imu_integration_rate_hz != _param_imu_integ_rate.get()) {
-		_param_imu_integ_rate.set(imu_integration_rate_hz);
-		_param_imu_integ_rate.commit_no_notification();
-	}
-
-	const float update_interval_us = 1e6f / _update_rate;
-	const float imu_integration_interval_us = 1e6f / (float)imu_integration_rate_hz;
-
-	_integrator_reset_samples = roundf(imu_integration_interval_us / update_interval_us);
-	_integrator.set_autoreset_interval(_integrator_reset_samples * update_interval_us);
-}
-
 void PX4Gyroscope::update(hrt_abstime timestamp_sample, float x, float y, float z)
 {
 	// Apply rotation (before scaling)
@@ -157,67 +116,22 @@ void PX4Gyroscope::update(hrt_abstime timestamp_sample, float x, float y, float
 
 	const Vector3f raw{x, y, z};
 
-	// Clipping (check unscaled raw values)
-	for (int i = 0; i < 3; i++) {
-		if (fabsf(raw(i)) > _clip_limit) {
-			_clipping_total[i]++;
-			_integrator_clipping(i)++;
-		}
-	}
-
 	// Apply range scale and the calibrating offset/scale
 	const Vector3f val_calibrated{((raw * _scale) - _calibration_offset)};
 
-	// publish raw data immediately
+	// publish
-	{
+	sensor_gyro_s report;
-		sensor_gyro_s report;
 
-		report.timestamp_sample = timestamp_sample;
+	report.timestamp_sample = timestamp_sample;
-		report.device_id = _device_id;
+	report.device_id = _device_id;
-		report.temperature = _temperature;
+	report.temperature = _temperature;
-		report.x = val_calibrated(0);
+	report.error_count = _error_count;
-		report.y = val_calibrated(1);
+	report.x = val_calibrated(0);
-		report.z = val_calibrated(2);
+	report.y = val_calibrated(1);
-		report.timestamp = hrt_absolute_time();
+	report.z = val_calibrated(2);
+	report.timestamp = hrt_absolute_time();
 
-		_sensor_pub.publish(report);
+	_sensor_pub.publish(report);
-	}
-
-	// Integrated values
-	Vector3f delta_angle;
-	uint32_t integral_dt = 0;
-
-	_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 = timestamp_sample;
-		report.error_count = _error_count;
-		report.device_id = _device_id;
-		delta_angle.copyTo(report.delta_angle);
-		report.dt = integral_dt;
-		report.samples = _integrator_samples;
-
-		for (int i = 0; i < 3; i++) {
-			report.clip_counter[i] = fabsf(roundf(_integrator_clipping(i)));
-		}
-
-		report.timestamp = hrt_absolute_time();
-
-		_sensor_integrated_pub.publish(report);
-
-
-		// reset integrator
-		ResetIntegrator();
-
-		// update vibration metrics
-		UpdateVibrationMetrics(delta_angle);
-	}
-
-	PublishStatus();
 }
 
 void PX4Gyroscope::updateFIFO(const FIFOSample &sample)
@@ -225,24 +139,36 @@ void PX4Gyroscope::updateFIFO(const FIFOSample &sample)
 	const uint8_t N = sample.samples;
 	const float dt = sample.dt;
 
-	// publish raw data immediately
 	{
-		// average
+		// trapezoidal integration (equally spaced, scaled by dt later)
-		float x = (float)sum(sample.x, N) / (float)N;
+		Vector3f integral{
-		float y = (float)sum(sample.y, N) / (float)N;
+			(0.5f * (_last_sample[0] + sample.x[N - 1]) + sum(sample.x, N - 1)),
-		float z = (float)sum(sample.z, N) / (float)N;
+			(0.5f * (_last_sample[1] + sample.y[N - 1]) + sum(sample.y, N - 1)),
+			(0.5f * (_last_sample[2] + sample.z[N - 1]) + sum(sample.z, N - 1)),
+		};
+
+		_last_sample[0] = sample.x[N - 1];
+		_last_sample[1] = sample.y[N - 1];
+		_last_sample[2] = sample.z[N - 1];
 
 		// Apply rotation (before scaling)
-		rotate_3f(_rotation, x, y, z);
+		rotate_3f(_rotation, integral(0), integral(1), integral(2));
+
+		// average
+		const float x = integral(0) / (float)N;
+		const float y = integral(1) / (float)N;
+		const float z = integral(2) / (float)N;
 
 		// Apply range scale and the calibration offset
 		const Vector3f val_calibrated{(Vector3f{x, y, z} * _scale) - _calibration_offset};
 
+		// publish
 		sensor_gyro_s report;
 
 		report.timestamp_sample = sample.timestamp_sample;
 		report.device_id = _device_id;
 		report.temperature = _temperature;
+		report.error_count = _error_count;
 		report.x = val_calibrated(0);
 		report.y = val_calibrated(1);
 		report.z = val_calibrated(2);
@@ -252,83 +178,7 @@ void PX4Gyroscope::updateFIFO(const FIFOSample &sample)
 	}
 
 
-	// clipping
+	// publish fifo
-	unsigned clip_count_x = clipping(sample.x, _clip_limit, N);
-	unsigned clip_count_y = clipping(sample.y, _clip_limit, N);
-	unsigned clip_count_z = clipping(sample.z, _clip_limit, N);
-
-	_clipping_total[0] += clip_count_x;
-	_clipping_total[1] += clip_count_y;
-	_clipping_total[2] += clip_count_z;
-
-	_integrator_clipping(0) += clip_count_x;
-	_integrator_clipping(1) += clip_count_y;
-	_integrator_clipping(2) += clip_count_z;
-
-	// integrated data (INS)
-	{
-		// reset integrator if previous sample was too long ago
-		if ((sample.timestamp_sample > _timestamp_sample_prev)
-		    && ((sample.timestamp_sample - _timestamp_sample_prev) > (N * dt * 2.0f))) {
-
-			ResetIntegrator();
-		}
-
-		// integrate
-		_integrator_samples += 1;
-		_integrator_fifo_samples += N;
-
-		// trapezoidal integration (equally spaced, scaled by dt later)
-		_integration_raw(0) += (0.5f * (_last_sample[0] + sample.x[N - 1]) + sum(sample.x, N - 1));
-		_integration_raw(1) += (0.5f * (_last_sample[1] + sample.y[N - 1]) + sum(sample.y, N - 1));
-		_integration_raw(2) += (0.5f * (_last_sample[2] + sample.z[N - 1]) + sum(sample.z, N - 1));
-		_last_sample[0] = sample.x[N - 1];
-		_last_sample[1] = sample.y[N - 1];
-		_last_sample[2] = sample.z[N - 1];
-
-
-		if (_integrator_fifo_samples > 0 && (_integrator_samples >= _integrator_reset_samples)) {
-
-			// Apply rotation (before scaling)
-			rotate_3f(_rotation, _integration_raw(0), _integration_raw(1), _integration_raw(2));
-
-			// scale calibration offset to number of samples
-			const Vector3f offset{_calibration_offset * _integrator_fifo_samples};
-
-			// Apply calibration and scale to seconds
-			const Vector3f delta_angle{((_integration_raw * _scale) - offset) * 1e-6f * dt};
-
-			// fill sensor_gyro_integrated and publish
-			sensor_gyro_integrated_s report;
-
-			report.timestamp_sample = sample.timestamp_sample;
-			report.error_count = _error_count;
-			report.device_id = _device_id;
-			delta_angle.copyTo(report.delta_angle);
-			report.dt = _integrator_fifo_samples * dt; // time span in microseconds
-			report.samples = _integrator_fifo_samples;
-
-			rotate_3f(_rotation, _integrator_clipping(0), _integrator_clipping(1), _integrator_clipping(2));
-			const Vector3f clipping{_integrator_clipping};
-
-			for (int i = 0; i < 3; i++) {
-				report.clip_counter[i] = fabsf(roundf(clipping(i)));
-			}
-
-			report.timestamp = hrt_absolute_time();
-			_sensor_integrated_pub.publish(report);
-
-			// update vibration metrics
-			UpdateVibrationMetrics(delta_angle);
-
-			// reset integrator
-			ResetIntegrator();
-		}
-
-		_timestamp_sample_prev = sample.timestamp_sample;
-	}
-
-	// publish sensor fifo
 	sensor_gyro_fifo_s fifo{};
 
 	fifo.device_id = _device_id;
@@ -343,62 +193,6 @@ void PX4Gyroscope::updateFIFO(const FIFOSample &sample)
 
 	fifo.timestamp = hrt_absolute_time();
 	_sensor_fifo_pub.publish(fifo);
-
-
-	PublishStatus();
-}
-
-void PX4Gyroscope::PublishStatus()
-{
-	// 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_hz = _update_rate;
-		status.temperature = _temperature;
-		status.vibration_metric = _vibration_metric;
-		status.coning_vibration = _coning_vibration;
-		status.clipping[0] = _clipping_total[0];
-		status.clipping[1] = _clipping_total[1];
-		status.clipping[2] = _clipping_total[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;
-	_integration_raw.zero();
-	_integrator_clipping.zero();
-
-	_timestamp_sample_prev = 0;
-}
-
-void PX4Gyroscope::UpdateClipLimit()
-{
-	// 99.9% of potential max
-	_clip_limit = fmaxf((_range / _scale) * 0.999f, INT16_MAX);
-}
-
-void 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;
-	_vibration_metric = 0.99f * _vibration_metric + 0.01f * delta_angle_diff.norm();
-
-	// Gyro delta angle coning metric = filtered length of (delta_angle x prev_delta_angle)
-	const Vector3f coning_metric = delta_angle % _delta_angle_prev;
-	_coning_vibration = 0.99f * _coning_vibration + 0.01f * coning_metric.norm();
-
-	_delta_angle_prev = delta_angle;
 }
 
 void PX4Gyroscope::print_status()

src/lib/drivers/gyroscope/PX4Gyroscope.hpp

@@ -37,14 +37,11 @@
 #include <drivers/drv_hrt.h>
 #include <lib/cdev/CDev.hpp>
 #include <lib/conversion/rotation.h>
-#include <lib/drivers/device/integrator.h>
 #include <px4_platform_common/module_params.h>
 #include <uORB/PublicationMulti.hpp>
 #include <uORB/PublicationMulti.hpp>
 #include <uORB/topics/sensor_gyro.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
 {
@@ -62,10 +59,9 @@ public:
 	void set_device_type(uint8_t devtype);
 	void set_error_count(uint64_t error_count) { _error_count = error_count; }
 	void increase_error_count() { _error_count++; }
-	void set_range(float range) { _range = range; UpdateClipLimit(); }
+	void set_range(float range) { _range = range; }
-	void set_scale(float scale) { _scale = scale; UpdateClipLimit(); }
+	void set_scale(float scale) { _scale = scale; }
 	void set_temperature(float temperature) { _temperature = temperature; }
-	void set_update_rate(uint16_t rate);
 
 	void update(hrt_abstime timestamp_sample, float x, float y, float z);
 
@@ -87,27 +83,11 @@ public:
 	void updateFIFO(const FIFOSample &sample);
 
 private:
-
-	void PublishStatus();
-	void ResetIntegrator();
-	void UpdateClipLimit();
-	void UpdateVibrationMetrics(const matrix::Vector3f &delta_angle);
-
 	uORB::PublicationQueuedMulti<sensor_gyro_s>      _sensor_pub;
 	uORB::PublicationMulti<sensor_gyro_fifo_s>       _sensor_fifo_pub;
-	uORB::PublicationMulti<sensor_gyro_integrated_s> _sensor_integrated_pub;
-	uORB::PublicationMulti<sensor_gyro_status_s>     _sensor_status_pub;
-
-	hrt_abstime	_status_last_publish{0};
-
-	Integrator		_integrator{5000, true}; // 200 Hz default
 
 	matrix::Vector3f	_calibration_offset{0.f, 0.f, 0.f};
 
-	matrix::Vector3f _delta_angle_prev{0.f, 0.f, 0.f};	// delta angle from the previous IMU measurement
-	float _vibration_metric{0.f};	// high frequency vibration level in the IMU delta angle data (rad)
-	float _coning_vibration{0.f};	// Level of coning vibration in the IMU delta angles (rad^2)
-
 	int			_class_device_instance{-1};
 
 	uint32_t		_device_id{0};
@@ -117,25 +97,11 @@ private:
 	float			_scale{1.f};
 	float			_temperature{0.f};
 
-	int16_t			_clip_limit{(int16_t)(_range / _scale)};
+	uint32_t		_error_count{0};
 
-	uint64_t		_error_count{0};
-
-	uint32_t		_clipping_total[3] {};
-
-	uint16_t		_update_rate{1000};
-
-	// integrator
-	hrt_abstime		_timestamp_sample_prev{0};
-	matrix::Vector3f	_integration_raw{};
-	matrix::Vector3f	_integrator_clipping{};
 	int16_t			_last_sample[3] {};
-	uint8_t			_integrator_reset_samples{4};
-	uint8_t			_integrator_samples{0};
-	uint8_t			_integrator_fifo_samples{0};
 
 	DEFINE_PARAMETERS(
-		(ParamInt<px4::params::IMU_GYRO_RATEMAX>) _param_imu_gyro_rate_max,
+		(ParamInt<px4::params::IMU_GYRO_RATEMAX>) _param_imu_gyro_rate_max
-		(ParamInt<px4::params::IMU_INTEG_RATE>) _param_imu_integ_rate
 	)
 };

src/modules/ekf2/ekf2_main.cpp

@@ -772,9 +772,9 @@ void Ekf2::Run()
 		updated = _vehicle_imu_subs[_imu_sub_index].update(&imu);
 
 		imu_sample_new.time_us = imu.timestamp_sample;
-		imu_sample_new.delta_ang_dt = imu.dt * 1.e-6f;
+		imu_sample_new.delta_ang_dt = imu.delta_angle_dt * 1.e-6f;
 		imu_sample_new.delta_ang = Vector3f{imu.delta_angle};
-		imu_sample_new.delta_vel_dt = imu.dt * 1.e-6f;
+		imu_sample_new.delta_vel_dt = imu.delta_velocity_dt * 1.e-6f;
 		imu_sample_new.delta_vel = Vector3f{imu.delta_velocity};
 
 		if (imu.delta_velocity_clipping > 0) {
@@ -783,7 +783,7 @@ void Ekf2::Run()
 			imu_sample_new.delta_vel_clipping[2] = imu.delta_velocity_clipping & vehicle_imu_s::CLIPPING_Z;
 		}
 
-		imu_dt = imu.dt;
+		imu_dt = imu.delta_angle_dt;
 
 		bias.accel_device_id = imu.accel_device_id;
 		bias.gyro_device_id = imu.gyro_device_id;
@@ -853,12 +853,10 @@ void Ekf2::Run()
 
 					if (_imu_sub_index < 0) {
 						if (_sensor_selection.accel_device_id != sensor_selection_prev.accel_device_id) {
-							PX4_WARN("accel id changed, resetting IMU bias");
 							_imu_bias_reset_request = true;
 						}
 
 						if (_sensor_selection.gyro_device_id != sensor_selection_prev.gyro_device_id) {
-							PX4_WARN("gyro id changed, resetting IMU bias");
 							_imu_bias_reset_request = true;
 						}
 					}

src/modules/logger/logged_topics.cpp

@@ -110,12 +110,12 @@ void LoggedTopics::add_default_topics()
 	add_topic_multi("distance_sensor", 1000);
 	add_topic_multi("optical_flow", 1000);
 	add_topic_multi("sensor_accel", 1000);
-	add_topic_multi("sensor_accel_status", 1000);
 	add_topic_multi("sensor_baro", 1000);
 	add_topic_multi("sensor_gyro", 1000);
-	add_topic_multi("sensor_gyro_status", 1000);
 	add_topic_multi("sensor_mag", 1000);
 	add_topic_multi("vehicle_gps_position", 1000);
+	add_topic_multi("vehicle_imu", 500);
+	add_topic_multi("vehicle_imu_status", 1000);
 
 #ifdef CONFIG_ARCH_BOARD_PX4_SITL
 	add_topic("actuator_controls_virtual_fw");

src/modules/mavlink/mavlink_messages.cpp

@@ -84,12 +84,8 @@
 #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_baro.h>
 #include <uORB/topics/sensor_combined.h>
-#include <uORB/topics/sensor_gyro_integrated.h>
-#include <uORB/topics/sensor_gyro_status.h>
 #include <uORB/topics/sensor_mag.h>
 #include <uORB/topics/sensor_selection.h>
 #include <uORB/topics/tecs_status.h>
@@ -106,6 +102,8 @@
 #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_imu.h>
+#include <uORB/topics/vehicle_imu_status.h>
 #include <uORB/topics/vehicle_magnetometer.h>
 #include <uORB/topics/vehicle_odometry.h>
 #include <uORB/topics/vehicle_rates_setpoint.h>
@@ -1062,12 +1060,11 @@ public:
 
 	unsigned get_size() override
 	{
-		return _raw_accel_sub.advertised() ? (MAVLINK_MSG_ID_SCALED_IMU_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0;
+		return _raw_imu_sub.advertised() ? (MAVLINK_MSG_ID_SCALED_IMU_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0;
 	}
 
 private:
-	uORB::Subscription _raw_accel_sub{ORB_ID(sensor_accel_integrated), 0};
+	uORB::Subscription _raw_imu_sub{ORB_ID(vehicle_imu), 0};
-	uORB::Subscription _raw_gyro_sub{ORB_ID(sensor_gyro_integrated), 0};
 	uORB::Subscription _raw_mag_sub{ORB_ID(sensor_mag), 0};
 
 	// do not allow top copy this class
@@ -1080,28 +1077,25 @@ protected:
 
 	bool send(const hrt_abstime t) override
 	{
-		if (_raw_accel_sub.updated() || _raw_gyro_sub.updated() || _raw_mag_sub.updated()) {
+		if (_raw_imu_sub.updated() || _raw_mag_sub.updated()) {
 
-			sensor_accel_integrated_s sensor_accel{};
+			vehicle_imu_s imu{};
-			_raw_accel_sub.copy(&sensor_accel);
+			_raw_imu_sub.copy(&imu);
-
-			sensor_gyro_integrated_s sensor_gyro{};
-			_raw_gyro_sub.copy(&sensor_gyro);
 
 			sensor_mag_s sensor_mag{};
 			_raw_mag_sub.copy(&sensor_mag);
 
 			mavlink_scaled_imu_t msg{};
 
-			msg.time_boot_ms = sensor_accel.timestamp / 1000;
+			msg.time_boot_ms = imu.timestamp / 1000;
 
 			// Accelerometer in mG
-			const float accel_dt_inv = 1.e6f / (float)sensor_accel.dt;
+			const float accel_dt_inv = 1.e6f / (float)imu.delta_velocity_dt;
-			const Vector3f accel = Vector3f{sensor_accel.delta_velocity} * accel_dt_inv * 1000.0f / CONSTANTS_ONE_G;
+			const Vector3f accel = Vector3f{imu.delta_velocity} * accel_dt_inv * 1000.0f / CONSTANTS_ONE_G;
 
 			// Gyroscope in mrad/s
-			const float gyro_dt_inv = 1.e6f / (float)sensor_gyro.dt;
+			const float gyro_dt_inv = 1.e6f / (float)imu.delta_angle_dt;
-			const Vector3f gyro = Vector3f{sensor_gyro.delta_angle} * gyro_dt_inv * 1000.0f;
+			const Vector3f gyro = Vector3f{imu.delta_angle} * gyro_dt_inv * 1000.0f;
 
 			msg.xacc = (int16_t)accel(0);
 			msg.yacc = (int16_t)accel(1);
@@ -1152,12 +1146,11 @@ public:
 
 	unsigned get_size() override
 	{
-		return _raw_accel_sub.advertised() ? (MAVLINK_MSG_ID_SCALED_IMU2_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0;
+		return _raw_imu_sub.advertised() ? (MAVLINK_MSG_ID_SCALED_IMU2_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0;
 	}
 
 private:
-	uORB::Subscription _raw_accel_sub{ORB_ID(sensor_accel_integrated), 1};
+	uORB::Subscription _raw_imu_sub{ORB_ID(vehicle_imu), 1};
-	uORB::Subscription _raw_gyro_sub{ORB_ID(sensor_gyro_integrated), 1};
 	uORB::Subscription _raw_mag_sub{ORB_ID(sensor_mag), 1};
 
 	// do not allow top copy this class
@@ -1170,28 +1163,25 @@ protected:
 
 	bool send(const hrt_abstime t) override
 	{
-		if (_raw_accel_sub.updated() || _raw_gyro_sub.updated() || _raw_mag_sub.updated()) {
+		if (_raw_imu_sub.updated() || _raw_mag_sub.updated()) {
 
-			sensor_accel_integrated_s sensor_accel{};
+			vehicle_imu_s imu{};
-			_raw_accel_sub.copy(&sensor_accel);
+			_raw_imu_sub.copy(&imu);
-
-			sensor_gyro_integrated_s sensor_gyro{};
-			_raw_gyro_sub.copy(&sensor_gyro);
 
 			sensor_mag_s sensor_mag{};
 			_raw_mag_sub.copy(&sensor_mag);
 
 			mavlink_scaled_imu2_t msg{};
 
-			msg.time_boot_ms = sensor_accel.timestamp / 1000;
+			msg.time_boot_ms = imu.timestamp / 1000;
 
 			// Accelerometer in mG
-			const float accel_dt_inv = 1.e6f / (float)sensor_accel.dt;
+			const float accel_dt_inv = 1.e6f / (float)imu.delta_velocity_dt;
-			const Vector3f accel = Vector3f{sensor_accel.delta_velocity} * accel_dt_inv * 1000.0f / CONSTANTS_ONE_G;
+			const Vector3f accel = Vector3f{imu.delta_velocity} * accel_dt_inv * 1000.0f / CONSTANTS_ONE_G;
 
 			// Gyroscope in mrad/s
-			const float gyro_dt_inv = 1.e6f / (float)sensor_gyro.dt;
+			const float gyro_dt_inv = 1.e6f / (float)imu.delta_angle_dt;
-			const Vector3f gyro = Vector3f{sensor_gyro.delta_angle} * gyro_dt_inv * 1000.0f;
+			const Vector3f gyro = Vector3f{imu.delta_angle} * gyro_dt_inv * 1000.0f;
 
 			msg.xacc = (int16_t)accel(0);
 			msg.yacc = (int16_t)accel(1);
@@ -1241,12 +1231,11 @@ public:
 
 	unsigned get_size() override
 	{
-		return _raw_accel_sub.advertised() ? (MAVLINK_MSG_ID_SCALED_IMU3_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0;
+		return _raw_imu_sub.advertised() ? (MAVLINK_MSG_ID_SCALED_IMU3_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0;
 	}
 
 private:
-	uORB::Subscription _raw_accel_sub{ORB_ID(sensor_accel_integrated), 2};
+	uORB::Subscription _raw_imu_sub{ORB_ID(vehicle_imu), 2};
-	uORB::Subscription _raw_gyro_sub{ORB_ID(sensor_gyro_integrated), 2};
 	uORB::Subscription _raw_mag_sub{ORB_ID(sensor_mag), 2};
 
 	// do not allow top copy this class
@@ -1259,28 +1248,25 @@ protected:
 
 	bool send(const hrt_abstime t) override
 	{
-		if (_raw_accel_sub.updated() || _raw_gyro_sub.updated() || _raw_mag_sub.updated()) {
+		if (_raw_imu_sub.updated() || _raw_mag_sub.updated()) {
 
-			sensor_accel_integrated_s sensor_accel{};
+			vehicle_imu_s imu{};
-			_raw_accel_sub.copy(&sensor_accel);
+			_raw_imu_sub.copy(&imu);
-
-			sensor_gyro_integrated_s sensor_gyro{};
-			_raw_gyro_sub.copy(&sensor_gyro);
 
 			sensor_mag_s sensor_mag{};
 			_raw_mag_sub.copy(&sensor_mag);
 
 			mavlink_scaled_imu3_t msg{};
 
-			msg.time_boot_ms = sensor_accel.timestamp / 1000;
+			msg.time_boot_ms = imu.timestamp / 1000;
 
 			// Accelerometer in mG
-			const float accel_dt_inv = 1.e6f / (float)sensor_accel.dt;
+			const float accel_dt_inv = 1.e6f / (float)imu.delta_velocity_dt;
-			const Vector3f accel = Vector3f{sensor_accel.delta_velocity} * accel_dt_inv * 1000.0f / CONSTANTS_ONE_G;
+			const Vector3f accel = Vector3f{imu.delta_velocity} * accel_dt_inv * 1000.0f / CONSTANTS_ONE_G;
 
 			// Gyroscope in mrad/s
-			const float gyro_dt_inv = 1.e6f / (float)sensor_gyro.dt;
+			const float gyro_dt_inv = 1.e6f / (float)imu.delta_angle_dt;
-			const Vector3f gyro = Vector3f{sensor_gyro.delta_angle} * gyro_dt_inv * 1000.0f;
+			const Vector3f gyro = Vector3f{imu.delta_angle} * gyro_dt_inv * 1000.0f;
 
 			msg.xacc = (int16_t)accel(0);
 			msg.yacc = (int16_t)accel(1);
@@ -2817,13 +2803,7 @@ public:
 			return size;
 		}
 
-		for (auto &x : _sensor_accel_status_sub) {
+		for (auto &x : _vehicle_imu_status_sub) {
-			if (x.advertised()) {
-				return size;
-			}
-		}
-
-		for (auto &x : _sensor_gyro_status_sub) {
 			if (x.advertised()) {
 				return size;
 			}
@@ -2835,16 +2815,10 @@ public:
 private:
 	uORB::Subscription _sensor_selection_sub{ORB_ID(sensor_selection)};
 
-	uORB::Subscription _sensor_accel_status_sub[3] {
+	uORB::Subscription _vehicle_imu_status_sub[3] {
-		{ORB_ID(sensor_accel_status), 0},
+		{ORB_ID(vehicle_imu_status), 0},
-		{ORB_ID(sensor_accel_status), 1},
+		{ORB_ID(vehicle_imu_status), 1},
-		{ORB_ID(sensor_accel_status), 2},
+		{ORB_ID(vehicle_imu_status), 2},
-	};
-
-	uORB::Subscription _sensor_gyro_status_sub[3] {
-		{ORB_ID(sensor_gyro_status), 0},
-		{ORB_ID(sensor_gyro_status), 1},
-		{ORB_ID(sensor_gyro_status), 2},
 	};
 
 	/* do not allow top copying this class */
@@ -2859,10 +2833,10 @@ protected:
 	{
 		bool updated = _sensor_selection_sub.updated();
 
-		// check for sensor_accel_status update
+		// check for vehicle_imu_status update
 		if (!updated) {
 			for (int i = 0; i < 3; i++) {
-				if (_sensor_accel_status_sub[i].updated() || _sensor_gyro_status_sub[i].updated()) {
+				if (_vehicle_imu_status_sub[i].updated()) {
 					updated = true;
 					break;
 				}
@@ -2882,29 +2856,16 @@ protected:
 			sensor_selection_s sensor_selection{};
 			_sensor_selection_sub.copy(&sensor_selection);
 
-			// primary gyro coning and high frequency vibration metrics
-			if (sensor_selection.gyro_device_id != 0) {
-				for (auto &x : _sensor_gyro_status_sub) {
-					sensor_gyro_status_s status;
-
-					if (x.copy(&status)) {
-						if (status.device_id == sensor_selection.gyro_device_id) {
-							msg.vibration_x = status.coning_vibration;
-							msg.vibration_y = status.vibration_metric;
-							break;
-						}
-					}
-				}
-			}
-
 			// primary accel high frequency vibration metric
 			if (sensor_selection.accel_device_id != 0) {
-				for (auto &x : _sensor_accel_status_sub) {
+				for (auto &x : _vehicle_imu_status_sub) {
-					sensor_accel_status_s status;
+					vehicle_imu_status_s status;
 
 					if (x.copy(&status)) {
-						if (status.device_id == sensor_selection.accel_device_id) {
+						if (status.accel_device_id == sensor_selection.accel_device_id) {
-							msg.vibration_z = status.vibration_metric;
+							msg.vibration_x = status.gyro_coning_vibration;
+							msg.vibration_y = status.gyro_vibration_metric;
+							msg.vibration_z = status.accel_vibration_metric;
 							break;
 						}
 					}
@@ -2913,11 +2874,11 @@ protected:
 
 			// accel 0, 1, 2 cumulative clipping
 			for (int i = 0; i < 3; i++) {
-				sensor_accel_status_s acc_status;
+				vehicle_imu_status_s status;
 
-				if (_sensor_accel_status_sub[i].copy(&acc_status)) {
+				if (_vehicle_imu_status_sub[i].copy(&status)) {
 
-					const uint32_t clipping = acc_status.clipping[0] + acc_status.clipping[1] + acc_status.clipping[2];
+					const uint32_t clipping = status.accel_clipping[0] + status.accel_clipping[1] + status.accel_clipping[2];
 
 					switch (i) {
 					case 0:

src/modules/mavlink/mavlink_receiver.cpp

@@ -2188,15 +2188,8 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
 	/* gyro */
 	{
 		if (_px4_gyro == nullptr) {
-			// 2294028: DRV_GYR_DEVTYPE_GYROSIM, BUS: 1, ADDR: 2, TYPE: SIMULATION
+			// 1311244: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION
-			_px4_gyro = new PX4Gyroscope(2294028);
+			_px4_gyro = new PX4Gyroscope(1311244);
-
-			if (_px4_gyro == nullptr) {
-				PX4_ERR("PX4Gyroscope alloc failed");
-
-			} else {
-				_px4_gyro->set_update_rate(200); // TODO: measure actual
-			}
 		}
 
 		if (_px4_gyro != nullptr) {
@@ -2208,15 +2201,8 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
 	/* accelerometer */
 	{
 		if (_px4_accel == nullptr) {
-			// 1311244: DRV_ACC_DEVTYPE_ACCELSIM, BUS: 1, ADDR: 1, TYPE: SIMULATION
+			// 1311244: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION
 			_px4_accel = new PX4Accelerometer(1311244);
-
-			if (_px4_accel == nullptr) {
-				PX4_ERR("PX4Accelerometer alloc failed");
-
-			} else {
-				_px4_accel->set_update_rate(200); // TODO: measure actual
-			}
 		}
 
 		if (_px4_accel != nullptr) {
@@ -2230,10 +2216,6 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
 		if (_px4_mag == nullptr) {
 			// 197388: DRV_MAG_DEVTYPE_MAGSIM, BUS: 3, ADDR: 1, TYPE: SIMULATION
 			_px4_mag = new PX4Magnetometer(197388);
-
-			if (_px4_mag == nullptr) {
-				PX4_ERR("PX4Magnetometer alloc failed");
-			}
 		}
 
 		if (_px4_mag != nullptr) {
@@ -2247,10 +2229,6 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
 		if (_px4_baro == nullptr) {
 			// 6620172: DRV_BARO_DEVTYPE_BAROSIM, BUS: 1, ADDR: 4, TYPE: SIMULATION
 			_px4_baro = new PX4Barometer(6620172);
-
-			if (_px4_baro == nullptr) {
-				PX4_ERR("PX4Barometer alloc failed");
-			}
 		}
 
 		if (_px4_baro != nullptr) {
@@ -2629,7 +2607,7 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
 	/* accelerometer */
 	{
 		if (_px4_accel == nullptr) {
-			// 1311244: DRV_ACC_DEVTYPE_ACCELSIM, BUS: 1, ADDR: 1, TYPE: SIMULATION
+			// 1311244: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION
 			_px4_accel = new PX4Accelerometer(1311244);
 
 			if (_px4_accel == nullptr) {
@@ -2647,8 +2625,8 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
 	/* gyroscope */
 	{
 		if (_px4_gyro == nullptr) {
-			// 2294028: DRV_GYR_DEVTYPE_GYROSIM, BUS: 1, ADDR: 2, TYPE: SIMULATION
+			// 1311244: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION
-			_px4_gyro = new PX4Gyroscope(2294028);
+			_px4_gyro = new PX4Gyroscope(1311244);
 
 			if (_px4_gyro == nullptr) {
 				PX4_ERR("PX4Gyroscope alloc failed");