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.