kylemanna
12/18/2014 - 5:31 PM

## mixer_naze.py

``````#!/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')
``````