mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-06-28 07:20:35 +08:00
Anton Matosov
160 lines
2.8 KiB
Python
Executable File
160 lines
2.8 KiB
Python
Executable File
#!/usr/bin/python
|
|
#
|
|
# Generate multirotor mixer scale tables compatible with the ArduCopter layout
|
|
#
|
|
|
|
import math
|
|
|
|
print "/*"
|
|
print "* These file is automatically generated by multi_tables.py - do not edit."
|
|
print "*/"
|
|
print ""
|
|
|
|
def rcos(angleInRadians):
|
|
return math.cos(math.radians(angleInRadians))
|
|
|
|
CCW = 1.0
|
|
CW = -CCW
|
|
|
|
quad_x = [
|
|
[ 45, CCW],
|
|
[-135, CCW],
|
|
[-45, CW],
|
|
[135, CW],
|
|
]
|
|
|
|
quad_plus = [
|
|
[ 90, CCW],
|
|
[ -90, CCW],
|
|
[ 0, CW],
|
|
[ 180, CW],
|
|
]
|
|
|
|
quad_v = [
|
|
[ 18.8, 0.4242],
|
|
[ -18.8, 1.0],
|
|
[ -18.8, -0.4242],
|
|
[ 18.8, -1.0],
|
|
]
|
|
|
|
quad_wide = [
|
|
[ 68, CCW],
|
|
[ -129, CCW],
|
|
[ -68, CW],
|
|
[ 129, CW],
|
|
]
|
|
|
|
hex_x = [
|
|
[ 90, CW],
|
|
[ -90, CCW],
|
|
[ -30, CW],
|
|
[ 150, CCW],
|
|
[ 30, CCW],
|
|
[-150, CW],
|
|
]
|
|
|
|
hex_plus = [
|
|
[ 0, CW],
|
|
[ 180, CCW],
|
|
[-120, CW],
|
|
[ 60, CCW],
|
|
[ -60, CCW],
|
|
[ 120, CW],
|
|
]
|
|
|
|
hex_cox = [
|
|
[ 60, CW],
|
|
[ 60, CCW],
|
|
[ 180, CW],
|
|
[ 180, CCW],
|
|
[ -60, CW],
|
|
[ -60, CCW],
|
|
]
|
|
|
|
octa_x = [
|
|
[ 22.5, CW],
|
|
[-157.5, CW],
|
|
[ 67.5, CCW],
|
|
[ 157.5, CCW],
|
|
[ -22.5, CCW],
|
|
[-112.5, CCW],
|
|
[ -67.5, CW],
|
|
[ 112.5, CW],
|
|
]
|
|
|
|
octa_plus = [
|
|
[ 0, CW],
|
|
[ 180, CW],
|
|
[ 45, CCW],
|
|
[ 135, CCW],
|
|
[ -45, CCW],
|
|
[-135, CCW],
|
|
[ -90, CW],
|
|
[ 90, CW],
|
|
]
|
|
|
|
octa_cox = [
|
|
[ 45, CCW],
|
|
[ -45, CW],
|
|
[-135, CCW],
|
|
[ 135, CW],
|
|
[ -45, CCW],
|
|
[ 45, CW],
|
|
[ 135, CCW],
|
|
[-135, CW],
|
|
]
|
|
|
|
twin_engine = [
|
|
[ 90, 0.0],
|
|
[-90, 0.0],
|
|
]
|
|
|
|
def variableName(variable):
|
|
for variableName, value in list(globals().iteritems()):
|
|
if value is variable:
|
|
return variableName
|
|
|
|
tables = [quad_x, quad_plus, quad_v, quad_wide, hex_x, hex_plus, hex_cox, octa_x, octa_plus, octa_cox, twin_engine]
|
|
|
|
|
|
def printEnum():
|
|
print "enum MultirotorMixer::Geometry : unsigned int {"
|
|
for table in tables:
|
|
print "\t{},".format(variableName(table).upper())
|
|
|
|
print "\n\tMAX_GEOMETRY"
|
|
print "}; // enum MultirotorMixer::Geometry\n"
|
|
|
|
def printScaleTables():
|
|
for table in tables:
|
|
print "const MultirotorMixer::Rotor _config_{}[] = {{".format(variableName(table))
|
|
for (angle, yawScale) in table:
|
|
rollScale = rcos(angle + 90)
|
|
pitchScale = rcos(angle)
|
|
print "\t{{ {:9f}, {:9f}, {:9f} }},".format(rollScale, pitchScale, yawScale)
|
|
print "};\n"
|
|
|
|
def printScaleTablesIndex():
|
|
print "const MultirotorMixer::Rotor *_config_index[] = {"
|
|
for table in tables:
|
|
print "\t&_config_{}[0],".format(variableName(table))
|
|
print "};\n"
|
|
|
|
|
|
def printScaleTablesCounts():
|
|
print "const unsigned _config_rotor_count[] = {"
|
|
for table in tables:
|
|
print "\t{}, /* {} */".format(len(table), variableName(table))
|
|
print "};\n"
|
|
|
|
|
|
|
|
printEnum()
|
|
|
|
print "namespace {"
|
|
printScaleTables()
|
|
printScaleTablesIndex()
|
|
printScaleTablesCounts()
|
|
|
|
print "} // anonymous namespace\n"
|