IMU quaternion v4

[adrianmiriuta]

phase out rmat (use quaternion)

implemented static gyro noise integration lock trough standard deviation modulus
improved gyro Integration (PCDM Acta Mech 224, 3091–3109 (2013))
separated from Mahony PI
corrected acc
corrected gyro
increased default dcm_ki dcm_kp for faster convergence
removed ATTITUDE task rescheduling

[brucesdad13]

Sweet 😁 Have flown this a bit on your betaflight fork with my whoop (an older version) and loved it.

[adrianmiriuta]

@brucesdad13
thx !

it still needs beta testing.

[kidBrazil]

@adrianmiriuta Thanks for your contribution. We are in the process of reviewing it still.

Could you tell me a little more about how much testing has gone into this so far?

[adrianmiriuta]

@kidBrazil

it is indoor tested (working) on:
LUX_RACE
REVO
LUXV2_RACE
FF_FORTINIF4
OMNIBUSF4SD

Because of the manifold improvements in
Attitude, Gyro, Acc, Mag handling, ImuAHRS changes
it needs serious beta testing ! (from different testers)

It was submitted here about 2 months ago:
betaflight#5182
Then after long discussions it was put on hold.
Then it was eventually taken over here:
iNavFlight/inav#2894
the functions where name changed, any similarities rejected ... (and so on)
Now the devs from BF argue to drop my PR and port the copy.
betaflight#5182 (comment)

[kidBrazil]

@adrianmiriuta Ok, I will enlist some beta testers to do some thorough testing of your PR

[robert-b]

adrian,
have you checked the pi controller inside mahony?
how did you test this?
no overshoot?

[adrianmiriuta]

@robert-b

PI controller response with minimal overshoot.

set debug_mode = IMU

debug[0] = gyro absolute rotation speed
debug[1] = PI controller output
debug[2] = acc absoulute value
debug[3] = gyro static noise standard derivation (1 sigma)

tested with a non mounted LUX_RACE
convergence time after gyro overflow (> 2000°/s)

[kidBrazil]

@adrianmiriuta - We have been conducting some testing all day. Results are promising so far, no issues reported - some users reported better feel in the air but hard to say if its something quantifiable.

CPU usage seems largely unchanged for most users as well.

Will report back when testing is fully concluded.

[robert-b]

yes, that was my thinking.
the correction with mahony is 2nd order in comparison with the madgwick linear-gradient variant.
i would reject the pr only because the p value is WAY to high.
you have to reduce it!

for example flying through a turbulent zone and the controller goes crazy.
else it looks nice.

[adrianmiriuta]

@kidBrazil
cpu load is about
2% higher on F3
1% higher on F4, F7
in my tests

[adrianmiriuta]

@robert-b
I didn't noticed any oscillations in the PI output (in the logs),
but I only tested indoor (only single flips and rolls ).
If this shows as a problem in the testing ,
I can reduce the default dcm factors.

[robert-b]

adrian,
even light overshooting is bad.

[kidBrazil]

@robert-b @adrianmiriuta I had multiple users test this branch specifically today - They all tested with outdoor flying and I told them to really push the limits as much as they felt comfortable.

All have reported positive results, no one complained of issues with tune or feel. Most of them used Diff from previous build too so no retuning.

EDIT - Found a few more willing participants.. will update after they test as wsell.

[LexioTech]

I found some places where minor optimizations could be done or just code cleanup. If you have time could you fix those?

[LexioTech]

Could be just float modulus = quaternionModulus(q);

[LexioTech]

Could be just float norm = quaternionNorm(i)

[LexioTech]

return(sqrtf(quaternionNorm(q)));

[LexioTech]

We should skip the const floats and go for direct set:

o->w = l->w * r->w - l->x * r->x - l->y * r->y - l->z * r->z;
o->x = l->w * r->x + l->x * r->w + l->y * r->z - l->z * r->y;
o->y = l->w * r->y - l->x * r->z + l->y * r->w + l->z * r->x;
o->z = l->w * r->z + l->x * r->y - l->y * r->x + l->z * r->w;

Same thing, just more clear code.

[adrianmiriuta]

@LexioTech
thx.

[robert-b]

doesn't make much sense.
macro optimization - which is done by the compiler ...

[adrianmiriuta]

@robert-b
it's cleanup
it's good

[LexioTech]

@adrianmiriuta Thank you! 👍
@robert-b Anything that cleans up the code and removes duplications makes sense. And if for some reason a math equation that is used in 3 places (and not via in a function) needs to be changed, then you need to change it in 3 places... not just 1 like this.

Yes, the compiler is smart and will optimize this, but we also need to optimize the code for reading and coding friendliness.

[LexioTech]

Nice! Thank you for taking the time to do these minor cleanups 👍

[robert-b]

void quaternionNormalize(quaternion *q) { float modulus = sqrtf(q->w * q->w + q->x * q->x + q->y * q->y + q->z * q->z); if (modulus == 0) { modulus = 0.0000001; } q->w /= modulus; q->x /= modulus; q->y /= modulus; q->z /= modulus; }
you assign a double to a float ...
multiplications are faster than divisions - still remains true these days.

and above there is a medianfilter template.
on every line the WTF counter is incremented multiple times.

[adrianmiriuta]

@robert-b

you assign a double to a float ...

you mean ?
modulus = 0.0000001f;

multiplications are faster than divisions - still remains true these days.
and above there is a medianfilter template.
on every line the WTF counter is incremented multiple times.

sorry I don't understand what you mean.

[robert-b]

@LexioTech
above there is a medianfilter template.
on every line the WTF counter is incremented multiple times.
if you are consequent - then you need it to change.
not just mentioning minor issues ...

@adrianmiriuta
the 'new' assigment is correct.
and rewrite the division using multiplications.

but the re-viewers and you did not recognize that the quaternions aft the assigment 'explodes ...
in case of the assigment the quaternion has to be re-initialized:

// If quaternion has become inappropriately short or is nan reinit.
// THIS SHOULD NEVER ACTUALLY HAPPEN
if ((fabs(norm) < 1e-3) || (norm != norm))
{
     o->w = 1.0f;
     o->x = 0;
     o->y = 0;
     o->z = 0;
}
else
	...

my 2 cents on a code review.

[adrianmiriuta]

@robert-b

and rewrite the division using multiplications.

How would you do that ?

but the re-viewers and you did not recognize that the quaternions aft the assigment 'explodes ...
in case of the assigment the quaternion has to be re-initialized:

Now I understand.

this is only a zero division avoidance . (not my best implementation ...)

this function is also used to normalize vectors In a quaternion typeset storage.
quaternionNormalize(vAcc);
so your approach cannot be used

[robert-b]

hmm,
later on you need to work with the quaternion.
in your case the copter is def. dead.
// If quaternion has become inappropriately short or is nan reinit.
// THIS SHOULD NEVER ACTUALLY HAPPEN
if ((fabs(modulus) < 1e-3) || (modulus != modulus))
{
o->w = 1.0f;
o->x = 0;
o->y = 0;
o->z = 0;
}
else
{
modulus = 1.0f / modulus;
o->w *= modulus;
o->x *= modulus;
o->y *= modulus;
o->z *= modulus;
}
and pls. use this snippet as it is.
the quaternion HAS to be reset when the length is invalid!

[adrianmiriuta]

@robert-b

No !

I use quaternionNormalize() also on vectors in quaternion storage.
So your approach cannot be used.

If a unit quaternion is that degraded,
that means that you have a massive problem with the Implementation ! (that is not the case)

If you want to monitor that you should write a extra function to do that !
you are welcome to do that, if you feel like it.

[robert-b]

:-)

[kidBrazil]

@adrianmiriuta - We have decided to include this feature as a Release Candidate following our next release so that we can get even more testers on board.

One of the things that has been mentioned to me a couple of times is that this feature breaks Black Box Explorer functionality. I have not confirmed this myself but will look into it.

[adrianmiriuta]

@kidBrazil

Ok !
I will push some related cherry picks from BF also to the PR.

I used blackbox log pretty extensive to debug the code,
I didn't notice anything unusual.
Do you know what feature is broken ?

[kidBrazil]

@adrianmiriuta - I was sent this screenshot from the user who complained about BBE being broken. He says the LPF and notch frequencies are not showing up properly. Could be entirely unrelated to this branch so I will investigate as well.

EDIT -To get testing to work we took your branch, merged it with Master and the IMU 104 so we could test on HELIOSPRING as well. So something might have introduced the bug outside of your work. I will check to make sure. If you want to investigate as well I have a branch called TEST-imuquaternion-104-master

[adrianmiriuta]

@kidBrazil
attention I introduced a bug with commit 2258d06

@LexioTech
undone change request
#79 (comment)

this change is forbidden.
because you can use same quaternion as imput and output.
you need const w, x, y, z as buffer.

sorry I overlooked that.

[adrianmiriuta]

@kidBrazil

I am not able to replicate the BBE issue.
Can you please ask the user for a diff.

[kidBrazil]

@adrianmiriuta I was not able to replicate the issue either. And so far only 2 people piped up so might be nothing.

I will get a diff for you so we can go over it. We will be making an RC version with quaternions soon so we can get even more testing done. So far all feedback has been positive or "no change" which I believe is fine also.

[kidBrazil]

@adrianmiriuta I managed to get the BB log in question here and will paste the DIFF below

diff

version

ButterFlight / HELIOSPRING (HESP) 3.5.0 | IMUF: 1.0.4 Mar 28 2018 / 16:42:18 (235529c) MSP API: 1.39

name

name Hazak 6S

feature

feature -LED_STRIP
feature ESC_SENSOR
feature LEGACY_SA_SUPPORT

serial

serial 1 2048 115200 57600 0 115200
serial 2 64 115200 57600 0 115200
serial 3 1024 115200 57600 0 115200

aux

aux 0 0 0 1700 2100 0
aux 1 35 1 1700 2100 0

master

set acc_hardware = AUTO
set rssi_channel = 8
set rc_interp = MANUAL
set rc_interp_int = 12
set serialrx_provider = CRSF
set blackbox_p_ratio = 128
set dshot_idle_value = 300
set motor_pwm_protocol = PROSHOT1000
set bat_capacity = 1050
set vbat_scale = 107
set beeper_dshot_beacon_tone = 3
set yaw_motors_reversed = ON
set small_angle = 180
set yaw_deadband = 10
set osd_warnings = 63
set osd_cap_alarm = 800
set osd_vbat_pos = 2369
set osd_rssi_pos = 2105
set osd_tim_1_pos = 408
set osd_tim_2_pos = 2455
set osd_throttle_pos = 2433
set osd_vtx_channel_pos = 2424
set osd_current_pos = 2438
set osd_mah_drawn_pos = 2401
set osd_craft_name_pos = 2410
set osd_power_pos = 2446
set osd_warnings_pos = 2376
set osd_avg_cell_voltage_pos = 2337
set osd_esc_tmp_pos = 2080
set osd_esc_rpm_pos = 2048
set osd_stat_endbatt = ON
set osd_stat_tim_1 = ON
set debug_mode = GYRO
set vtx_band = 5
set vtx_freq = 5658
set vcd_video_system = 2
set camera_control_key_delay = 130

profile

profile 0

set dterm_lowpass = 90
set dterm_notch_hz = 0
set dterm_setpoint_weight = 100
set crash_setpoint_threshold = 500
set crash_recovery = ON
set p_pitch = 35
set d_pitch = 20
set p_roll = 35
set d_roll = 20

rateprofile

rateprofile 0

set roll_rc_rate = 170
set pitch_rc_rate = 170
set yaw_rc_rate = 185
set roll_expo = 12
set pitch_expo = 12
set yaw_expo = 6
set roll_srate = 45
set pitch_srate = 30
set yaw_srate = 0

HELIO 3_5.zip
BTTR_BLACKBOX_LOG_Quaternion_20180329_103220.zip

[adrianmiriuta]

@kidBrazil

We will be making an RC version with quaternions soon so we can get even more testing done.

Take your time, tough testing is a good thing.

So far all feedback has been positive or "no change" which I believe is fine also.

That's not very comforting !

From the "diff" i take most of your testers fly FPV ?
All changes in the PR are related to Attitude handling

1. no loss gyro integration
2. statistical noise removal
3. better acc gyro calibration
4. quick convergence after gyro over range.

Do you have testers who fly LOS ?
Bugs would affect HORIZON_MODE, ANGLE_MODE, HEADFREE_MODE,
essentialy all modes that rely on Attitude quaternion .

[adrianmiriuta]

@kidBrazil

I will get a diff for you so we can go over it.

I looked over the logs
It seems to be a BBL log Header parsing error

if(flightLog.getSysConfig().gyro_lowpass_hz!=null) drawMarkerLine(flightLog.getSysConfig().gyro_lowpass_hz, PLOTTED_BLACKBOX_RATE, 'GYRO LPF cutoff', WIDTH, HEIGHT, (15*offset++) + MARGIN, "rgba(128,255,128,0.50)");

but my javascript is too poor to deepen this.

[adrianmiriuta]

@kidBrazil
I see your testers fly with HELIOSPRING.
can you find out how high the static gyro drift "on the bench" is °/minute (with 32k/32k) ?
and the value of debug[4] with set debug_mode=IMU ?

[kidBrazil]

@adrianmiriuta So, looks like after some investigation this issue with BBE only affects HELIOSPRING - Ornery has disabled the notch filtering ENTIRELY on that target which is why it doesn't show up.. sorry I missed that completely and sorry if I wasted any of your time.

That said I will look into the parser as well for BBE just to make sure.

Unfortunately I would say that 99% of my test pilots are doing FPV flights at the moment, I should be able to find some people willing to test LOS. I will mention specifically that testers should try it in one of the other Attitude modes as well.

Thanks again for the collab.