Mixer corner cases

[RomanBapst]

Hey, I have another concern about the mixer algorithm. Namely,
https://github.com/PX4/Firmware/blob/master/src/modules/systemlib/mixer/mixer_multirotor.cpp#L317

• this for cycle happens for each motor and changes “yaw” and “thrust” variables directly.
  a) https://github.com/PX4/Firmware/blob/master/src/modules/systemlib/mixer/mixer_multirotor.cpp#L341 - this line restricts thrust reduction at 0.15f, but this reduction can happen for each motor, so total reduction, in theory, is dependant on motor count, which is kinda strange.
  b) Yaw correction that happened for the first motor might later get overwritten by correction in the opposite direction in one of the later motors. This issue, probably, is less important.

I’m not sure what are the possible real-life problematic cases and what is the right way to fix it. Possibly, something similar to roll_pitch_scale is needed - I can’t say that I fully understand all the transformations that calculate the boost and this scale, so it’s hard for me to tell if it can be applied to yaw too.

[aaronpu]

yes ,i found this issue too, but sometimes, it is bad , when the Multicopter turn right and yaw value out of range, this issue will happen on CCW motors , but i dont know how to solve this except changing "out>1.5f" or more bigger.

//-----here--------
if (out > 1.5f) {

        float thrust_reduction = fminf(0.15f, out - 1.0f);
        thrust -= thrust_reduction;

        if (fabsf(_rotors[i].yaw_scale) <= FLT_EPSILON) {
            yaw = 0.0f;

        } else {
            yaw = (1.0f - ((roll * _rotors[i].roll_scale + pitch * _rotors[i].pitch_scale) *
                       roll_pitch_scale + thrust + boost)) / _rotors[i].yaw_scale;
        }

        if (status_reg != NULL) {
            (*status_reg) |= PX4IO_P_STATUS_MIXER_YAW_LIMIT;
        }
    }`

[LorenzMeier]

We reworked this recently.

[muharred]

As far as I can tell, the issue should still be present in the current master: line https://github.com/PX4/Firmware/blob/2cfcf3402e814f8a5cde7616232482853775bb5a/src/modules/systemlib/mixer/mixer_multirotor.cpp#L347 gets processed for each motor separately. So maximum possible thrust reduction because of yaw saturation still depends on motor count.

[LorenzMeier]

@MaEtUgR @priseborough Can you have a look?

[MaEtUgR]

I once developed a much simpler mixer and flew with it for the entire tests of my master thesis so it works. But I never found the time to extensively test it for safety which was my main remaining concern.

The time to have a closer look if it can be taken over was always postponed.
Let me post the last implementation of it here in the hope that it helps the discussion:

unsigned
MultirotorMixer::mix(float *outputs, unsigned space, uint16_t *status_reg)
{
	/* Summary of mixing strategy:
	1) mix roll, pitch and thrust without yaw.
	2) if some outputs violate range [0,1] then try to shift all outputs to minimize violation ->
		increase or decrease total thrust (boost). The total increase or decrease of thrust is limited
		(max_thrust_diff). If after the shift some outputs still violate the bounds then scale roll & pitch.
		In case there is violation at the lower and upper bound then try to shift such that violation is equal
		on both sides.
	3) mix in yaw and scale if it leads to limit violation.
	4) scale all outputs to range [idle_speed,1]
	*/

	float		roll    = constrain(get_control(0, 0) * _roll_scale, -1.0f, 1.0f);
	float		pitch   = constrain(get_control(0, 1) * _pitch_scale, -1.0f, 1.0f);
	float		yaw     = constrain(get_control(0, 2) * _yaw_scale, -1.0f, 1.0f);
	float		thrust  = constrain(get_control(0, 3), 0.0f, 1.0f);

	if (status_reg != NULL) (*status_reg) = 0;		// clean register for saturation status flags

	// perform initial mix pass yielding unbounded outputs, ignore yaw, just to check if they already exceed the limits
	float		min = 1;
	float		max = 0;

	for(int i = 0; i < _rotor_count; i++) {
		outputs[i] = roll * _rotors[i].roll_scale + pitch * _rotors[i].pitch_scale; // first we mix only the moments for roll and pitch on outputs

		float out = (outputs[i] + thrust) * _rotors[i].out_scale;

		if (out < min) min = out; 			// calculate min and max output values of any rotor
		if (out > max) max = out;
	}

	float boost = 0.0f;						// value added to demanded thrust (can also be negative)
	float limit_scale = 1.0f;				// scale for demanded roll and pitch

	if(max - min > 1) {						// hard case where we can't meet the controller output because it exceeds the maximal difference
		boost = (1 - max - min) / 2;		// from equation: (max - 1) + b = -(min + b) which states that after the application of boost the violation above 1 and below 0 is equal
		limit_scale = 1 / (max - min);		// we want to scale such that roll and pitch produce min = 0 and max = 1 with the boost from above applied
	} else if(min < 0)
		boost = -min;						// easy cases where we just shift the the throttle such that the controller output can be met
	else if (max > 1)
		boost = 1 - max;

	thrust += boost;
	for(int i = 0; i <_rotor_count; i++)
		outputs[i] = (outputs[i] * limit_scale);

	// notify if saturation has occurred
	if(status_reg != NULL) {
		if (min < 0.0f) (*status_reg) |= PX4IO_P_STATUS_MIXER_LOWER_LIMIT;
		if (max > 1.0f) (*status_reg) |= PX4IO_P_STATUS_MIXER_UPPER_LIMIT;
	}

	// mix again but now with thrust boost, scale roll/pitch and also add yaw
	min = 1;
	max = 0;
	float min_term = 0;
	float max_term = 0;

	for(int i = 0; i < _rotor_count; i++) {
		float yaw_term = yaw * _rotors[i].yaw_scale;
		float out = (outputs[i] + yaw_term + thrust) * _rotors[i].out_scale;

		if (out < min) {
			min = out;
			min_term = yaw_term;
		}
		if (out > max) {
			max = out;
			max_term = yaw_term;
		}
	}

	boost = 0.0f;
	float low_limit_scale = 1.0f;			// scale for demanded yaw only
	float high_limit_scale = 1.0f;

	if(max - min > 1) {						// hard case where we can't meet the controller output because it exceeds the maximal difference
		boost = (1 - max - min) / 2;		// from equation: (max - 1) + b = -(min + b) which states that after the application of boost the violation above 1 and below 0 is equal
		if(min < 0 && min_term < 0) {		// we want to scale only the yaw term to fill up the remaining control actuation such that min 0 and max 1 and roll pitch does NOT get rescaled
			limit_scale = (min + boost - min_term) / -(min_term);
			//printf("-");
		}
		if (max > 1 && max_term > 0){
			limit_scale = (1 - (max + boost - max_term)) / max_term;
			//printf("+");
		}
		limit_scale = low_limit_scale < high_limit_scale ? low_limit_scale : high_limit_scale;
		limit_scale = limit_scale > 0 ? limit_scale : 0;
	} else if(min < 0)
		boost = -min;						// easy cases where we just shift the the throttle such that the controller output can be met
	else if (max > 1)
		boost = 1 - max;

	// inform about yaw limit reached
	if(status_reg != NULL) {
		if (min < 0.0f || max > 1.0f) (*status_reg) |= PX4IO_P_STATUS_MIXER_YAW_LIMIT;
	}

	/* add yaw and scale outputs to range idle_speed...1 */
	for (unsigned i = 0; i < _rotor_count; i++) {
		outputs[i] = outputs[i] + yaw * _rotors[i].yaw_scale * limit_scale + thrust + boost;

		outputs[i] = constrain(_idle_speed + (outputs[i] * (1.0f - _idle_speed)), _idle_speed, 1.0f);
	}

	return _rotor_count;
}

[AndreasAntener]

Apart from @MaEtUgR mixer input for discussions, didn't this get fixed in #6401?

[muharred]

It looks like it was fixed in the mentioned PR, yes.

[MaEtUgR]

@AndreasAntener Thanks, don't worry I will follow up on my proposal in a nicer way (pr) 😃