# Mixing Motors #3

I’ve written about this before (here & here). In the previous code I was making a few assumptions:
1. Overall motor rotation is zero – so there is the same number of clockwise and counter-clockwise motors
2. Thrust vector is always pointing downwards
3. No physical units. The old mixer worked in ratios – full rotation along X axis, 0.3 rotation along Z Axis and so on

Now I want to get rid of all these assumptions and build a mixer that given a desired torque and the motor+quad configuration – it can figure out how much throttle to apply to each motor.

This is proving more complicated than I thought and the fact that I don’t have any formal math background doesn’t help at all.

So my requirements:
– Work in physical units. I want to to send the mixer 2 quantities: a torque in Nm and a collective force in N.
– Any (valid) motor configuration. X and + frames, quad, hexa, octo, spider, Y6 etc. Including motors with random thrust vectors – like a tricopter. Non-downward thrust vectors opens the door to a nice simplification as we’ll see below.
– Unbalanced setups. In reality it often happens that motors differ and I want my mixer to account for this and compensate without forcing the PIDs to do it

So first step is to have a model for the motor. I have the following struct to represent the relevant motor data:

```struct Motor
{
math::vec3f position;
math::vec3f thrust_vector;
bool clockwise = true;
float max_thrust = 0; //N
float max_rpm = 0;
float acceleration = 0; //rpm/s
float deceleration = 0; //rpm/s
};```

Position is in local space, relative to the center of mass of the UAV.
Thrust vector is normalized.
Max_thrust is in N and is the thrust of the motor at max_rpm.
Acceleration and deceleration are used to model motor+prop inertia.

Using the position and thrust_vector I can calculate the max torque the motor will produce:

max_torque = thrust_vector X position * max_thrust; (X is cross product)

Using max_rpm and max_thrust I can figure out how much throttle too apply to get a certain thrust using the following approximation:

```//thrust increases approximately with throttle^2
auto throttle = math::sqrt<float, math::safe>(target_thrust / max_thrust);
```

Now the problem is – how to figure out what throttle to send to each motor to get a certain torque and total force?

Let’s consider a simple case: a perfectly balanced quadcopter with a + frame. Each motor points downwards so they generate a torque along one of the axis (and arms) but there is the yaw torque as well that needs to be taken into account. This can be modeled by simply tilting a bit the thrust vector which in turn creates a small torque on the Z axis. The tilt factor depends on the motor and prop but it’s usually a factor of the RPM or RPM^2. For now let’s consider it constant.

So each motor produces a max torque: A, B, C, D.
A and C are opposite, just like B and D:

A = -C
B = -D

What I want to find out is the factors a, b, c and d in the equation below:
1. aA + bB + cC + dD = T    where T is the target torque of the quad

As you can imagine there are an infinite number of factors that give a good answer so we need more constraints. These are:
2. I also want a total thrust of Th.
3. I want a, b, c, d to have the minimum values that result in the desired torque. So I want the optimal solution.

For a quad, equations 1 and 2 are enough to calculate the optimal factors but for a hexa, octo or any other setup it’s not enough. I don’t know how to express eq 3 mathematically but this equation should result in an unique set of factors.
So in the past days I tried to figure out how to calculate the coefficients for some very weird (but valid) motor configurations and in the end I gave up on the math approach.

So what’s left when the analytic approach is out of reach? You try brute force of course.
I wrote a small iterative algorithm that tries to find out the factors with a form of binary search. It’s very short and seems to be fast enough. It takes a motor config, a target torque and a step size and computes the factors with a precision of 0.0001.

A step size of 0.01 requires ~3-7000 iterations, 0.1 requires 3-700 and 0.5 can solve it in 50-100 iterations. Pretty good.
However I cannot prove that the algorithm will always converge (if I could prove this, then I could also solve the initial problem to begin with) and I’ve been running the algorithm all day yesterday with random valid motor configs and random target torques and so far I had no bad cases.

Here it is:

```void test_mm(math::vec3f const& target, float step)
{
struct Motor
{
math::vec3f torque; //precomputed maximum torque
float factor;
};

//some test configuration with random data
std::vector<Motor> motors = {{
{{2, 3, 0.1}, 0},
{{5, -1, -0.1}, 0},
{{-1, -7, 0.1}, 0},
{{-4, 2, -0.1}, 0},
{{-0.1, .2, -1}, 0},
}};

size_t iteration = 0;
while (true)
{
//first calculate the crt torque
math::vec3f crt;
for (auto& m: motors)
{
crt += m.torque * m.factor;
}

//check if we're done
if (math::equals(crt, target, 0.0001f))
{
QLOGI("{}: Done in {} iterations", step, iteration);
break;
}

//how far are we for the target?
//divide by motor count because I want to distribute the difference to all motors
auto diff = (target - crt) / float(motors.size());

//distribute the diff to all motors
for (auto& m: motors)
{
//figure out how much each motor can influence the target torque
//  by doing a dot product with the normalized torque vector
auto t = math::normalized(m.torque);
auto f = math::dot(t, diff) * step; //go toqards the difference in small steps so we can converge
m.factor += f;
}

iteration++;
}
}```

And the test code:

```q::util::Rand rnd;
while (true)
{
math::vec3f target(rnd.get_float() * 40.f, rnd.get_float() * 40.f, rnd.get_float() * 10.f);
test_mm(target, 0.01);
test_mm(target, 0.1);
test_mm(target, 0.5);
}
```

Now I have to test in in the simulator.