Vertical speed with baro and mpu giving erroneous readouts

[hw999]

oXs Version = 2.12.9, downloaded this afternoon, hardware mpu6050 and ms5611. If I roll or pitch the testboard, roll and pitch readouts work as they should and return to zero when level. Vertical speed readout however is not as expected. If I pitch or roll +/- 90 degrees it goes to -0.5 -up to -0.8 m/s and stays at this value as long as there is roll or pitch. It returns to 0 m/s when level again. The testboard is not moved up or down during this test procedure.

[hw999]

I made some additional tests.

Roll and pitch values : for +/-90 degrees I expect +/- 0.5 rad. I get consistently around +/- 1.5 rad

For vertical speed : at power up more or less levelled :

pitch 0.08rad 0.02rad
roll 0.05rad then 1.51rad VS 0.0m/s -0.4m/s up to -0.8m/s

The vertical speed of -0.8m/s slowly goes back to 0m/s over a timespan of 15 up to 30 seconds when holding sensor in roll 1.5rad position. When returning to level position same procedure.

The sensor is never moved up or down, remains close to table.

[mstrens]

I am not sure there is really a mistake in the software. I noticed a similar issue with my GY86 sensor (mpu6050 +ms5611). The algorithm that calculates Vspeed uses the pressure (form ms5611) and the vertical acceleration when there is a mpu6050. The vertical acceleration is calculated based on the accelerations in the 3 axis (and the gyro). If the mp6050 is not perfectly calibrated, it provides different values for the vertical acceleration depending on the orientation and this even when the sensor is not moving at all.

The algorithm detects that situation and can compensated it but it requires a quite long time to react. In my case, I had an error of e.g. 0.4m/s when I rotate the sensor by 90° and it took about 1 min in that position to get back 0m/s (when sensor was not moving). If after 1 min, I move the sensor to level again, I get first an error of 0.4m/s that disappear after 1 min.

I expect that this issue can be avoided/reduced if the sensor is perfectly calibrated for accelerations Calibration is not easy. Each time I try the command mpucal=h, I get so different results because the sensor is not perfectly flat.

I am currently making some tests. When I set accOffset Z to 0, with sensor on level, I get a vertical acc of -60 or +60 when 180° reverse. When I change the offset to -1000, the same test, gives vertical acc equal to about 0.

[hw999]

Thanks for reply. When it comes to vertical acceleration I was so far of the opinion that calibration of the MPU does not have a big impact. I might of course be wrong on that. On another project based on code from prunkdump the vertical acceleration did not respond in any meaningfull way to changes in orientation. And I did not care about calibration at all. The response to accelaration however was fast and responsive to inputs. The amount of time it takes to recover surprises me. Could you please guide me to the file and line of code where I can get the calculated value of the acceleration in Z-axis ?

PS what about the strange values for roll and pitch ? If this gets fed into the calculation for z-speed I would not be surprised about anything. Just guessing - I did not look into the code so far. Long story short - are the values out of the 6050 properly scaled ? I am not a 6050 guy - but on a 9250 this mattered more for the filters (mahony or kalman did not matter) than any calibration.

[mstrens]

In mpu.cpp, there is a line with kalmanFilter4d_update( (float) vario1.rawRelAltitudeCm , (float) azWorldAverage /accScale1G 981.0 , (float) &zTrack , (float*)&vTrack);

You can add just after if (msgEverySec(0)) printf("az=%i\n", (int) ((float) azWorldAverage /accScale1G * 981.0 )); It will print once every sec the vertical acceleration (without the gravity) for any orientation. It is this value that is used afterward to calculate the vspeed.

[hw999]

Please have a look at my previous post. Added some text.

[mstrens]

Can you further explain the issue you found with pitch and roll. I did not understand what you said.

Pitch and roll are not used for Vspeed calculation. But vertical acceleration (without gravity) is used. The orientation of the sensor play a big role in the way vertical acceleration is calculated. When sensor do not move up or down, vertical acceleration should be 0 (or close to 0).

I can't exclude an error in the scale. In fact the code was first written only for calculating the Vspeed. Later on I added code to manage a stabilization algorithm. I made some changes to the scale factors. I hope the changes where consistent but it is not bad to check it again.

[hw999]

As mentionned above : Roll and pitch values : for +/-90 degrees I expect +/- 0.5 rad. I get consistently around +/- 1.5 rad I just get these values on a Radiomaster TX16s with edgetx 2.9.2

What is transmitted to the RX and why ? CRSF protocol expects : UAS Attitude (0x1E)

Telemetry Item

int16_t pitch angle in radians/10000
    e.g. 180 degrees sent as 0x7AB7 / 31415
    e.g. 45 degrees sent as 0x1EAD / 7853
    e.g. -45 degrees sent as 0xE153 / -7853
int16_t roll
int16_t yaw

All values must be in the +/-180 degree +/-PI radian range. https://github.com/crsf-wg/crsf/wiki

It's many years since I developped my own stuff and I have to work into it again. Could you have a look at the acc values ? Looks to me like there is maybe a problem there. Another question : how often is the Kalman-filter updated ?

PS quote : "Pitch and roll are not used for Vspeed calculation. But vertical acceleration (without gravity) is used." Question : how is pitch and roll and in the end v-acc calculated ? If I remember correctly its based mainly on acc. Just an idea based on my observations of roll and pitch : the acc values you use are way to high and not properly scaled. If this should be the case it would explain why the Kalman filter is freaking out. The update rate put aside for the moment.

[mstrens]

In oXs roll and pitch are stored in degree (common for all protocols) They are converted to different values/units when the frames are generated to the RX depending in the protocol. For ELRS (=crsf), oXs is supposed to convert from degree to 1/10000 rad. Still I found an error: There is (in crsf_out.cpp) fillBufferI16( (int16_t) ( (fields[PITCH].value 175) / 100) ) ; //pitch (must be in 1/1000 of deci rad ) //} else { // fillBufferI16( (int16_t) 0); //} //if ( fields[ROLL].available ) { fillBufferI16( (int16_t) ( (fields[ROLL].value 175) / 100) ) ; //roll

There is no need to divide by 100. e.g. 180 *175 = 31500 (so nearly 31415)

So code should be fillBufferI16( (int16_t) ( (fields[PITCH].value 175) ) ) ; //pitch (must be in 1/1000 of deci rad ) //} else { // fillBufferI16( (int16_t) 0); //} //if ( fields[ROLL].available ) { fillBufferI16( (int16_t) ( (fields[ROLL].value 175) ) ) ; //roll

To calculate roll/pitch and vertical acc, oXs uses (in mpu.cpp) bool MPU::getAccZWorld(){ // return true when a value is available ; read the IMU and calculate the acc on Z axis (world)

This function use a Mahony algorithm Mahony_update( (float) oax, (float) oay, (float) oaz , ((float) ogx) gyroScaleRad, ((float) ogy) gyroScaleRad , ((float) ogz) * gyroScaleRad, deltat); And then, based on quaternion, it calculates gravity vector, roll, pitch,...

[hw999]

Please let me know when changes are in the test-branch. Looking forward to test.

Mahony_update( (float) oax, (float) oay, (float) oaz , ((float) ogx) gyroScaleRad, ((float) ogy) gyroScaleRad , ((float) ogz) * gyroScaleRad, deltat); This call sounds somewhat familiar to me. Remember well how I spent some hours struggling to make the call correctly. Have the authors of this code clearly defined what input they expect and if so is this done accordingly ? How often is the filter updated ? Just asking because I would like to have a clearer picture.

[hw999]

I hope this question is not too stupid : for what reason do you use a Mahony-algorithm AND a Kalman-algorithm ? To my knowledge they do more or less the same in different ways. I just noticed that both of them are present in oXs.

[mstrens]

First, I just noticed that the code was OK in the conversion from degree to rad for ELRS. In fact roll and pitch are internally in 1/100 of degree (previously in degree but I changed to 1/100 on request of a user and doc was not changed) . So it was correct to take the value in 1/100 of degree and 175 /100 (18000175/100 = 31500). Note: you can see the internal values of oXs using the FV command on the Pc keyboard. For pitch and roll, this command FV takes the internal value (integer in 1/100 of degree) and divide it by 100 while printing. So the result is indeed in a range -180/+180. So I do not understand the strange values you get on the handset.

Mahony and kalman are 2 different fusion algorithms. Mahony merges acceleration and gyro data to give the orientation of the sensor. Kalman (the one I use) merges altitude (from baro) and vertical acceleration (from mpu after Mahony).

[hw999]

Wow. I was unaware of the FV-command. Some samples :

[16:31:37:904] Vspeed = 0 cm/s␍␊ [16:31:37:904] Baro Rel altitude = 48 cm␍␊ [16:31:37:904] Pitch = 30.000000 degree␍␊ [16:31:37:904] Roll = 28.000000 degree␍␊ [16:31:37:904] Acc X = 0.534000g␍␊ [16:31:37:904] Acc Y = 0.451000g␍␊ [16:31:37:904] Acc Z = 0.790000g␍␊ [16:31:37:904] pwmTop= 20000␍␊

Handset : Pitch 0.50 rad Roll 0.49 rad

[16:36:15:419] Vspeed = -36 cm/s␍␊ [16:36:15:419] Baro Rel altitude = 19 cm␍␊ [16:36:15:419] Pitch = -15.000000 degree␍␊ [16:36:15:419] Roll = 110.000000 degree␍␊ [16:36:15:419] Acc X = -0.277000g␍␊ [16:36:15:419] Acc Y = 0.881000g␍␊ [16:36:15:425] Acc Z = -0.350000g␍␊ [16:36:15:425] pwmTop= 20000␍␊

Handset : Pitch -0.26rad Roll 1.92 rad

---

[16:38:23:015] Vspeed = 21 cm/s␍␊ [16:38:23:015] Baro Rel altitude = 25 cm␍␊ [16:38:23:015] Pitch = 71.000000 degree␍␊ [16:38:23:015] Roll = 11.000000 degree␍␊ [16:38:23:015] Acc X = 0.972000g␍␊ [16:38:23:020] Acc Y = 0.075000g␍␊ [16:38:23:020] Acc Z = 0.309000g␍␊ [16:38:23:020] pwmTop= 20000␍␊

Handset : Pitch 1.24rad Roll 0.19 rad

All measurements in steady state after moving. FW uploaded yesterday without any changes except I2C adresses. I know that if I send a value of 1.00 for roll that it shows as roll 1.00 on the handset. I use roll, pitch and yaw for voltages in my Sport fork of oxs (ESC, RB10). If you are interested let me know.

Are there other hidden but usefull commands ? Where can I look them up ?

[mstrens]

So, Roll and pitch are OK. When you send "?" as command, you get nearly all commands (including FV , FVP, FVN). There are a few more that I use just for debugging (to start/stop sending some messages to the PC). E.g. debugtlm=Y or N , Not sure that they work very well. It was just for me.

I just put on github a version 2.12.10 that allows:

• to set up manually the offsets values for the accelerations (X,Y,Z): AOX=xxxxx, AOY=yyyy or AOZ=yyyyy.
• to display (or not) the vertical acceleration: DEBUGACCZ=Y or N. Yo can try to change your offsets in such a way that the vertical accelerations stay close to 0 when the mpu do not move but is the orientation changed by 180/90°

[hw999]

Roll and pitch are ok with the FV-command - but look at the values for VS with FV and the values for roll and pitch on the handset - they don't match. Look at sample 1 : pitch 30 degrees with FV - 0.5rad or 90 degrees on the handset.

DEBUGACCZ=Y or N. will try it out. Thanks.

[mstrens]

oXs send in rad in ELRS protocol. 30° is equal to 0.5rad ( by definition 3.14 rad = 180°).

If you get the right value in rad on the handset, then oXs is for sure OK. Could it be that the conversion from rad to deg is wrong on the handset?

[hw999]

oXs send in rad in ELRS protocol. 30° is equal to 0.5rad ( by definition 3.14 rad = 180°).

If you get the right value in rad on the handset, then oXs is for sure OK. Could it be that the conversion from rad to deg is wrong on the handset?

I am humbled. The conversion in my head was wrong.

Mahony is responsible for z-acceleration - right ? Have you done some tuning on this one ? How often is it updated per second ?