Refactor imuCalcKpGain

[ledvinap]

Rewrite of old code, shall be functionally identical. Timer wraparound bugs fixed.
Both original and new version do not match function comments, another change is necessary.

[github-actions]

Do you want to test this code? You can flash it directly from Betaflight Configurator:

• Simply put #12859 (this pull request number) in the Select commit field of the Configurator firmware flasher tab (you need to Enable expert mode, Show release candidates and Development).

WARNING: It may be unstable. Use only for testing!

[ledvinap]

@ctzsnooze : Refactored the code instead of trying to fix it. Can you test it, please?

[ledvinap]

Actually, there is change: low gain is used during quiet phase

[blckmn]

AUTOMERGE: (FAIL)

• github identifies PR as mergeable -> PASS
• assigned to a milestone -> PASS
• cooling off period lapsed -> PASS
• commit count less or equal to three -> PASS
• Don't merge label NOT found -> PASS
• at least one RN: label found -> PASS
• Tested label found -> FAIL
• assigned to an approver -> PASS
• approver count at least three -> PASS

[ctzsnooze]

does arState mean attitudeResetState ?

[ledvinap]

Yes. I don't like too long names when variable score is so small.. Maybe only 'state' will be less confusing

[ctzsnooze]

Thank you very much for this, it's a very nice re-factoring.

I'll test it independently of the GPS changes, with a specific debug to look at the returned dcmKp value, as soon as I can.

[ctzsnooze]

Hi @ledvinap

This is an image showing values during the boot phase, with dcm_kp at default of 2500, which should return an armed dcmKpGain value within of 0.25 imuMahonyAHRSupdate. Everything works perfectly!

The debug is logging dcmKpGain * 100 from within imuMahonyAHRSupdate into the channel shown in yellow up the top. Logging was set to 'always'; I arm about half-way across. The values written in yellow are the dcmKpGain values.

Initial dcmKpGain is 25 (shows as 2500 in the log); after 500ms it goes to 2.5, and stays there while disarmed, and on arming we get the expected value of 0.25.

This image shows a GPS Rescue failsafe initiation "too close to home", which results in an immediate disarm. There is the same brief 500ms period where dcmKpGain is 25 (100x normal), then back to the expected 'disarm' value of 2.5 (10x normal). In this particular situation, the quad could be falling due to being disarmed because the rescue was initiated too close to home. I don't know that it is so good to boost the IMU while falling.

This image shows a simple power-up, arm, disarm sequence. dcmKpGain follows the pattern as above when disarmed by GPS Rescue being too close.

On disarming, after being armed, the IMU gain goes up to 25 (100x normal) for 500ms, then stays at 2.5 (10x normal). If the quad is stationary on the ground, I guess this is good, but I don't know for sure that it's so good if the disarm was mid-air in the event of a crash.

This confirms that refactoring in this PR is working as intended, and that's excellent.

Now the only question is whether these various boosts and timings are appropriate. I personally lack enough understanding of the IMU code to answer that question. I imagine that a 100x boost above normal dcmKpGain immediately after a disarm, even if only for 500ms, could, if disarmed in the air, perhaps lead to some weirdness in level mode on re-arming that could take a while to resolve. However, as I say, I don't really understand this.

Also, for what it's worth, I typically plug my battery in while holding the quad in my hand (most of mine are battery-below designs). We would then get the 100x dcmKpGain boost during a period in which the quad is moving around a whole lot. However perhaps this gain boost happens only when the gyro signal indicates that the quad is still and the accelerometer is zeroed, I don't know.

Anyhow, I will approve the PR because the code works perfectly! Thank you!

[ctzsnooze]

@ledvinap this PR is ready to be merged, I think, unless you feel that the functional behaviour should be changed, and want to include those changes in this PR. If you have no strong feelings about making functional changes to the dcmKpGain behaviour then we can merge as is. We could look at functional changes later.

Please let us know what you think?