Collations

collations.md

Given motor equations:

Z = -b (m1^2 + m2^2 + m3^2 + m4^2)
L =  b (m4^2 - m2^2)
M =  b (m1^2 - m3^2)
N =  d (m2^2 + m4^2 - m1^2 - m3^2)

(Assuming b = d = 1 for simplicity. Ignoring the (-) sign in Z until the end)

Lift

Each motor must maintain at the same z (by default). Given Z we derive z as:

Z = (z^2 + z^2 + z^2 + z^2)
Z = 4z^2
z = sqrt(Z/4)

Yaw

Each motor already maintains some uniform z, and we apply a (+n) yaw change to motors 2 and 4, and a (-n) yaw change to 1 and 3. Given an N we derive n as:

N = (z + n)^2 + (z + n)^2 - (z - n)^2 - (z - n)^2  
N = 2(z^2 + 2zn + n^2) - 2(z^2 - 2zn + n^2)
N = 8zn
n = N/(8z)

Pitch

Motors 1 and 3 have a uniform z, and some yaw appied (-n). We derive m as:

M = (z - n + m)^2 - (z - n - m)^2
M = (k + m)^2 - (k - m)^2                       [k = z-n]
M = k^2 + 2km + m^2 - (k^2 - 2km + m^2)
M = 4km
m = M/(4k)
m = M/(4(z-n))                                  [sub]

Roll

Motors 2 and 4 have a uniform z, and some applied yaw appied (+n). We derive l as:

L = (z + n + l)^2 - (z + n - l)^2
L = (k + l)^2 - (k - l)^2                       [k = z+n]
L = k^2 + 2kl + l^2 - (k^2 - 2kl + l^2)
L = 4kl
l = L/(4k)
l = L/(4(z+n))                                  [sub]

Combination

Given a Z, M, N, L, and computed z, m, n, l

m1 = z - n + m = sqrt(Z/4) - N/(8n) + M/(4(z-n))
m2 = z + n - l = sqrt(Z/4) + N/(8n) - L/(4(z+n))
m3 = z - n - m = sqrt(Z/4) - N/(8n) - M/(4(z-n))
m4 = z + n + l = sqrt(Z/4) + N/(8n) + L/(4(z+n))

When taking into account b, d, and Z having a negative sign. Can be implemented as:

// We get a negative Z, and adjust it with 'b'
Z = -(Z/b);
L = L / b;
M = M / b;
N = N / d;

// Compute RPM adjustments
z = sqrt(Z/4.0);
n = (N / (8.0 * z));
m = (M / (4.0 * (z - n)));
l = (L / (4.0 * (z + n)));

// Set RPMs
motors[0] = z - n + m;
motors[1] = z + n - l;
motors[2] = z - n - m;
motors[3] = z + n + l;