WIP: Thrust imbalance detection with OSD warning

[vmzhivetyev]

This pull request introduces thrust imbalance detection to provide early warnings for potential issues such as loose or damaged props and motors.

The motivation behind this comes from personal experience. A loose prop can likely lead to a burned motor and/or a nasty crash. I had a flight this winter in the mountains (where you absolutely don't want to crash) where a prop came loose mid flight and it was unnoticeable. Then suddenly a motor went up in smoke and the quad dropped from the sky. Looking at "motor diagnostics" in the DVR recording later it is easily noticeable that the burned motor was struggling to produce thrust. Tho I assume that the cold conditions were most probably the cause of the prop getting loose mid flight as plastic and metal expand/shrink differently as a result of changes in temperature (motor did heat up after takeoff while the prop didn't).

So even when all the preflight checks have been done there is always a probability of such things happening after the takeoff.

I'm eager to hear your thoughts and suggestions on this pull request as this is my first contribution to the BF.

Approach

After thorough analysis of test flights blackbox logs, I came up with a detection method based on absolute sum multiplication of all I terms. Surprisingly such a simple approach provides a comprehensive measure of the quadcopter's thrust problems. When a motor struggles, whether due to a loose prop or another issue, it disrupts balance across all axes, thus leading to coupled buildup of I term in yaw, pitch, and roll. It should also work with non-quad configuration since the logic doesn't actually depend on the number of motors an aircraft has (but I'm not sure about the sensitivity of the chosen indicator with increase in number of motors).

Here is a graph showing the comparison of such measure in a flight with loose prop and other flights with normal props.

Note: the mult values are overriden with zero when the quad is rotating faster than 200 degrees per second or throttle is lower than 10% (as described below in the implementation) as evaluating values of I terms in such cases would be unreliable.

Implementation

The detection monitors absolute sum multiplication of I terms in all axes, triggering a warning when it exceeds a set threshold for a specified duration (0.1s by default). This provides the pilot with an early indication of potential propeller or motor issues.

Warning is triggered when abs I sum multiplication exceeds the threshold thrust_imbalance_threshold for a minimum amount of time thrust_imbalance_trigger_delay. The triggered warning can be hidden after I mult stays below the threshold for thrust_imbalance_untrigger_delay. Trigger delay reduces the probability of false positives. Untrigger delay ensures that the pilot will have extended amount of time to notice the warning. Moreover, going over the threshold might show a back-and-forth pattern, where it happens repeatedly but only for short duration each time. A low value of thrust_imbalance_trigger_delay is preferred to ensure that the warning would still be triggered even if the threshold is exceeded only for a brief moment.

I've implemented changes aimed at minimizing the probability of false positive detections. While the original detection logic remains intact, I've adjusted it to utilize multiplication of 'I' terms rather than their sum. This adjustment ensures that the measure of imbalance reflects the idea of "all I terms are high" instead of "some I terms are high".

Also the detection will now be disabled temporarily if the quad is experiencing rotational speed greater than 200dps or when throttle is lower than 10%. That basically makes the detection active when the quad is not doing any fast rotations and has good authority of P and D terms. Under these conditions, the I term values do accurately reflect the thrust balance, isolating other factors.

Changes

• Detection is implemented in a new file flight/flight_health.c;
• Added new task in tasks.c to run the detection at 100Hz;
• Added cli-settable variables which are part of pidProfile_t because the threshold depends on performance of the PID tune itself:
  • thrust_imbalance_threshold,
  • thrust_imbalance_trigger_delay,
  • thrust_imbalance_untrigger_delay.
• Added OSD warning with text "THRUST IMBALANCE";
• Added OSD menu item which allows adjusting the threshold in the field under PROFILE -> MISC PP -> THRST IMBL THRS;
• Added debug mode THRUST_IMBALANCE;
• All the parts of the feature are covered with #ifdef USE_THRUST_IMBALANCE_DETECTION which is defined in target/common_pre.h;

Testing

I've done some testing on a real quadcopter including examining blackbox logs in debug_mode THRUST_IMBALANCE. The testing results are promising and the implementation and configuration options appear to be working as expected and described here.

I would appreciate if you could test that yourself and give some feedback.

Parameters tuning

Important: Before tuning the threshold make sure that your props are in acceptable shape and properly tightened (to minimize the risk of slipping).

thrust_imbalance_trigger_delay = 1 (0.1 seconds) was chosen as a default value because often a loose prop can be slipping in an oscillating manner so the multiplication of absolute I terms would be jumping around the threshold relatively fast. That's why this value should be as low as possible to ensure the detection in such cases. Can be set to zero which will trigger the warning without any delay.

Brute Force

Set a relatively low threshold set thrust_imbalance_threshold = 5 for example. Repeatedly do test flights and increase the threshold until the warning stops showing up. When you have a proper PID tune, the threshold is expected to be around the value of 20 (which is the default value). Threshold can be adjusted in the OSD menu at PROFILE -> MISC PP -> THRST IMBL THRS.

Using Blackbox Logs

You could also examine some blackbox logs with debug_mode set to THRUST_IMBALANCE. Record a flight with tightened props in a good shape which includes aggressive stick movements to provoke I term buildup. Blackbox Viewer will tell you that your DEBUG data is about "Gyro Calibration" which isn't true. It just doesn't know what the data is and it assumes the wrong thing. You are interested in "Gyro Calibration Z" value which is in fact the mult value of abs I terms divided by 1e5 so that it corresponds with the domain of threshold cli variable. Based on the values you see in the log you can judge a threshold value that would suit your quad. Keep in mind that prop wash provoke rise of I terms and can lead to "false positives".

[github-actions]

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

• Simply put #13369 (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!

[vmzhivetyev]

I've done more tests today and this needs work. I will come up with updates a bit later.

[vmzhivetyev]

I've implemented changes aimed at minimizing the probability of false positive detections. While the original detection logic remains intact, I've adjusted it to utilize multiplication of 'I' terms rather than their sum. This adjustment ensures that the measure of imbalance reflects the idea of "all I terms are high" instead of "some I terms are high".

Also the detection will now be disabled temporarily if the quad is experiencing rotational speed greater than 200dps or when throttle is lower than 10%. That basically makes the detection active when the quad is not doing any fast rotations and has good authority of P and D terms. Under these conditions, the I term values do accurately reflect the thrust balance, isolating other factors.

I've tested this logic with a lot of my blackbox logs and it seems to work reliably. No false positives under value of 2e6 (which corresponds to the threshold value of 20 in cli) even in violent prop wash cases.

The first plot is showing the result of multiplication of I terms for a flight with a very loose prop where it stays above 5e6 for more than 0.76 seconds.

Now I need to do some tests irl.

[ledvinap]

IS there any reason to fine-tune this value?

[vmzhivetyev]

Actually I think there isn't a good one. What value you'd say would be optimal to be the hardcoded default? 1 sec?

[ledvinap]

1 sec sounds good. And the logic may be updated so that imbalance is reset always after this time, triggering again shortly after if it persists. Frequency of retrigger may indicate how bad is the situation (current code is very 'latching' when noise is present - multiple consecutive samples must be positive for trigger, them multiple negative ones are necessary for untrigger)

[ledvinap]

(maybe this may fail due to I-TERM windup prevention code in PID - IIRC I-term my be reset to zero in some conditions)

[vmzhivetyev]

As I understand, both I term windup and I term relax features only limit I term's ability to change, neither of them reset it to zero. Moreover both of these features are complementary to the implemented detection logic in this PR since they prevent I term from rising as a result of motor saturation or fast moves so that preventing false positive detections. That said I don't see a problem here.

Edit: I term can be reset to zero in some cases indeed. But as I see in code it only happens in specific scenarios like crash detection etc. it won't happen mid flight.

Also I've found a recent PR explaining current logic of both I term windup and relax #11806.

[ledvinap]

The algorithm is simple, so it is worth implementing. At least for typical quad configuration, it may work reasonably well.

[vmzhivetyev]

This way the code should be a lot easier to read and understand. Also fixed trigger/untrigger behaviour.

[ledvinap]

1 sec sounds good. And the logic may be updated so that imbalance is reset always after this time, triggering again shortly after if it persists. Frequency of retrigger may indicate how bad is the situation (current code is very 'latching' when noise is present - multiple consecutive samples must be positive for trigger, them multiple negative ones are necessary for untrigger)

[ledvinap]

Suggested change

	bool checkTimeReached(timeUs_t currentTimeUs, timeUs_t* timeToReach) {
	static bool checkTimeReached(timeUs_t currentTimeUs, timeUs_t* timeToReach) {

[ledvinap]

Suggested change

	#define THRUST_IMBALANCE_GYRO_RATE_MAX_DPS 200 // degrees per second
	#define THRUST_IMBALANCE_THROTTLE_MIN_PCT 10 // percent
	#define THRUST_IMBALANCE_BLANKING_TIME_US 500000 // 0.5 sec
	#define THRUST_IMBALANCE_CONFIG_TO_WORKING_DOMAIN 1e5f
	#define THRUST_IMBALANCE_GYRO_RATE_MAX_DPS 200 // disable detection with some gyro axis above this threshold
	#define THRUST_IMBALANCE_THROTTLE_MIN_PCT 10 // disable detection when throttle is below this threshold
	#define THRUST_IMBALANCE_BLANKING_TIME_US 500000 // 0.5 sec; delay enabling of detection
	#define THRUST_IMBALANCE_CONFIG_TO_WORKING_DOMAIN 1e5f ; see code

threshold should be computed as (thrust_imbalance_threshold * SOME_SCALING_FACTOR) ^ 3. It will allow much greater control over threshold range and user will specify value that is directly related to PID I term.
(mathematically, geometric mean of I-term should be used. But third power is faster than cube root)