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: https://github.com/betaflight/betaflight/pull/5182 Then after long discussions it was put on hold. Then it was eventually taken over here: https://github.com/iNavFlight/inav/pull/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. https://github.com/betaflight/betaflight/pull/5182#issuecomment-374572410

[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.

[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.

[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 2258d065175bc9f9282ccef3fe5bb854403ec500

@LexioTech undone change request https://github.com/ButterFlight/butterflight/pull/79#discussion_r177906818

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 (235529cfd) 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.