BMI088 connected via I2C does not work right[Bug]

[slgrobotics]

Describe the bug

I am having hard time making bmi088_i2c driver to work.

It is used only in CrazyFlie, other platforms use SPI version (which I don't have).
I use SeedStudio BMI088 breakboard, very basic, with Raspberry Pi 4 and modified emlid build.
Gyro sensor data comes with significant jitter (looks like spikes). Accel data looks normal.
The EKF orientation output, as seen in QGC, is slow to react and goes wild when sensor is moved even slightly.

The question is - is bmi088_i2c driver still supported/tested by the team? Would it be possible to verify that it works on CrazyFlie 2.1 hardware?

To Reproduce

Follow CrazyFlie build/flash procedure and see if copter orientation shows properly in QGroundControl. If it does - it is my problem, otherwise something is in the bmi088_i2c code.

Expected behavior

CrazyFlie copter orientation shows properly in QGroundControl

Screenshot / Media

No response

Flight Log

https://review.px4.io/plot_app?log=39964b55-f407-47e5-b828-56f814986e1c

Software Version

Current Main as of May 8, 2024

Flight controller

Raspberry Pi + SeedStudio BMI088 over I2C

Vehicle type

Rover

How are the different components wired up (including port information)

No response

Additional context

@mrpollo - related to Community call today

[slgrobotics]

Some observations:

1. PX4 produces messages (vehicle in rest):

WARN: [health_and_arming_checks] Preflight Fail: Gyro 0 inconsistent - check cal
WARN: [health_and_arming_checks] Preflight Fail: horizontal velocity unstable

2. Accelerometer "leveling" goes well, but attempt to calibrate Gyroscope produces a lot of failed attempts with "motion, retrying..." messages. It finally succeeds though.

3. The top-right heading indicator in QGC is rotating (drifting) clockwise slowly, indicating strong Gyro influence.

[slgrobotics]

I put some tracing in and here are my findings:

1. BMI088 Gyro frequently (~1-2 seconds) outputs INT16_MIN values ONLY on Z axis (likely indicating invalid data). This happens with both SeeedStudio boards I have.
2. There is code to react to this, but those frames aren't discarded as they should be in the bmi088_i2c code. Instead the code converts it to INT16_MAX - which causes the spikes.
3. The bmi088 (SPI version) of the code does discard those frames only if ALL X, Y and Z values are INT16_MIN, and thus all hardware with SPI-connected devices may be at risk:

main/src/drivers/imu/bosch/bmi088/BMI088_Gyroscope.cpp : 439 

		if (!(gyro_x == INT16_MIN && gyro_y == INT16_MIN && gyro_z == INT16_MIN)) {
			gyro.x[index] = gyro_x;
			gyro.y[index] = (gyro_y == INT16_MIN) ? INT16_MAX : -gyro_y;
			gyro.z[index] = (gyro_z == INT16_MIN) ? INT16_MAX : -gyro_z;
			++index;
		}

Maybe SPI connection is less prone to this behavior (which I doubt). Bugs - they are sooo buggy sometimes...

4. There's similar code on the accelerometer side, but I didn't detect any occurrences of INT16_MIN coming in.
5. There's also an obsolete "include board_dma_alloc.h" that causes compile error, still in gyro I2C code.

Basically, the BMI088_Gyroscope.cpp code on the bmi088_i2c side should look like this around line 500:

bool BMI088_Gyroscope::SimpleFIFORead(const hrt_abstime &timestamp_sample)
{
...
		if(xyz[0] == INT16_MIN || xyz[1] == INT16_MIN || xyz[2] == INT16_MIN) {
			//PX4_WARN("Gyro: INT16_MIN frame rejected");
			continue;
		}

		gyro.x[i] = xyz[0];
		gyro.y[i] = -xyz[1];
		gyro.z[i] = -xyz[2];
		gyro.samples++;
	}
...
}

I think that SPI side needs another pass due to questionable frame rejection logic. Not to mention that having separate SPI and I2C branches is waiting for trouble.

WARN  [bmi088_i2c] Gyro: INT16_MIN frame rejected
WARN  [health_and_arming_checks] Preflight Fail: Kill switch engaged
WARN  [bmi088_i2c] gyro.z == INT16_MIN
WARN  [bmi088_i2c] Gyro: INT16_MIN frame rejected
WARN  [bmi088_i2c] gyro.z == INT16_MIN
WARN  [bmi088_i2c] Gyro: INT16_MIN frame rejected
WARN  [bmi088_i2c] gyro.z == INT16_MIN
WARN  [bmi088_i2c] Gyro: INT16_MIN frame rejected
WARN  [bmi088_i2c] gyro.z == INT16_MIN
WARN  [bmi088_i2c] Gyro: INT16_MIN frame rejected
WARN  [bmi088_i2c] gyro.z == INT16_MIN
WARN  [bmi088_i2c] Gyro: INT16_MIN frame rejected

@mrpollo @dagar - please review. I can make a pull request, but I won't be able to test it on CrazyFlie or SPI hardware, only on my two rovers with Raspberry Pi 4 and SeeedStudio boards.
@muramura - FYI (frame rejection code commited on May 1, 2023)

[mrpollo]

Please make the PR @slgrobotics

[slgrobotics]

I'll make a PR, but for now with my code changes I am having strange discrepancy between the MPU9250/SPI and BMI088/I2C, working in parallel on the same rover (as sensor_gyro.00/01).

It's just a couple of turns, and MPU9250 in red shows rotations correctly. BMI088 in blue reflects the same rotation in short bursts - which probably indicates some data conversion issue or resets. Moreover, the same bursts appear on X and Y axes, which is totally wrong. Accelerometers don't seem to be out of sync, although both are picking a lot of vibrations. I'll look into it further.