#!/usr/bin/python
# Python script for naze32 multirotor mixers
#
# BorisB / Richard
# Version 0.31
# Source: http://www.rcgroups.com/forums/showpost.php?p=30231234&postcount=910
#
######################################################################################################
import getopt
from optparse import OptionParser
import numpy as np
parser = OptionParser()
parser.add_option("--A", dest="A", default=0, help="A=Y Distance from COG to front motors")
parser.add_option('--B', dest="B", default=0, help="B=Y Distance from COG to back motors")
parser.add_option('--C', dest="C", default=0, help="C=X Distance from COG to front motors")
parser.add_option('--D', dest="D", default=0, help="D=X Distance from COG to side motors")
parser.add_option('--E', dest="E", default=0, help="E=X Distance from COG to back motors")
opts, args = parser.parse_args()
A = float(opts.A)
B = float(opts.B)
C = float(opts.C)
D = float(opts.D)
E = float(opts.E)
Aneg = 0 - A
Bneg = 0 - B
Cneg = 0 - C
Dneg = 0 - D
Eneg = 0 - E
if A<0 or B<0 or C<0 or D<0 or E<0:
print('ERROR: Only use positive values as input')
print('--h for help')
quit()
# 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 Dynamic through Option parser
A = np.array([[B, Aneg, B, Aneg, 0, 0], # Y axis
[ Eneg, Cneg, E, C, Dneg, D], # 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)
#checkpoints
rollcheck = float(roll1) + float(roll2) + float(roll3) + float(roll4) +float(roll5) + float(roll6)
pitchcheck = float(pitch1) + float(pitch2) + float(pitch3) + float(pitch4) +float(pitch5) + float(pitch6)
yawcheck = float(yaw1) + float(yaw2) + float(yaw3) + float(yaw4) +float(yaw5) + float(yaw6)
print('')
print ('---------------------------------------------')
print('for spider a symetrical roll and yaw is expected, pitch will be asymetrical')
if rollcheck==0:
print('sum of roll should be zero: ' + format(rollcheck) + ' OK')
else:
print('sum of roll should be zero: '+ format(rollcheck) + ' NOT OK')
if pitchcheck==0:
print('sum of pitch no zero value expected: '+ format(pitchcheck) + ' NOT OK')
else:
print('sum of pitch no zero value expected: '+ format(pitchcheck) + ' OK')
if yawcheck==0:
print('sum of yaw should be zero: '+ format(yawcheck) + ' OK')
else:
print('sum of yaw should be zero: '+ format(yawcheck) + ' NOT OK')
if float(roll1)<0 and float(roll2)<0 and float(roll5)<0:
print('Motors 1,2,5 same roll polarity expected: OK')
else:
print('Motors 1,2,5 same roll polarity expected: NOT OK')
if float(pitch2)<0 and float(pitch4)<0:
print('Motors 2,4 same pitch polarity expected: OK')
else:
print('Motors 2,4 same pitch polarity expected: NOT OK')