There may be a bug in void AP_MotorsMatrix::output_armed_stabilizing()

[luweiagi]

@lthall
bug is in void AP_MotorsMatrix::output_armed_stabilizing() at
https://github.com/ArduPilot/ardupilot/blob/master/libraries/AP_Motors/AP_MotorsMatrix.cpp#L366

code should be modified from A to B:
A:

B:

why add

    if (throttle_avg_max + rpy_high > 1.0f) {
        rpy_scale = MIN(rpy_scale, (1.0f - throttle_avg_max) / rpy_high);
    }

let's prove it:

we write python code to find:

STEP = 0.1
throttle_avg_max = np.arange(STEP, 0.5+STEP, STEP)
for avg_max in throttle_avg_max:
    # rpy_low = np.arange(-1.0, 1.0-STEP, STEP)
    rpy_low = np.arange(-1.0, 0-STEP, STEP)
    for low in rpy_low:
        rpy_high = np.arange(0.0, 1.0, STEP)
        for high in rpy_high:
            if high <= low:
                continue
            rpy_scale = 1.0
            if high - low > 1.0:
                rpy_scale = 1.0 / (high - low)
            if avg_max + low < 0.0:
                rpy_scale = min(rpy_scale, -avg_max / low)
            if avg_max + high > 1.0:
                # rpy_scale = min(rpy_scale, (1.0 - avg_max) / high)
                if (1.0 - avg_max) / high < rpy_scale:
                    print("rpy_low = {}, avg_max = {}, rpy_high = {}".format(low, avg_max, high))

The python code produces many results, we only choose one example to analyze:
rpy_low = -0.2, avg_max = 0.5, rpy_high = 0.9

// =====================
// === second bug: =======
// =====================

here has bug in red box:

[rmackay9]

@luweiagi,

Thanks for the investigation and report.

Maybe you could provide some more evidence showing the current values are incorrect? Perhaps pass in some test value and provide the resulting output. Another alternative may be to test in SITL to see if you can recreate situations where the existing formula comes up with the incorrect output?

Sorry to be annoying but it takes quite a lot of effort to investigate these reports and most of the time we determine that there is no actual error. It's more often something has been overlooked in the report.

[luweiagi]

@luweiagi,

Thanks for the investigation and report.

Maybe you could provide some more evidence showing the current values are incorrect? Perhaps pass in some test value and provide the resulting output. Another alternative may be to test in SITL to see if you can recreate situations where the existing formula comes up with the incorrect output?

Sorry to be annoying but it takes quite a lot of effort to investigate these reports and most of the time we determine that there is no actual error. It's more often something has been overlooked in the report.

OK, I have updated it.

[lthall]

Hi @luweiagi,

As always, I appreciate you looking so closely at the code!!

I don't believe this is a bug. The purpose of rpy_scale is to limit the roll, pitch and yaw if we want to prioritise low throttle. When we have high throttle we always prioritise roll, pitch and yaw over throttle. This means we only adjust the rpy_scale if the lower limit can't fit. If the upper limit can't fit then we reduce the throttle to give it room.

The second limit problem you pointed out isn't really a bug but more of an approximation. It is possible that one of the three axis may be able to be increased without increasing the saturation but the calculations required to remove this approximation would be expansive and complicated.

Have I missed something?

[luweiagi]

Hi @luweiagi,

As always, I appreciate you looking so closely at the code!!

I don't believe this is a bug. The purpose of rpy_scale is to limit the roll, pitch and yaw if we want to prioritise low throttle. When we have high throttle we always prioritise roll, pitch and yaw over throttle. This means we only adjust the rpy_scale if the lower limit can't fit. If the upper limit can't fit then we reduce the throttle to give it room.

The second limit problem you pointed out isn't really a bug but more of an approximation. It is possible that one of the three axis may be able to be increased without increasing the saturation but the calculations required to remove this approximation would be expansive and complicated.

Have I missed something?

@lthall Thanks for your patient reply!
I get your meaning of " The purpose of rpy_scale is to limit the roll, pitch and yaw if we want to prioritise low throttle. When we have high throttle we always prioritise roll, pitch and yaw over throttle. This means we only adjust the rpy_scale if the lower limit can't fit. If the upper limit can't fit then we reduce the throttle to give it room."

The second limit problem you pointed out isn't really a bug but more of an approximation. It is possible that one of the three axis may be able to be increased without increasing the saturation but the calculations required to remove this approximation would be expansive and complicated.

I have a case I can not figure out:
case: avg_max=0.3, rpy_low=-0.2, rpy_high=1.8, throttle_thrust=0.2.
If we go with the logic of the second bug, we can get:

But, I think another method which follow what you said:

The purpose of rpy_scale is to limit the roll, pitch and yaw if we want to prioritise low throttle

rpy_high - rpy_low = 1.8 - (-0.2) = 2.0
2.0 > 1.0, so we get
scale = 1 / (rpy_high - rpy_low) = 0.5
and
rpy_low = rpy_low * scale = -0.1
rpy_high = rpy_high * scale = 0.9
so:
throttle_thrust + rpy_low = 0.2 + (-0.1) = 0.1
throttle_thrust + rpy_high = 0.2 + 0.9 = 1.1
because throttle_thrust + rpy_high > 1, we must do re-scale:
scale = (1 - throttle_thrust) / rpy_high = (1 - 0.2) / 0.9 = 0.8 / 0.9
so:
throttle_thrust + rpy_low = 0.2 + (-0.1) * (0.8 / 0.9) = 0.2 + (-0.08/0.9) = 0.2 + (-0.089) = 0.111
throttle_thrust + rpy_high = 0.2 + 0.9 * 0.8 / 0.9 = 0.2 + 0.8 = 1

This method truely follow what you said:

The purpose of rpy_scale is to limit the roll, pitch and yaw if we want to prioritise low throttle

Because we want to prioritise low throttle , so throttle_thrust = 0.2 is keeped, not reduce. So copter will not descend faster than desired(throttle_thrust) which I think is not safe. But reduce the throttle for rpy control is also not safe, so I am confused which method we should choose. How do we balance the hazards of descent exceeding expectations and attitude loss control?

[peterbarker]

@lthall ball is in your court on this one, I think.

[lthall]

Sorry @luweiagi, I missed your follow up. I think the problem appears here because you are using values of rpy_low and rpy_high that we can't obtain with a real motor mixer. The key limitation is that the inputs roll, pitch, yaw and thrust are orthogonal.

If we have roll, pitch and yaw all one we would get quad X outputs of 3, -1, -1, -1.

    // calculate any scaling needed to make the combined thrust outputs fit within the output range
    float rpy_scale = 1.0f;
    if (rpy_high - rpy_low > 1.0f) {
        rpy_scale = 1.0f / (rpy_high - rpy_low);
    }

rpy_scale = 1/4 = 0.25

    if (throttle_avg_max + rpy_low < 0) {
        rpy_scale = MIN(rpy_scale, -throttle_avg_max / rpy_low);
    }

if throttle_avg_max = 0.3, throttle_avg_max + rpy_low < 0
-throttle_avg_max / rpy_low = 0.3 > 0.25

    // calculate how close the motors can come to the desired throttle
    rpy_high *= rpy_scale;
    rpy_low *= rpy_scale;

We scale that down to get 0.75, -0.25, -0.25, -0.25
rpy_high = 0.75,
rpy_low = -0.25

    throttle_thrust_best_rpy = -rpy_low;

throttle_thrust_best_rpy = - rpy_low = 0.25

    float thr_adj = throttle_thrust - throttle_thrust_best_rpy;

throttle_thrust = 0.2
thr_adj = 0.2 - 0.25 = -0.05

    if (rpy_scale < 1.0f) {
        // Full range is being used by roll, pitch, and yaw.
        limit.roll = true;
        limit.pitch = true;
        limit.yaw = true;
        if (thr_adj > 0.0f) {
            limit.throttle_upper = true;
        }

thr_adj = -0.05 < 0.0

        thr_adj = 0.0f;
    } else if (thr_adj < 0.0f) {
        // Throttle can't be reduced to desired value
        // todo: add lower limit flag and ensure it is handled correctly in altitude controller
        thr_adj = 0.0f;
    } else if (thr_adj > 1.0f - (throttle_thrust_best_rpy + rpy_high)) {
        // Throttle can't be increased to desired value
        thr_adj = 1.0f - (throttle_thrust_best_rpy + rpy_high);
        limit.throttle_upper = true;
    }

So I think the reason you are getting this funny result is you are using values that we can't get with a properly orthogonal mixer.