[UPDATE] - Code clean and format

src/drivers/imu/bosch/bmi088/bmi088_i2c/BMI088.cpp

@@ -79,6 +79,7 @@ int BMI088::init()
 		DEVICE_DEBUG("I2C::init failed (%i)", ret);
 		return ret;
 	}
+
 	int res = Reset() ? 0 : -1;
 	_state = STATE::SELFTEST;
 

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

@@ -39,7 +39,6 @@ using namespace time_literals;
 
 namespace Bosch::BMI088::Accelerometer
 {
-
 BMI088_Accelerometer::BMI088_Accelerometer(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation,
 		int bus_frequency, spi_mode_e spi_mode, spi_drdy_gpio_t drdy_gpio) :
 	BMI088(DRV_ACC_DEVTYPE_BMI088, "BMI088_Accelerometer", bus_option, bus, device, spi_mode, bus_frequency, drdy_gpio),
@@ -105,6 +104,7 @@ int BMI088_Accelerometer::probe()
 		DEVICE_DEBUG("unexpected ACC_CHIP_ID 0x%02x", ACC_CHIP_ID);
 		return PX4_ERROR;
 	}
+
 	PX4_WARN("Probe success, ACC_CHIP_ID: 0x%02x", ACC_CHIP_ID);
 
 	return PX4_OK;
@@ -192,8 +192,7 @@ void BMI088_Accelerometer::RunImpl()
 		break;
 
 	case STATE::FIFO_READ: {
-			FIFOReadTest(now);
-			//NormalRead(now);
+			SimpleFIFORead(now);
 		}
 		break;
 	}
@@ -382,6 +381,7 @@ bool BMI088_Accelerometer::FIFORead(const hrt_abstime &timestamp_sample, uint8_t
 		perf_count(_bad_transfer_perf);
 		return false;
 	}
+
 	//PX4_WARN("Accel transfer success");
 	const size_t fifo_byte_counter = combine(buffer.FIFO_LENGTH_1 & 0x3F, buffer.FIFO_LENGTH_0);
 
@@ -486,47 +486,103 @@ bool BMI088_Accelerometer::SimpleFIFORead(const hrt_abstime &timestamp_sample)
 	data_i[0] = static_cast<uint8_t>(Register::FIFO_LENGTH_0);
 
 	transfer(&data_i[0], 1, &data_o[0], 2);
-	fifo_fill_level = data_o[0] + 256 * data_o[1];
-	//PX4_WARN("fifo_fill_level %d", fifo_fill_level);
+	fifo_fill_level = data_o[0] + (data_o[1] << 8);
 
-	//uint8_t custom_size = fifo_fill_level * 7;
-	uint8_t custom_size = fifo_fill_level;
-	uint8_t buffer_data[custom_size] = {0};
-	uint8_t buffer[1] = {0};
-	buffer[0] = static_cast<uint8_t>(Register::FIFO_DATA);
-	if(fifo_fill_level > 0){
-		transfer(&buffer[0], 1, &buffer_data[0], custom_size);
-		struct FIFO::bmi08x_sensor_data bmi08x_accel;
+	if (fifo_fill_level & 0x8000) {
+		return false;
+	}
 
-		/* This is a super-simple FIFO parsing loop, hoping it will only find valid accel data packets */
-		for(int i = 1; i < custom_size;)
-		{
-			/* Header of acceleration sensor data frame: 100001xxb, where x is INT1/INT2 tag, ignored here */
-			if(buffer_data[i] == (0x84 & 0x8c))
-			{
-				UnpackSensorData(&bmi08x_accel, &buffer_data[i + 1]);
-				//PX4_WARN("Frame: %03d ax:%f ay:%f az:%f", i/6, (double)bmi08x_accel.x, (double)bmi08x_accel.y, (double)bmi08x_accel.z);
+	int n_frames_to_read = 6;
 
-				accel.x[accel.samples] = bmi08x_accel.x;
-				accel.y[accel.samples] = bmi08x_accel.y;
-				accel.z[accel.samples] = bmi08x_accel.z;
+	// don't read more than 6 frames at a time
+	if (fifo_fill_level > n_frames_to_read * 7) {
+		fifo_fill_level = n_frames_to_read * 7;
+	}
+
+	if (fifo_fill_level == 0) {
+		return false;
+	}
+
+	uint8_t data[fifo_fill_level];
+
+	data[0] = static_cast<uint8_t>(Register::FIFO_DATA);
+
+	if (transfer(&data[0], 1, &data[0], fifo_fill_level) != PX4_OK) {
+		return false;
+	}
+
+	const uint8_t *p = &data[0];
+
+	while (fifo_fill_level >= 7) {
+		uint8_t frame_len = 2;
+
+		switch (p[0] & 0xFC) {
+		case 0x84: {
+				// accel frame
+				frame_len = 7;
+				const uint8_t *d = p + 1;
+				int16_t xyz[3] {
+					int16_t(uint16_t(d[0] | (d[1] << 8))),
+					int16_t(uint16_t(d[2] | (d[3] << 8))),
+					int16_t(uint16_t(d[4] | (d[5] << 8)))
+				};
+
+
+				const int16_t tX[3] = {1, 0, 0};
+				const int16_t tY[3] = {0, -1, 0};
+				const int16_t tZ[3] = {0, 0, -1};
+
+				float x = 0;
+				float y = 0;
+				float z = 0;
+
+				x = xyz[0] * tX[0] + xyz[1] * tX[1] + xyz[2] * tX[2];
+				y = xyz[0] * tY[0] + xyz[1] * tY[1] + xyz[2] * tY[2];
+				z = xyz[0] * tZ[0] + xyz[1] * tZ[1] + xyz[2] * tZ[2];
+
+				accel.x[accel.samples] = x;
+				accel.y[accel.samples] = y;
+				accel.z[accel.samples] = z;
 				accel.samples++;
-				i += 7;
+
+				break;
 			}
-			else{
-				i++;
+
+		case 0x40:
+			// skip frame
+			frame_len = 2;
+			break;
+
+		case 0x44:
+			// sensortime frame
+			frame_len = 4;
+			break;
+
+		case 0x48:
+			// fifo config frame
+			frame_len = 2;
+			break;
+
+		case 0x50:
+			// sample drop frame
+			frame_len = 2;
+			break;
 		}
+
+		p += frame_len;
+		fifo_fill_level -= frame_len;
 	}
 
 	_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) {
+		//PX4_WARN("accel.samples: %d", accel.samples);
 		_px4_accel.updateFIFO(accel);
 		return true;
 	}
-	}
-	return false;
+
+	return true;
 }
 
 void BMI088_Accelerometer::FIFOReset()
@@ -576,18 +632,23 @@ void BMI088_Accelerometer::UpdateTemperature()
 	}
 }
 
-bool BMI088_Accelerometer::SelfTest() {
+bool BMI088_Accelerometer::SelfTest()
+{
 	PX4_WARN("Running self-test with datasheet recomended steps(page 17)");
 	// Reset
 	PX4_WARN("Reseting the sensor");
+
 	if (RegisterWrite(Register::ACC_SOFTRESET, 0xB6) == PX4_OK) {
 		PX4_WARN("Reset success");
 	}
+
 	usleep(100000);
 	PX4_WARN("Accel on");
+
 	if (RegisterWrite(Register::ACC_PWR_CTRL, 0x04) == PX4_OK) {
 		PX4_WARN("Accel on success");
 	}
+
 	usleep(100000);
 	PX4_WARN("Sensor ErrReg: 0x%02x", CheckSensorErrReg());
 	Configure();
@@ -597,10 +658,12 @@ bool BMI088_Accelerometer::SelfTest() {
 	PX4_WARN("ACC_CHIP_ID: 0x%02x", ACC_CHIP_ID);
 	usleep(30000);
 	PX4_WARN("Sensor ErrReg: 0x%02x", CheckSensorErrReg());
+
 	if (RegisterWrite(Register::ACC_PWR_CONF, 0) == PX4_OK) {
 		PX4_WARN("Start sensor success");
 		PX4_WARN("ACC_PWR_CONF(0): 0x%02x", RegisterRead(Register::ACC_PWR_CONF));
 	}
+
 	usleep(2000000);
 	PX4_WARN("Sensor ErrReg: 0x%02x", CheckSensorErrReg());
 
@@ -608,6 +671,7 @@ bool BMI088_Accelerometer::SelfTest() {
 		PX4_WARN("Range set success");
 		PX4_WARN("ACC_RANGE(0x03): 0x%02x", RegisterRead(Register::ACC_RANGE));
 	}
+
 	usleep(100000);
 	PX4_WARN("Sensor ErrReg: 0x%02x", CheckSensorErrReg());
 
@@ -615,6 +679,7 @@ bool BMI088_Accelerometer::SelfTest() {
 		PX4_WARN("Conf set success");
 		PX4_WARN("ACC_CONF(0xA7): 0x%02x", RegisterRead(Register::ACC_CONF));
 	}
+
 	usleep(100000);
 	PX4_WARN("Sensor ErrReg: 0x%02x", CheckSensorErrReg());
 
@@ -623,6 +688,7 @@ bool BMI088_Accelerometer::SelfTest() {
 		PX4_WARN("Self-test positive mode set success");
 		PX4_WARN("ACC_SELF_TEST(0x0D): 0x%02x", RegisterRead(Register::ACC_SELF_TEST));
 	}
+
 	usleep(100000);
 	PX4_WARN("Sensor ErrReg: 0x%02x", CheckSensorErrReg());
 
@@ -637,6 +703,7 @@ bool BMI088_Accelerometer::SelfTest() {
 		PX4_WARN("Self-test negative mode set success");
 		PX4_WARN("ACC_SELF_TEST(0x09): 0x%02x", RegisterRead(Register::ACC_SELF_TEST));
 	}
+
 	usleep(600000);
 	PX4_WARN("Sensor ErrReg: 0x%02x", CheckSensorErrReg());
 	float *accel_mss2 = ReadAccelDataFIFO();
@@ -659,9 +726,11 @@ bool BMI088_Accelerometer::SelfTest() {
 	if (diff_x >= 1000) {
 		PX4_WARN("X Axis self-test success");
 	}
+
 	if (diff_y >= 1000) {
 		PX4_WARN("Y Axis self-test success");
 	}
+
 	if (diff_z >= 500) {
 		PX4_WARN("Z Axis self-test success");
 	}
@@ -691,6 +760,7 @@ float * BMI088_Accelerometer::ReadAccelData()
 	if (transfer(&cmd[0], 1, buffer, sizeof(buf)) == PX4_OK) {
 		PX4_WARN("ReadAccelData transfer success");
 	}
+
 	for (uint8_t i = 0; i < sizeof(buf); i++) {
 		PX4_WARN("buf[%d]: %f", i, (double)buf[i]);
 	}
@@ -766,28 +836,30 @@ float * BMI088_Accelerometer::ReadAccelDataFIFO()
 	transfer(&buffer[0], 1, &buffer_data[0], custom_size);
 
 	/* This is a super-simple FIFO parsing loop, hoping it will only find valid accel data packets */
-	for(int i = 1; i < custom_size;)
-	{
+	for (int i = 1; i < custom_size;) {
 		/* Header of acceleration sensor data frame: 100001xxb, where x is INT1/INT2 tag, ignored here */
-		if(buffer_data[i] == (0x84 & 0x8c))
-		{
+		if (buffer_data[i] == (0x84 & 0x8c)) {
 			UnpackSensorData(&bmi08x_accel, &buffer_data[i + 1]);
-			PX4_WARN("Frame: %03d ax:%f ay:%f az:%f", i/6, (double)bmi08x_accel.x, (double)bmi08x_accel.y, (double)bmi08x_accel.z);
+			PX4_WARN("Frame: %03d ax:%f ay:%f az:%f", i / 6, (double)bmi08x_accel.x, (double)bmi08x_accel.y,
+				 (double)bmi08x_accel.z);
 			accel_mg[0] = bmi08x_accel.x;
 			accel_mg[1] = bmi08x_accel.y;
 			accel_mg[2] = bmi08x_accel.z;
 			float *data_in_mg = SensorDataTomg(accel_mg);
-			PX4_WARN("Frame mg: %03d ax:%f ay:%f az:%f", i/6, (double)data_in_mg[0], (double)data_in_mg[1], (double)data_in_mg[2]);
+			PX4_WARN("Frame mg: %03d ax:%f ay:%f az:%f", i / 6, (double)data_in_mg[0], (double)data_in_mg[1],
+				 (double)data_in_mg[2]);
 			i += 7;
-		}
-		else{
+
+		} else {
 			i++;
 		}
 	}
+
 	return accel_mg;
 }
 
-uint8_t  BMI088_Accelerometer::CheckSensorErrReg(){
+uint8_t  BMI088_Accelerometer::CheckSensorErrReg()
+{
 	return RegisterRead(Register::ACC_ERR_REG);
 }
 
@@ -818,7 +890,8 @@ float* BMI088_Accelerometer::SensorDataTomg(float* data)
 	return data;
 }
 
-bool BMI088_Accelerometer::NormalRead(const hrt_abstime &timestamp_sample) {
+bool BMI088_Accelerometer::NormalRead(const hrt_abstime &timestamp_sample)
+{
 	const int16_t tX[3] = {1, 0, 0};
 	const int16_t tY[3] = {0, -1, 0};
 	const int16_t tZ[3] = {0, 0, -1};
@@ -856,107 +929,4 @@ bool BMI088_Accelerometer::NormalRead(const hrt_abstime &timestamp_sample) {
 	return true;
 }
 
-bool BMI088_Accelerometer::FIFOReadTest(const hrt_abstime &timestamp_sample){
-	sensor_accel_fifo_s accel{};
-	accel.timestamp_sample = timestamp_sample;
-	accel.samples = 0;
-	accel.dt = FIFO_SAMPLE_DT;
-
-	int fifo_fill_level = 0;
-
-	uint8_t data_o[2] = { 0, 0 };
-	uint8_t data_i[1] = {static_cast<uint8_t>(Register::FIFO_LENGTH_0)};
-	data_i[0] = static_cast<uint8_t>(Register::FIFO_LENGTH_0);
-
-	transfer(&data_i[0], 1, &data_o[0], 2);
-	fifo_fill_level = data_o[0] + (data_o[1]<<8);
-	if (fifo_fill_level & 0x8000) {
-		//PX4_WARN("fifo_fill_level & 0x8000");
-		// empty
-		return false;
-	}
-	int n_frames_to_read = 6;
-	// don't read more than 8 frames at a time
-	if (fifo_fill_level > n_frames_to_read*7) {
-		fifo_fill_level = n_frames_to_read*7;
-	}
-
-	if (fifo_fill_level == 0) {
-		//PX4_WARN("fifo_fill_level == 0");
-		return false;
-	}
-
-    	uint8_t data[fifo_fill_level];
-	data[0] = static_cast<uint8_t>(Register::FIFO_DATA);
-	if(transfer(&data[0], 1, &data[0], fifo_fill_level) != PX4_OK) {
-		//PX4_WARN("transfer(&data[0], 1, &data[0], fifo_fill_level) != PX4_OK");
-		return false;
-	}
-	const uint8_t *p = &data[0];
-
-	while (fifo_fill_level >= 7) {
-		/*
-		the fifo frames are variable length, with the frame type in the first byte
-		*/
-		uint8_t frame_len = 2;
-		switch (p[0] & 0xFC) {
-			case 0x84: {
-				// accel frame
-				frame_len = 7;
-				const uint8_t *d = p+1;
-				int16_t xyz[3] {
-					int16_t(uint16_t(d[0] | (d[1]<<8))),
-					int16_t(uint16_t(d[2] | (d[3]<<8))),
-					int16_t(uint16_t(d[4] | (d[5]<<8)))};
-
-
-				const int16_t tX[3] = {1, 0, 0};
-				const int16_t tY[3] = {0, -1, 0};
-				const int16_t tZ[3] = {0, 0, -1};
-
-				float x = 0;
-				float y = 0;
-				float z = 0;
-
-				x = xyz[0] * tX[0] + xyz[1] * tX[1] + xyz[2] * tX[2];
-				y = xyz[0] * tY[0] + xyz[1] * tY[1] + xyz[2] * tY[2];
-				z = xyz[0] * tZ[0] + xyz[1] * tZ[1] + xyz[2] * tZ[2];
-
-				accel.x[accel.samples] = x;
-				accel.y[accel.samples] = y;
-				accel.z[accel.samples] = z;
-				accel.samples++;
-
-				break;
-			}
-			case 0x40:
-				// skip frame
-				frame_len = 2;
-				break;
-			case 0x44:
-				// sensortime frame
-				frame_len = 4;
-				break;
-			case 0x48:
-				// fifo config frame
-				frame_len = 2;
-				break;
-			case 0x50:
-				// sample drop frame
-				frame_len = 2;
-				break;
-		}
-		p += frame_len;
-		fifo_fill_level -= frame_len;
-	}
-	_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) {
-		//PX4_WARN("accel.samples: %d", accel.samples);
-		_px4_accel.updateFIFO(accel);
-		return true;
-	}
-	return true;
-}
 } // namespace Bosch::BMI088::Accelerometer

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

@@ -114,7 +114,6 @@ private:
 	uint8_t CheckSensorErrReg();
 	bool SimpleFIFORead(const hrt_abstime &timestamp_sample);
 	bool NormalRead(const hrt_abstime &timestamp_sample);
-	bool FIFOReadTest(const hrt_abstime &timestamp_sample);
 
 	PX4Accelerometer _px4_accel;
 

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

@@ -136,6 +136,7 @@ void BMI088_Gyroscope::RunImpl()
 		}
 
 		break;
+
 	case STATE::CONFIGURE:
 		if (Configure()) {
 			// if configure succeeded then start reading from FIFO
@@ -170,8 +171,7 @@ void BMI088_Gyroscope::RunImpl()
 		break;
 
 	case STATE::FIFO_READ: {
-			FIFOReadTest(now);
-			//NormalRead(now);
+			SimpleFIFORead(now);
 		}
 		break;
 	}
@@ -342,6 +342,7 @@ bool BMI088_Gyroscope::FIFORead(const hrt_abstime &timestamp_sample, uint8_t sam
 {
 	FIFOTransferBuffer buffer{};
 	const size_t transfer_size = math::min(samples * sizeof(FIFO::DATA) + 1, FIFO::SIZE);
+
 	//PX4_WARN("Estimated transfer size: %d", transfer_size);
 	if (transfer((uint8_t *)&buffer, 1, (uint8_t *)&buffer, transfer_size) != PX4_OK) {
 		perf_count(_bad_transfer_perf);
@@ -397,7 +398,8 @@ void BMI088_Gyroscope::FIFOReset()
 	}
 }
 
-bool BMI088_Gyroscope::SelfTest() {
+bool BMI088_Gyroscope::SelfTest()
+{
 	//Datasheet page 17 self test
 
 	//Set bit0 to enable built in self test
@@ -405,15 +407,19 @@ bool BMI088_Gyroscope::SelfTest() {
 	usleep(10000);
 	uint8_t res = 0;
 	uint8_t test_res = false;
+
 	while (true) {
 		res = RegisterRead(Register::SELF_TEST);
+
 		if ((res & 0x02) == 0x02) {
 			if ((res & 0x04) == 0x00) {
 				PX4_WARN("Gyro Self-test success");
 				test_res = true;
+
 			} else {
 				PX4_WARN("Gyro Self-test error");
 			}
+
 			break;
 		}
 	}
@@ -422,7 +428,8 @@ bool BMI088_Gyroscope::SelfTest() {
 	return test_res;
 }
 
-bool BMI088_Gyroscope::NormalRead(const hrt_abstime &timestamp_sample) {
+bool BMI088_Gyroscope::NormalRead(const hrt_abstime &timestamp_sample)
+{
 	float x = 0;
 	float y = 0;
 	float z = 0;
@@ -453,7 +460,7 @@ bool BMI088_Gyroscope::NormalRead(const hrt_abstime &timestamp_sample) {
 	return true;
 }
 
-bool BMI088_Gyroscope::FIFOReadTest(const hrt_abstime &timestamp_sample)
+bool BMI088_Gyroscope::SimpleFIFORead(const hrt_abstime &timestamp_sample)
 {
 	uint8_t n_frames;
 	sensor_gyro_fifo_s gyro{};
@@ -468,6 +475,7 @@ bool BMI088_Gyroscope::FIFOReadTest(const hrt_abstime &timestamp_sample)
 	n_frames &= 0x7F;
 
 	int n_frames_to_read = 6;
+
 	// don't read more than 8 frames at a time
 	if (n_frames > n_frames_to_read) {
 		n_frames = n_frames_to_read;
@@ -479,22 +487,26 @@ bool BMI088_Gyroscope::FIFOReadTest(const hrt_abstime &timestamp_sample)
 
 	uint8_t data[6 * n_frames];
 	data[0] = static_cast<uint8_t>(Register::FIFO_DATA);
+
 	if (transfer(&data[0], 1, &data[0], 6 * n_frames) != PX4_OK) {
 		//PX4_WARN("transfer(&data[0], 1, &data[0], fifo_fill_level) != PX4_OK");
 		return false;
 	}
+
 	for (uint8_t i = 0; i < n_frames; i++) {
 		const uint8_t *d = &data[i * 6];
 		int16_t xyz[3] {
 			int16_t(uint16_t(d[0] | d[1] << 8)),
 			int16_t(uint16_t(d[2] | d[3] << 8)),
-			int16_t(uint16_t(d[4] | d[5]<<8)) };
+			int16_t(uint16_t(d[4] | d[5] << 8))
+		};
 
 		gyro.x[i] = xyz[0];
 		gyro.y[i] = (xyz[1] == INT16_MIN) ? INT16_MAX : -xyz[1];
 		gyro.z[i] = (xyz[2] == INT16_MIN) ? INT16_MAX : -xyz[2];
 		gyro.samples++;
 	}
+
 	_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));
 
@@ -503,6 +515,7 @@ bool BMI088_Gyroscope::FIFOReadTest(const hrt_abstime &timestamp_sample)
 		_px4_gyro.updateFIFO(gyro);
 		return true;
 	}
+
 	return true;
 
 }

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

@@ -98,7 +98,7 @@ private:
 
 	bool SelfTest();
 	bool NormalRead(const hrt_abstime &timestamp_sample);
-	bool FIFOReadTest(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")};

src/drivers/imu/bosch/bmi088/bmi088_i2c/Bosch_BMI088_Accelerometer_Registers.hpp

@@ -187,8 +187,7 @@ enum header : uint8_t {
 	FIFO_input_config_frame = 0b01001000,
 	sample_drop_frame       = 0b01010000,
 };
-struct bmi08x_sensor_data
-{
+struct bmi08x_sensor_data {
 	/*! X-axis sensor data */
 	int16_t x;
 

src/drivers/imu/bosch/bmi088/bmi088_i2c/bmi088_i2c_main.cpp

@@ -79,10 +79,12 @@ extern "C" int bmi088_i2c_main(int argc, char *argv[])
 	}
 
 	const char *verb = cli.optarg();
+
 	if (!verb) {
 		ThisDriver::print_usage();
 		return -1;
 	}
+
 	BusInstanceIterator iterator(MODULE_NAME, cli, cli.type);
 
 	if (!strcmp(verb, "start")) {