import numpy as np
# The first two lines of the A matrix represent the coordinates of each rotor in the X,Y plane,
# and the third line the direction in which they spin.
# Note that in this example the X axis is vertical, and the Y coordinates are in the top row of A.
A = np.array([[-0.17, 0.17, -0.25, 0.25, -0.33, 0.33],
[-0.35, -0.35, 0., 0., 0.35, 0.35],
[-0.1, 0.1, 0.1, -0.1, -0.1, 0.1 ]])
# Moore-Penrose pseudoinverse of A
B = np.linalg.pinv(A)
# normalizing columns of B and transpose
B_normalized = (B.T / B.max(axis=0)[:, np.newaxis])
# scale and round to 256 to return final coefficients
coeffs = np.around(256 * B_normalized).astype(int)
print(coeffs)
[[ -67 67 -256 256 -189 189] [-256 -256 0 0 256 256] [-154 154 256 -256 -115 115]]
# output defines
import string
rows = ['ROLL_COEF" ', 'PITCH_COEF" ', 'YAW_COEF" ']
for i, r in enumerate(rows):
print('<define name="' + r + 'value="{' + string.join(['{:>4d}'.format(c) for c in coeffs[i]], ', ') + '}"/>')
<define name="ROLL_COEF" value="{ -67, 67, -256, 256, -189, 189}"/> <define name="PITCH_COEF" value="{-256, -256, 0, 0, 256, 256}"/> <define name="YAW_COEF" value="{-154, 154, 256, -256, -115, 115}"/>