#!/usr/bin/python
# Python script for naze32 multirotor mixers
#
######################################################################################################
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.
# Settings Mini Hex
A = np.array([[128, -110, 128, -110, 0, 0], # Y axis
[ -72, -96.5, 72, 96.5, -145, 145], # X axis
[ 1, 1, -1, -1, -1, 1]]) # Motor cw/ccw
# Settings Super Hex
#A = np.array([[15, -14, 15, -14, 0, 0], # Y axis
# [ -8, -11, 8, 11, -17, 17], # X axis
# [ 1, 1, -1, -1, -1, 1]]) # Motor cw/ccw
# 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 1 to return final coefficients
coeffs = np.around(1*B_normalized,2)
import string
print('')
print ('Custom mix voor naze')
print ('---------------------------------------------')
print('Motor: 1 2 3 4 5 6')
rows = ['PITCH ', 'ROLL ', 'YAW ']
for i, r in enumerate(rows):
print(r + '{' + ','.join([format(c) for c in coeffs[i]]) + '}')
x=0
for c in coeffs[1]:
#print(format(c))
x += 1
if x==1:
roll1=format(c)
if x==2:
roll2=format(c)
if x==3:
roll3=format(c)
if x==4:
roll4=format(c)
if x==5:
roll5=format(c)
if x==6:
roll6=format(c)
x=0
for c in coeffs[0]:
#print(format(c))
x += 1
if x==1:
pitch1=format(c)
if x==2:
pitch2=format(c)
if x==3:
pitch3=format(c)
if x==4:
pitch4=format(c)
if x==5:
pitch5=format(c)
if x==6:
pitch6=format(c)
x=0
for c in coeffs[2]:
#print(format(c))
x += 1
if x==1:
yaw1=format(c)
if x==2:
yaw2=format(c)
if x==3:
yaw3=format(c)
if x==4:
yaw4=format(c)
if x==5:
yaw5=format(c)
if x==6:
yaw6=format(c)
# nicely output result as defines for copy-pasting
print ('---------------------------------------------')
print ('')
print ('mixer custom')
print ('cmix 1 1 ' + roll1 + ' ' + pitch1 + ' ' + yaw1)
print ('cmix 2 1 ' + roll2 + ' ' + pitch2 + ' ' + yaw2)
print ('cmix 3 1 ' + roll3 + ' ' + pitch3 + ' ' + yaw3)
print ('cmix 4 1 ' + roll4 + ' ' + pitch4 + ' ' + yaw4)
print('cmix 5 1 ' + roll5 + ' ' + pitch5 + ' ' + yaw5)
print('cmix 6 1 ' + roll6 + ' ' + pitch6 + ' ' + yaw6)
