WIP: drivers/imu/bosch output data changes

msg/sensor_accel.msg

@@ -7,6 +7,8 @@ float32 x                 # acceleration in the FRD board frame X-axis in m/s^2
 float32 y                 # acceleration in the FRD board frame Y-axis in m/s^2
 float32 z                 # acceleration in the FRD board frame Z-axis in m/s^2
 
+float32 range # TODO
+
 float32 temperature       # temperature in degrees Celsius
 
 uint32 error_count

msg/sensor_gyro.msg

@@ -7,6 +7,8 @@ float32 x                 # angular velocity in the FRD board frame X-axis in ra
 float32 y                 # angular velocity in the FRD board frame Y-axis in rad/s
 float32 z                 # angular velocity in the FRD board frame Z-axis in rad/s
 
+float32 range # TODO
+
 float32 temperature       # temperature in degrees Celsius
 
 uint32 error_count

src/drivers/imu/bosch/bmi055/BMI055_Accelerometer.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2022 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -42,13 +42,13 @@ namespace Bosch::BMI055::Accelerometer
 
 BMI055_Accelerometer::BMI055_Accelerometer(const I2CSPIDriverConfig &config) :
 	BMI055(config),
-	_px4_accel(get_device_id(), config.rotation)
+	_rotation(config.rotation)
 {
 	if (config.drdy_gpio != 0) {
 		_drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME"_accel: DRDY missed");
 	}
 
-	ConfigureSampleRate(_px4_accel.get_max_rate_hz());
+	ConfigureSampleRate(RATE);
 }
 
 BMI055_Accelerometer::~BMI055_Accelerometer()
@@ -202,16 +202,6 @@ void BMI055_Accelerometer::RunImpl()
 
 					uint8_t samples = fifo_frame_counter;
 
-					// tolerate minor jitter, leave sample to next iteration if behind by only 1
-					if (samples == _fifo_samples + 1) {
-						// sample timestamp set from data ready already corresponds to _fifo_samples
-						if (timestamp_sample == 0) {
-							timestamp_sample = now - static_cast<int>(FIFO_SAMPLE_DT);
-						}
-
-						samples--;
-					}
-
 					if (FIFORead((timestamp_sample == 0) ? now : timestamp_sample, samples)) {
 						success = true;
 
@@ -263,23 +253,23 @@ void BMI055_Accelerometer::ConfigureAccel()
 
 	switch (PMU_RANGE) {
 	case range_2g_set:
-		_px4_accel.set_scale(CONSTANTS_ONE_G / 1024.f); // 1024 LSB/g, 0.98mg/LSB
-		_px4_accel.set_range(2.f * CONSTANTS_ONE_G);
+		_accel_scale = CONSTANTS_ONE_G / 1024.f; // 1024 LSB/g, 0.98mg/LSB
+		_accel_range = 2.f * CONSTANTS_ONE_G;
 		break;
 
 	case range_4g_set:
-		_px4_accel.set_scale(CONSTANTS_ONE_G / 512.f); // 512 LSB/g, 1.95mg/LSB
-		_px4_accel.set_range(4.f * CONSTANTS_ONE_G);
+		_accel_scale = CONSTANTS_ONE_G / 512.f; // 512 LSB/g, 1.95mg/LSB
+		_accel_range = 4.f * CONSTANTS_ONE_G;
 		break;
 
 	case range_8g_set:
-		_px4_accel.set_scale(CONSTANTS_ONE_G / 256.f); // 256 LSB/g, 3.91mg/LSB
-		_px4_accel.set_range(8.f * CONSTANTS_ONE_G);
+		_accel_scale = CONSTANTS_ONE_G / 256.f; // 256 LSB/g, 3.91mg/LSB
+		_accel_range = 8.f * CONSTANTS_ONE_G;
 		break;
 
 	case range_16g_set:
-		_px4_accel.set_scale(CONSTANTS_ONE_G / 128.f); // 128 LSB/g, 7.81mg/LSB
-		_px4_accel.set_range(16.f * CONSTANTS_ONE_G);
+		_accel_scale = CONSTANTS_ONE_G / 128.f; // 128 LSB/g, 7.81mg/LSB
+		_accel_range = 16.f * CONSTANTS_ONE_G;
 		break;
 	}
 }
@@ -415,32 +405,51 @@ bool BMI055_Accelerometer::FIFORead(const hrt_abstime &timestamp_sample, uint8_t
 		return false;
 	}
 
-	sensor_accel_fifo_s accel{};
-	accel.timestamp_sample = timestamp_sample;
-	accel.samples = samples;
-	accel.dt = FIFO_SAMPLE_DT;
+	sensor_accel_s sensor_accel{};
+	sensor_accel.timestamp_sample = timestamp_sample;
+	sensor_accel.device_id = get_device_id();
+
+	float accel_sum[3] {};
 
 	for (int i = 0; i < samples; i++) {
 		const FIFO::DATA &fifo_sample = buffer.f[i];
 
 		// acc_x_msb<11:4> + acc_x_lsb<3:0>
-		const int16_t accel_x = combine(fifo_sample.ACCD_X_MSB, fifo_sample.ACCD_X_LSB) >> 4;
-		const int16_t accel_y = combine(fifo_sample.ACCD_Y_MSB, fifo_sample.ACCD_Y_LSB) >> 4;
-		const int16_t accel_z = combine(fifo_sample.ACCD_Z_MSB, fifo_sample.ACCD_Z_LSB) >> 4;
+		int16_t accel_x = combine(fifo_sample.ACCD_X_MSB, fifo_sample.ACCD_X_LSB) >> 4;
+		int16_t accel_y = combine(fifo_sample.ACCD_Y_MSB, fifo_sample.ACCD_Y_LSB) >> 4;
+		int16_t accel_z = combine(fifo_sample.ACCD_Z_MSB, fifo_sample.ACCD_Z_LSB) >> 4;
 
 		// sensor's frame is +x forward, +y left, +z up
 		//  flip y & z to publish right handed with z down (x forward, y right, z down)
-		accel.x[i] = accel_x;
-		accel.y[i] = (accel_y == INT16_MIN) ? INT16_MAX : -accel_y;
-		accel.z[i] = (accel_z == INT16_MIN) ? INT16_MAX : -accel_z;
+		// accel_x = accel_x;
+		accel_y = (accel_y == INT16_MIN) ? INT16_MAX : -accel_y;
+		accel_z = (accel_z == INT16_MIN) ? INT16_MAX : -accel_z;
+
+		rotate_3i(_rotation, accel_x, accel_y, accel_z);
+
+		accel_sum[0] += accel_x;
+		accel_sum[1] += accel_y;
+		accel_sum[2] += accel_z;
 	}
 
-	_px4_accel.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) +
-				   perf_event_count(_fifo_empty_perf) + perf_event_count(_fifo_overflow_perf));
+	if (samples > 0) {
+		sensor_accel.x = (accel_sum[0] / samples) * _accel_scale;
+		sensor_accel.y = (accel_sum[1] / samples) * _accel_scale;
+		sensor_accel.z = (accel_sum[2] / samples) * _accel_scale;
 
-	_px4_accel.updateFIFO(accel);
+		sensor_accel.range = _accel_range;
+		sensor_accel.samples = samples;
+		sensor_accel.temperature = _temperature;
+		sensor_accel.error_count = perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) +
+					   perf_event_count(_fifo_empty_perf) + perf_event_count(_fifo_overflow_perf);
 
-	return true;
+		sensor_accel.timestamp = hrt_absolute_time();
+		_sensor_accel_pub.publish(sensor_accel);
+
+		return true;
+	}
+
+	return false;
 }
 
 void BMI055_Accelerometer::FIFOReset()
@@ -472,7 +481,7 @@ void BMI055_Accelerometer::UpdateTemperature()
 	float temperature = static_cast<int8_t>(RegisterRead(Register::ACCD_TEMP)) * 0.5f + 23.f;
 
 	if (PX4_ISFINITE(temperature)) {
-		_px4_accel.set_temperature(temperature);
+		_temperature = temperature;
 
 	} else {
 		perf_count(_bad_transfer_perf);

src/drivers/imu/bosch/bmi055/BMI055_Accelerometer.hpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2022 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -35,10 +35,11 @@
 
 #include "BMI055.hpp"
 
-#include <lib/drivers/accelerometer/PX4Accelerometer.hpp>
-
 #include "Bosch_BMI055_Accelerometer_Registers.hpp"
 
+#include <uORB/PublicationMulti.hpp>
+#include <uORB/topics/sensor_accel.h>
+
 namespace Bosch::BMI055::Accelerometer
 {
 
@@ -58,7 +59,7 @@ private:
 	static constexpr uint32_t RATE{2000}; // 2000 Hz
 	static constexpr float FIFO_SAMPLE_DT{1e6f / RATE};
 
-	static constexpr int32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]))};
+	static constexpr int32_t FIFO_MAX_SAMPLES{FIFO::SIZE / sizeof(FIFO::DATA)};
 
 	// Transfer data
 	struct FIFOTransferBuffer {
@@ -97,7 +98,13 @@ private:
 
 	void UpdateTemperature();
 
-	PX4Accelerometer _px4_accel;
+	uORB::PublicationMulti<sensor_accel_s> _sensor_accel_pub{ORB_ID(sensor_accel)};
+
+	const enum Rotation _rotation;
+	float _accel_scale{0.f};
+	float _accel_range{0.f};
+
+	float _temperature{NAN};
 
 	perf_counter_t _bad_register_perf{perf_alloc(PC_COUNT, MODULE_NAME"_accel: bad register")};
 	perf_counter_t _bad_transfer_perf{perf_alloc(PC_COUNT, MODULE_NAME"_accel: bad transfer")};

src/drivers/imu/bosch/bmi055/BMI055_Gyroscope.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2022 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -33,8 +33,6 @@
 
 #include "BMI055_Gyroscope.hpp"
 
-#include <px4_platform/board_dma_alloc.h>
-
 using namespace time_literals;
 
 namespace Bosch::BMI055::Gyroscope
@@ -42,13 +40,13 @@ namespace Bosch::BMI055::Gyroscope
 
 BMI055_Gyroscope::BMI055_Gyroscope(const I2CSPIDriverConfig &config) :
 	BMI055(config),
-	_px4_gyro(get_device_id(), config.rotation)
+	_rotation(config.rotation)
 {
 	if (config.drdy_gpio != 0) {
 		_drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME"_gyro: DRDY missed");
 	}
 
-	ConfigureSampleRate(_px4_gyro.get_max_rate_hz());
+	ConfigureSampleRate(RATE);
 }
 
 BMI055_Gyroscope::~BMI055_Gyroscope()
@@ -202,16 +200,6 @@ void BMI055_Gyroscope::RunImpl()
 
 					uint8_t samples = fifo_frame_counter;
 
-					// tolerate minor jitter, leave sample to next iteration if behind by only 1
-					if (samples == _fifo_samples + 1) {
-						// sample timestamp set from data ready already corresponds to _fifo_samples
-						if (timestamp_sample == 0) {
-							timestamp_sample = now - static_cast<int>(FIFO_SAMPLE_DT);
-						}
-
-						samples--;
-					}
-
 					if (FIFORead((timestamp_sample == 0) ? now : timestamp_sample, samples)) {
 						success = true;
 
@@ -256,28 +244,28 @@ void BMI055_Gyroscope::ConfigureGyro()
 
 	switch (RANGE) {
 	case gyro_range_2000_dps:
-		_px4_gyro.set_scale(math::radians(1.f / 16.384f));
-		_px4_gyro.set_range(math::radians(2000.f));
+		_gyro_scale = math::radians(1.f / 16.384f);
+		_gyro_range = math::radians(2000.f);
 		break;
 
 	case gyro_range_1000_dps:
-		_px4_gyro.set_scale(math::radians(1.f / 32.768f));
-		_px4_gyro.set_range(math::radians(1000.f));
+		_gyro_scale = math::radians(1.f / 32.768f);
+		_gyro_range = math::radians(1000.f);
 		break;
 
 	case gyro_range_500_dps:
-		_px4_gyro.set_scale(math::radians(1.f / 65.536f));
-		_px4_gyro.set_range(math::radians(500.f));
+		_gyro_scale = math::radians(1.f / 65.536f);
+		_gyro_range = math::radians(500.f);
 		break;
 
 	case gyro_range_250_dps:
-		_px4_gyro.set_scale(math::radians(1.f / 131.072f));
-		_px4_gyro.set_range(math::radians(250.f));
+		_gyro_scale = math::radians(1.f / 131.072f);
+		_gyro_range = math::radians(250.f);
 		break;
 
 	case gyro_range_125_dps:
-		_px4_gyro.set_scale(math::radians(1.f / 262.144f));
-		_px4_gyro.set_range(math::radians(125.f));
+		_gyro_scale = math::radians(1.f / 262.144f);
+		_gyro_range = math::radians(125.f);
 		break;
 	}
 }
@@ -413,29 +401,44 @@ bool BMI055_Gyroscope::FIFORead(const hrt_abstime &timestamp_sample, uint8_t sam
 		return false;
 	}
 
-	sensor_gyro_fifo_s gyro{};
-	gyro.timestamp_sample = timestamp_sample;
-	gyro.samples = samples;
-	gyro.dt = FIFO_SAMPLE_DT;
+	sensor_gyro_s sensor_gyro{};
+	sensor_gyro.timestamp_sample = timestamp_sample;
+	sensor_gyro.device_id = get_device_id();
+
+	float gyro_sum[3] {};
 
 	for (int i = 0; i < samples; i++) {
 		const FIFO::DATA &fifo_sample = buffer.f[i];
 
-		const int16_t gyro_x = combine(fifo_sample.RATE_X_MSB, fifo_sample.RATE_X_LSB);
-		const int16_t gyro_y = combine(fifo_sample.RATE_Y_MSB, fifo_sample.RATE_Y_LSB);
-		const int16_t gyro_z = combine(fifo_sample.RATE_Z_MSB, fifo_sample.RATE_Z_LSB);
+		int16_t gyro_x = combine(fifo_sample.RATE_X_MSB, fifo_sample.RATE_X_LSB);
+		int16_t gyro_y = combine(fifo_sample.RATE_Y_MSB, fifo_sample.RATE_Y_LSB);
+		int16_t gyro_z = combine(fifo_sample.RATE_Z_MSB, fifo_sample.RATE_Z_LSB);
 
 		// sensor's frame is +x forward, +y left, +z up
 		//  flip y & z to publish right handed with z down (x forward, y right, z down)
-		gyro.x[i] = gyro_x;
-		gyro.y[i] = (gyro_y == INT16_MIN) ? INT16_MAX : -gyro_y;
-		gyro.z[i] = (gyro_z == INT16_MIN) ? INT16_MAX : -gyro_z;
+		// gyro_x = gyro_x;
+		gyro_y = (gyro_y == INT16_MIN) ? INT16_MAX : -gyro_y;
+		gyro_z = (gyro_z == INT16_MIN) ? INT16_MAX : -gyro_z;
+
+		rotate_3i(_rotation, gyro_x, gyro_y, gyro_z);
+
+		gyro_sum[0] += gyro_x * _gyro_scale;
+		gyro_sum[1] += gyro_y * _gyro_scale;
+		gyro_sum[2] += gyro_z * _gyro_scale;
 	}
 
-	_px4_gyro.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) +
-				  perf_event_count(_fifo_empty_perf) + perf_event_count(_fifo_overflow_perf));
+	sensor_gyro.x = gyro_sum[0] / samples;
+	sensor_gyro.y = gyro_sum[1] / samples;
+	sensor_gyro.z = gyro_sum[2] / samples;
 
-	_px4_gyro.updateFIFO(gyro);
+	sensor_gyro.range = _gyro_range;
+	sensor_gyro.samples = samples;
+	sensor_gyro.temperature = NAN;
+	sensor_gyro.error_count = perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) +
+				  perf_event_count(_fifo_empty_perf) + perf_event_count(_fifo_overflow_perf);
+
+	sensor_gyro.timestamp = hrt_absolute_time();
+	_sensor_gyro_pub.publish(sensor_gyro);
 
 	return true;
 }

src/drivers/imu/bosch/bmi055/BMI055_Gyroscope.hpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2022 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -35,10 +35,11 @@
 
 #include "BMI055.hpp"
 
-#include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
-
 #include "Bosch_BMI055_Gyroscope_Registers.hpp"
 
+#include <uORB/PublicationMulti.hpp>
+#include <uORB/topics/sensor_gyro.h>
+
 namespace Bosch::BMI055::Gyroscope
 {
 
@@ -58,7 +59,7 @@ private:
 	static constexpr uint32_t RATE{2000}; // 2000 Hz
 	static constexpr float FIFO_SAMPLE_DT{1e6f / RATE};
 
-	static constexpr int32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0]))};
+	static constexpr int32_t FIFO_MAX_SAMPLES{FIFO::SIZE / sizeof(FIFO::DATA)};
 
 	// Transfer data
 	struct FIFOTransferBuffer {
@@ -95,7 +96,11 @@ private:
 	bool FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples);
 	void FIFOReset();
 
-	PX4Gyroscope _px4_gyro;
+	uORB::PublicationMulti<sensor_gyro_s> _sensor_gyro_pub{ORB_ID(sensor_gyro)};
+
+	const enum Rotation _rotation;
+	float _gyro_scale{0.f};
+	float _gyro_range{0.f};
 
 	perf_counter_t _bad_register_perf{perf_alloc(PC_COUNT, MODULE_NAME"_gyro: bad register")};
 	perf_counter_t _bad_transfer_perf{perf_alloc(PC_COUNT, MODULE_NAME"_gyro: bad transfer")};

src/drivers/imu/bosch/bmi055/CMakeLists.txt

@@ -35,6 +35,7 @@ px4_add_module(
 	MODULE drivers__imu__bosch__bmi055
 	MAIN bmi055
 	COMPILE_FLAGS
+		${MAX_CUSTOM_OPT_LEVEL}
 	SRCS
 		Bosch_BMI055_Accelerometer_Registers.hpp
 		Bosch_BMI055_Gyroscope_Registers.hpp
@@ -48,7 +49,5 @@ px4_add_module(
 
 		bmi055_main.cpp
 	DEPENDS
-		drivers_accelerometer
-		drivers_gyroscope
 		px4_work_queue
 	)

src/drivers/imu/bosch/bmi088/BMI088_Accelerometer.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2022 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -42,13 +42,13 @@ namespace Bosch::BMI088::Accelerometer
 
 BMI088_Accelerometer::BMI088_Accelerometer(const I2CSPIDriverConfig &config) :
 	BMI088(config),
-	_px4_accel(get_device_id(), config.rotation)
+	_rotation(config.rotation)
 {
 	if (config.drdy_gpio != 0) {
 		_drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME"_accel: DRDY missed");
 	}
 
-	ConfigureSampleRate(_px4_accel.get_max_rate_hz());
+	ConfigureSampleRate(RATE);
 }
 
 BMI088_Accelerometer::~BMI088_Accelerometer()
@@ -218,12 +218,6 @@ void BMI088_Accelerometer::RunImpl()
 				} else {
 					samples = fifo_byte_counter / sizeof(FIFO::DATA);
 
-					// tolerate minor jitter, leave sample to next iteration if behind by only 1
-					if (samples == _fifo_samples + 1) {
-						timestamp_sample -= static_cast<int>(FIFO_SAMPLE_DT);
-						samples--;
-					}
-
 					if (samples > FIFO_MAX_SAMPLES) {
 						// not technically an overflow, but more samples than we expected or can publish
 						FIFOReset();
@@ -286,23 +280,23 @@ void BMI088_Accelerometer::ConfigureAccel()
 
 	switch (ACC_RANGE) {
 	case acc_range_3g:
-		_px4_accel.set_scale(CONSTANTS_ONE_G * (powf(2, ACC_RANGE + 1) * 1.5f) / 32768.f);
-		_px4_accel.set_range(3.f * CONSTANTS_ONE_G);
+		_accel_scale = CONSTANTS_ONE_G * (powf(2, ACC_RANGE + 1) * 1.5f) / 32768.f;
+		_accel_range = 3.f * CONSTANTS_ONE_G;
 		break;
 
 	case acc_range_6g:
-		_px4_accel.set_scale(CONSTANTS_ONE_G * (powf(2, ACC_RANGE + 1) * 1.5f) / 32768.f);
-		_px4_accel.set_range(6.f * CONSTANTS_ONE_G);
+		_accel_scale = CONSTANTS_ONE_G * (powf(2, ACC_RANGE + 1) * 1.5f) / 32768.f;
+		_accel_range = 6.f * CONSTANTS_ONE_G;
 		break;
 
 	case acc_range_12g:
-		_px4_accel.set_scale(CONSTANTS_ONE_G * (powf(2, ACC_RANGE + 1) * 1.5f) / 32768.f);
-		_px4_accel.set_range(12.f * CONSTANTS_ONE_G);
+		_accel_scale = CONSTANTS_ONE_G * (powf(2, ACC_RANGE + 1) * 1.5f) / 32768.f;
+		_accel_range = 12.f * CONSTANTS_ONE_G;
 		break;
 
 	case acc_range_24g:
-		_px4_accel.set_scale(CONSTANTS_ONE_G * (powf(2, ACC_RANGE + 1) * 1.5f) / 32768.f);
-		_px4_accel.set_range(24.f * CONSTANTS_ONE_G);
+		_accel_scale = CONSTANTS_ONE_G * (powf(2, ACC_RANGE + 1) * 1.5f) / 32768.f;
+		_accel_range = 24.f * CONSTANTS_ONE_G;
 		break;
 	}
 }
@@ -483,15 +477,17 @@ bool BMI088_Accelerometer::FIFORead(const hrt_abstime &timestamp_sample, uint8_t
 		return false;
 	}
 
-	sensor_accel_fifo_s accel{};
-	accel.timestamp_sample = timestamp_sample;
-	accel.samples = 0;
-	accel.dt = FIFO_SAMPLE_DT;
+	sensor_accel_s sensor_accel{};
+	sensor_accel.timestamp_sample = timestamp_sample;
+	sensor_accel.device_id = get_device_id();
 
 	// first find all sensor data frames in the buffer
 	uint8_t *data_buffer = (uint8_t *)&buffer.f[0];
 	unsigned fifo_buffer_index = 0; // start of buffer
 
+	float accel_sum[3] {};
+	int accel_samples = 0;
+
 	while (fifo_buffer_index < math::min(fifo_byte_counter, transfer_size - 4)) {
 		// look for header signature (first 6 bits) followed by two bits indicating the status of INT1 and INT2
 		switch (data_buffer[fifo_buffer_index] & 0xFC) {
@@ -500,16 +496,23 @@ bool BMI088_Accelerometer::FIFORead(const hrt_abstime &timestamp_sample, uint8_t
 				// Frame length: 7 bytes (1 byte header + 6 bytes payload)
 
 				FIFO::DATA *fifo_sample = (FIFO::DATA *)&data_buffer[fifo_buffer_index];
-				const int16_t accel_x = combine(fifo_sample->ACC_X_MSB, fifo_sample->ACC_X_LSB);
-				const int16_t accel_y = combine(fifo_sample->ACC_Y_MSB, fifo_sample->ACC_Y_LSB);
-				const int16_t accel_z = combine(fifo_sample->ACC_Z_MSB, fifo_sample->ACC_Z_LSB);
+				int16_t accel_x = combine(fifo_sample->ACC_X_MSB, fifo_sample->ACC_X_LSB);
+				int16_t accel_y = combine(fifo_sample->ACC_Y_MSB, fifo_sample->ACC_Y_LSB);
+				int16_t accel_z = combine(fifo_sample->ACC_Z_MSB, fifo_sample->ACC_Z_LSB);
 
 				// sensor's frame is +x forward, +y left, +z up
 				//  flip y & z to publish right handed with z down (x forward, y right, z down)
-				accel.x[accel.samples] = accel_x;
-				accel.y[accel.samples] = (accel_y == INT16_MIN) ? INT16_MAX : -accel_y;
-				accel.z[accel.samples] = (accel_z == INT16_MIN) ? INT16_MAX : -accel_z;
-				accel.samples++;
+				// accel_x = accel_x;
+				accel_y = (accel_y == INT16_MIN) ? INT16_MAX : -accel_y;
+				accel_z = (accel_z == INT16_MIN) ? INT16_MAX : -accel_z;
+
+				rotate_3i(_rotation, accel_x, accel_y, accel_z);
+
+				accel_sum[0] += accel_x;
+				accel_sum[1] += accel_y;
+				accel_sum[2] += accel_z;
+
+				accel_samples++;
 
 				fifo_buffer_index += 7; // move forward to next record
 			}
@@ -549,11 +552,20 @@ bool BMI088_Accelerometer::FIFORead(const hrt_abstime &timestamp_sample, uint8_t
 		}
 	}
 
-	_px4_accel.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) +
-				   perf_event_count(_fifo_empty_perf) + perf_event_count(_fifo_overflow_perf));
+	if (accel_samples > 0) {
+		sensor_accel.x = (accel_sum[0] / accel_samples) * _accel_scale;
+		sensor_accel.y = (accel_sum[1] / accel_samples) * _accel_scale;
+		sensor_accel.z = (accel_sum[2] / accel_samples) * _accel_scale;
+
+		sensor_accel.range = _accel_range;
+		sensor_accel.samples = accel_samples;
+		sensor_accel.temperature = _temperature;
+		sensor_accel.error_count = perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) +
+					   perf_event_count(_fifo_empty_perf) + perf_event_count(_fifo_overflow_perf);
+
+		sensor_accel.timestamp = hrt_absolute_time();
+		_sensor_accel_pub.publish(sensor_accel);
 
-	if (accel.samples > 0) {
-		_px4_accel.updateFIFO(accel);
 		return true;
 	}
 
@@ -600,7 +612,7 @@ void BMI088_Accelerometer::UpdateTemperature()
 	float temperature = (Temp_int11 * 0.125f) + 23.f; // Temp_int11 * 0.125°C/LSB + 23°C
 
 	if (PX4_ISFINITE(temperature)) {
-		_px4_accel.set_temperature(temperature);
+		_temperature = temperature;
 
 	} else {
 		perf_count(_bad_transfer_perf);

src/drivers/imu/bosch/bmi088/BMI088_Accelerometer.hpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2022 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -35,10 +35,11 @@
 
 #include "BMI088.hpp"
 
-#include <lib/drivers/accelerometer/PX4Accelerometer.hpp>
-
 #include "Bosch_BMI088_Accelerometer_Registers.hpp"
 
+#include <uORB/PublicationMulti.hpp>
+#include <uORB/topics/sensor_accel.h>
+
 namespace Bosch::BMI088::Accelerometer
 {
 
@@ -58,7 +59,7 @@ private:
 	static constexpr uint32_t RATE{1600}; // 1600 Hz
 	static constexpr float FIFO_SAMPLE_DT{1e6f / RATE};
 
-	static constexpr int32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]))};
+	static constexpr int32_t FIFO_MAX_SAMPLES{FIFO::SIZE / sizeof(FIFO::DATA)};
 
 	// Transfer data
 	struct FIFOTransferBuffer {
@@ -101,7 +102,13 @@ private:
 
 	void UpdateTemperature();
 
-	PX4Accelerometer _px4_accel;
+	uORB::PublicationMulti<sensor_accel_s> _sensor_accel_pub{ORB_ID(sensor_accel)};
+
+	const enum Rotation _rotation;
+	float _accel_scale{0.f};
+	float _accel_range{0.f};
+
+	float _temperature{NAN};
 
 	perf_counter_t _bad_register_perf{perf_alloc(PC_COUNT, MODULE_NAME"_accel: bad register")};
 	perf_counter_t _bad_transfer_perf{perf_alloc(PC_COUNT, MODULE_NAME"_accel: bad transfer")};

src/drivers/imu/bosch/bmi088/BMI088_Gyroscope.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2022 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -33,8 +33,6 @@
 
 #include "BMI088_Gyroscope.hpp"
 
-#include <px4_platform/board_dma_alloc.h>
-
 using namespace time_literals;
 
 namespace Bosch::BMI088::Gyroscope
@@ -42,13 +40,13 @@ namespace Bosch::BMI088::Gyroscope
 
 BMI088_Gyroscope::BMI088_Gyroscope(const I2CSPIDriverConfig &config) :
 	BMI088(config),
-	_px4_gyro(get_device_id(), config.rotation)
+	_rotation(config.rotation)
 {
 	if (config.drdy_gpio != 0) {
 		_drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME"_gyro: DRDY missed");
 	}
 
-	ConfigureSampleRate(_px4_gyro.get_max_rate_hz());
+	ConfigureSampleRate(RATE);
 }
 
 BMI088_Gyroscope::~BMI088_Gyroscope()
@@ -203,16 +201,6 @@ void BMI088_Gyroscope::RunImpl()
 
 					uint8_t samples = fifo_frame_counter;
 
-					// tolerate minor jitter, leave sample to next iteration if behind by only 1
-					if (samples == _fifo_samples + 1) {
-						// sample timestamp set from data ready already corresponds to _fifo_samples
-						if (timestamp_sample == 0) {
-							timestamp_sample = now - static_cast<int>(FIFO_SAMPLE_DT);
-						}
-
-						samples--;
-					}
-
 					if (FIFORead((timestamp_sample == 0) ? now : timestamp_sample, samples)) {
 						success = true;
 
@@ -257,28 +245,28 @@ void BMI088_Gyroscope::ConfigureGyro()
 
 	switch (GYRO_RANGE) {
 	case gyro_range_2000_dps:
-		_px4_gyro.set_scale(math::radians(1.f / 16.384f));
-		_px4_gyro.set_range(math::radians(2000.f));
+		_gyro_scale = math::radians(1.f / 16.384f);
+		_gyro_range = math::radians(2000.f);
 		break;
 
 	case gyro_range_1000_dps:
-		_px4_gyro.set_scale(math::radians(1.f / 32.768f));
-		_px4_gyro.set_range(math::radians(1000.f));
+		_gyro_scale = math::radians(1.f / 32.768f);
+		_gyro_range = math::radians(1000.f);
 		break;
 
 	case gyro_range_500_dps:
-		_px4_gyro.set_scale(math::radians(1.f / 65.536f));
-		_px4_gyro.set_range(math::radians(500.f));
+		_gyro_scale = math::radians(1.f / 65.536f);
+		_gyro_range = math::radians(500.f);
 		break;
 
 	case gyro_range_250_dps:
-		_px4_gyro.set_scale(math::radians(1.f / 131.072f));
-		_px4_gyro.set_range(math::radians(250.f));
+		_gyro_scale = math::radians(1.f / 131.072f);
+		_gyro_range = math::radians(250.f);
 		break;
 
 	case gyro_range_125_dps:
-		_px4_gyro.set_scale(math::radians(1.f / 262.144f));
-		_px4_gyro.set_range(math::radians(125.f));
+		_gyro_scale = math::radians(1.f / 262.144f);
+		_gyro_range = math::radians(125.f);
 		break;
 	}
 }
@@ -414,29 +402,44 @@ bool BMI088_Gyroscope::FIFORead(const hrt_abstime &timestamp_sample, uint8_t sam
 		return false;
 	}
 
-	sensor_gyro_fifo_s gyro{};
-	gyro.timestamp_sample = timestamp_sample;
-	gyro.samples = samples;
-	gyro.dt = FIFO_SAMPLE_DT;
+	sensor_gyro_s sensor_gyro{};
+	sensor_gyro.timestamp_sample = timestamp_sample;
+	sensor_gyro.device_id = get_device_id();
+
+	float gyro_sum[3] {};
 
 	for (int i = 0; i < samples; i++) {
 		const FIFO::DATA &fifo_sample = buffer.f[i];
 
-		const int16_t gyro_x = combine(fifo_sample.RATE_X_MSB, fifo_sample.RATE_X_LSB);
-		const int16_t gyro_y = combine(fifo_sample.RATE_Y_MSB, fifo_sample.RATE_Y_LSB);
-		const int16_t gyro_z = combine(fifo_sample.RATE_Z_MSB, fifo_sample.RATE_Z_LSB);
+		int16_t gyro_x = combine(fifo_sample.RATE_X_MSB, fifo_sample.RATE_X_LSB);
+		int16_t gyro_y = combine(fifo_sample.RATE_Y_MSB, fifo_sample.RATE_Y_LSB);
+		int16_t gyro_z = combine(fifo_sample.RATE_Z_MSB, fifo_sample.RATE_Z_LSB);
 
 		// sensor's frame is +x forward, +y left, +z up
 		//  flip y & z to publish right handed with z down (x forward, y right, z down)
-		gyro.x[i] = gyro_x;
-		gyro.y[i] = (gyro_y == INT16_MIN) ? INT16_MAX : -gyro_y;
-		gyro.z[i] = (gyro_z == INT16_MIN) ? INT16_MAX : -gyro_z;
+		// gyro_x = gyro_x;
+		gyro_y = (gyro_y == INT16_MIN) ? INT16_MAX : -gyro_y;
+		gyro_z = (gyro_z == INT16_MIN) ? INT16_MAX : -gyro_z;
+
+		rotate_3i(_rotation, gyro_x, gyro_y, gyro_z);
+
+		gyro_sum[0] += gyro_x * _gyro_scale;
+		gyro_sum[1] += gyro_y * _gyro_scale;
+		gyro_sum[2] += gyro_z * _gyro_scale;
 	}
 
-	_px4_gyro.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) +
-				  perf_event_count(_fifo_empty_perf) + perf_event_count(_fifo_overflow_perf));
+	sensor_gyro.x = gyro_sum[0] / samples;
+	sensor_gyro.y = gyro_sum[1] / samples;
+	sensor_gyro.z = gyro_sum[2] / samples;
 
-	_px4_gyro.updateFIFO(gyro);
+	sensor_gyro.range = _gyro_range;
+	sensor_gyro.samples = samples;
+	sensor_gyro.temperature = NAN;
+	sensor_gyro.error_count = perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) +
+				  perf_event_count(_fifo_empty_perf) + perf_event_count(_fifo_overflow_perf);
+
+	sensor_gyro.timestamp = hrt_absolute_time();
+	_sensor_gyro_pub.publish(sensor_gyro);
 
 	return true;
 }

src/drivers/imu/bosch/bmi088/BMI088_Gyroscope.hpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2022 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -35,10 +35,11 @@
 
 #include "BMI088.hpp"
 
-#include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
-
 #include "Bosch_BMI088_Gyroscope_Registers.hpp"
 
+#include <uORB/PublicationMulti.hpp>
+#include <uORB/topics/sensor_gyro.h>
+
 namespace Bosch::BMI088::Gyroscope
 {
 
@@ -58,7 +59,7 @@ private:
 	static constexpr uint32_t RATE{2000}; // 2000 Hz
 	static constexpr float FIFO_SAMPLE_DT{1e6f / RATE};
 
-	static constexpr int32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0]))};
+	static constexpr int32_t FIFO_MAX_SAMPLES{FIFO::SIZE / sizeof(FIFO::DATA)};
 
 	// Transfer data
 	struct FIFOTransferBuffer {
@@ -95,7 +96,11 @@ private:
 	bool FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples);
 	void FIFOReset();
 
-	PX4Gyroscope _px4_gyro;
+	uORB::PublicationMulti<sensor_gyro_s> _sensor_gyro_pub{ORB_ID(sensor_gyro)};
+
+	const enum Rotation _rotation;
+	float _gyro_scale{0.f};
+	float _gyro_range{0.f};
 
 	perf_counter_t _bad_register_perf{perf_alloc(PC_COUNT, MODULE_NAME"_gyro: bad register")};
 	perf_counter_t _bad_transfer_perf{perf_alloc(PC_COUNT, MODULE_NAME"_gyro: bad transfer")};

src/drivers/imu/bosch/bmi088/CMakeLists.txt

@@ -35,6 +35,7 @@ px4_add_module(
 	MODULE drivers__imu__bosch__bmi088
 	MAIN bmi088
 	COMPILE_FLAGS
+		${MAX_CUSTOM_OPT_LEVEL}
 	SRCS
 		Bosch_BMI088_Accelerometer_Registers.hpp
 		Bosch_BMI088_Gyroscope_Registers.hpp
@@ -48,7 +49,5 @@ px4_add_module(
 
 		bmi088_main.cpp
 	DEPENDS
-		drivers_accelerometer
-		drivers_gyroscope
 		px4_work_queue
 	)

src/drivers/imu/bosch/bmi088_i2c/BMI088_Accelerometer.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2022 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -41,13 +41,13 @@ namespace Bosch::BMI088::Accelerometer
 {
 BMI088_Accelerometer::BMI088_Accelerometer(const I2CSPIDriverConfig &config) :
 	BMI088(config),
-	_px4_accel(get_device_id(), config.rotation)
+	_rotation(config.rotation)
 {
 	if (config.drdy_gpio != 0) {
 		_drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME"_accel: DRDY missed");
 	}
 
-	ConfigureSampleRate(1600);
+	ConfigureSampleRate(RATE);
 }
 
 BMI088_Accelerometer::~BMI088_Accelerometer()
@@ -204,23 +204,23 @@ void BMI088_Accelerometer::ConfigureAccel()
 
 	switch (ACC_RANGE) {
 	case acc_range_3g:
-		_px4_accel.set_scale(CONSTANTS_ONE_G * (powf(2, ACC_RANGE + 1) * 1.5f) / 32768.f);
-		_px4_accel.set_range(3.f * CONSTANTS_ONE_G);
+		_accel_scale = CONSTANTS_ONE_G * (powf(2, ACC_RANGE + 1) * 1.5f) / 32768.f;
+		_accel_range = 3.f * CONSTANTS_ONE_G;
 		break;
 
 	case acc_range_6g:
-		_px4_accel.set_scale(CONSTANTS_ONE_G * (powf(2, ACC_RANGE + 1) * 1.5f) / 32768.f);
-		_px4_accel.set_range(6.f * CONSTANTS_ONE_G);
+		_accel_scale = CONSTANTS_ONE_G * (powf(2, ACC_RANGE + 1) * 1.5f) / 32768.f;
+		_accel_range = 6.f * CONSTANTS_ONE_G;
 		break;
 
 	case acc_range_12g:
-		_px4_accel.set_scale(CONSTANTS_ONE_G * (powf(2, ACC_RANGE + 1) * 1.5f) / 32768.f);
-		_px4_accel.set_range(12.f * CONSTANTS_ONE_G);
+		_accel_scale = CONSTANTS_ONE_G * (powf(2, ACC_RANGE + 1) * 1.5f) / 32768.f;
+		_accel_range = 12.f * CONSTANTS_ONE_G;
 		break;
 
 	case acc_range_24g:
-		_px4_accel.set_scale(CONSTANTS_ONE_G * (powf(2, ACC_RANGE + 1) * 1.5f) / 32768.f);
-		_px4_accel.set_range(24.f * CONSTANTS_ONE_G);
+		_accel_scale = CONSTANTS_ONE_G * (powf(2, ACC_RANGE + 1) * 1.5f) / 32768.f;
+		_accel_range = 24.f * CONSTANTS_ONE_G;
 		break;
 	}
 }
@@ -386,10 +386,6 @@ bool BMI088_Accelerometer::FIFORead(const hrt_abstime &timestamp_sample, uint8_t
 		return false;
 	}
 
-	sensor_accel_fifo_s accel{};
-	accel.timestamp_sample = timestamp_sample;
-	accel.samples = 0;
-	accel.dt = FIFO_SAMPLE_DT;
 
 	// first find all sensor data frames in the buffer
 	uint8_t *data_buffer = (uint8_t *)&buffer.f[0];

src/drivers/imu/bosch/bmi088_i2c/BMI088_Accelerometer.hpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2022 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -35,8 +35,6 @@
 
 #include "BMI088.hpp"
 
-#include <lib/drivers/accelerometer/PX4Accelerometer.hpp>
-
 #include "Bosch_BMI088_Accelerometer_Registers.hpp"
 
 namespace Bosch::BMI088::Accelerometer
@@ -59,7 +57,7 @@ private:
 	static constexpr uint32_t RATE{1600}; // 1600 Hz
 	static constexpr float FIFO_SAMPLE_DT{1e6f / RATE};
 
-	static constexpr int32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]))};
+	static constexpr int32_t FIFO_MAX_SAMPLES{FIFO::SIZE / sizeof(FIFO::DATA)};
 
 	// Transfer data
 	struct FIFOTransferBuffer {
@@ -114,8 +112,6 @@ private:
 	bool SimpleFIFORead(const hrt_abstime &timestamp_sample);
 	bool NormalRead(const hrt_abstime &timestamp_sample);
 
-	PX4Accelerometer _px4_accel;
-
 	perf_counter_t _bad_register_perf{perf_alloc(PC_COUNT, MODULE_NAME"_accel: bad register")};
 	perf_counter_t _bad_transfer_perf{perf_alloc(PC_COUNT, MODULE_NAME"_accel: bad transfer")};
 	perf_counter_t _fifo_empty_perf{perf_alloc(PC_COUNT, MODULE_NAME"_accel: FIFO empty")};

src/drivers/imu/bosch/bmi088_i2c/BMI088_Gyroscope.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2022 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -33,8 +33,6 @@
 
 #include "BMI088_Gyroscope.hpp"
 
-#include <px4_platform/board_dma_alloc.h>
-
 using namespace time_literals;
 
 namespace Bosch::BMI088::Gyroscope
@@ -42,7 +40,7 @@ namespace Bosch::BMI088::Gyroscope
 
 BMI088_Gyroscope::BMI088_Gyroscope(const I2CSPIDriverConfig &config) :
 	BMI088(config),
-	_px4_gyro(get_device_id(), config.rotation)
+	_rotation(config.rotation)
 {
 	if (config.drdy_gpio != 0) {
 		_drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME"_gyro: DRDY missed");

src/drivers/imu/bosch/bmi088_i2c/BMI088_Gyroscope.hpp

@@ -35,8 +35,6 @@
 
 #include "BMI088.hpp"
 
-#include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
-
 #include "Bosch_BMI088_Gyroscope_Registers.hpp"
 
 namespace Bosch::BMI088::Gyroscope
@@ -58,7 +56,7 @@ private:
 	static constexpr uint32_t RATE{400}; // 2000 Hz
 	static constexpr float FIFO_SAMPLE_DT{1e6f / RATE};
 
-	static constexpr int32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0]))};
+	static constexpr int32_t FIFO_MAX_SAMPLES{FIFO::SIZE / sizeof(FIFO::DATA)};
 
 	// Transfer data
 	struct FIFOTransferBuffer {
@@ -98,7 +96,6 @@ private:
 	bool SelfTest();
 	bool NormalRead(const hrt_abstime &timestamp_sample);
 	bool SimpleFIFORead(const hrt_abstime &timestamp_sample);
-	PX4Gyroscope _px4_gyro;
 
 	perf_counter_t _bad_register_perf{perf_alloc(PC_COUNT, MODULE_NAME"_gyro: bad register")};
 	perf_counter_t _bad_transfer_perf{perf_alloc(PC_COUNT, MODULE_NAME"_gyro: bad transfer")};

src/drivers/imu/bosch/bmi088_i2c/CMakeLists.txt

@@ -48,7 +48,5 @@ px4_add_module(
 
 		bmi088_i2c_main.cpp
 	DEPENDS
-		drivers_accelerometer
-		drivers_gyroscope
 		px4_work_queue
 	)