dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Mixers: Rename geom -> geometry(ies)

This commit is contained in:
Julien Lecoeur 2017-10-13 13:58:18 +02:00 committed by Beat Küng
parent c95229faab
commit b6911c2266
1 changed files with 42 additions and 42 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

84
src/lib/mixer/geoms/tools/px_generate_mixers.py
View File

@ -63,7 +63,7 @@ __copyright__ = "Copyright (C) 2013-2017 PX4 Development Team."
__license__ = "BSD"
__email__ = "julien.lecoeur@gmail.com"
def parse_geom_toml(filename):
def parse_geometry_toml(filename):
'''
Parses toml geometry file and returns a dictionary with curated list of rotors
'''
@ -117,10 +117,10 @@ def parse_geom_toml(filename):
rotor_list.append(r)
# Clean dictionary
geom = {'info': d['info'],
geometry = {'info': d['info'],
'rotors': rotor_list}
return geom
return geometry
def torque_matrix(center, axis, dirs, Ct, Cm):
'''
@ -131,18 +131,18 @@ def torque_matrix(center, axis, dirs, Ct, Cm):
torque = Ct * np.cross(center, ax) - Cm * ax * dirs
return torque
def geom_to_torque_matrix(geom):
def geometry_to_torque_matrix(geometry):
'''
Compute torque matrix Am and Bm from geometry dictionnary
Am is a 3xN matrix where N is the number of rotors
Each column is the torque generated by one rotor
'''
Am = torque_matrix(center=np.array([rotor['position'] for rotor in geom['rotors']]),
axis=np.array([rotor['axis'] for rotor in geom['rotors']]),
Am = torque_matrix(center=np.array([rotor['position'] for rotor in geometry['rotors']]),
axis=np.array([rotor['axis'] for rotor in geometry['rotors']]),
dirs=np.array([[1.0 if rotor['direction'] == 'CCW' else -1.0]
for rotor in geom['rotors']]),
Ct=np.array([[rotor['Ct']] for rotor in geom['rotors']]),
Cm=np.array([[rotor['Cm']] for rotor in geom['rotors']])).T
for rotor in geometry['rotors']]),
Ct=np.array([[rotor['Ct']] for rotor in geometry['rotors']]),
Cm=np.array([[rotor['Cm']] for rotor in geometry['rotors']])).T
return Am
def thrust_matrix(axis, Ct):
@ -154,18 +154,18 @@ def thrust_matrix(axis, Ct):
thrust = Ct * ax
return thrust
def geom_to_thrust_matrix(geom):
def geometry_to_thrust_matrix(geometry):
'''
Compute thrust matrix At from geometry dictionnary
At is a 3xN matrix where N is the number of rotors
Each column is the thrust generated by one rotor
'''
At = thrust_matrix(axis=np.array([rotor['axis'] for rotor in geom['rotors']]),
Ct=np.array([[rotor['Ct']] for rotor in geom['rotors']])).T
At = thrust_matrix(axis=np.array([rotor['axis'] for rotor in geometry['rotors']]),
Ct=np.array([[rotor['Ct']] for rotor in geometry['rotors']])).T
return At
def geom_to_mix(geom):
def geometry_to_mix(geometry):
'''
Compute combined torque & thrust matrix A and mix matrix B from geometry dictionnary
@ -177,8 +177,8 @@ def geom_to_mix(geom):
roll torque, pitch torque, yaw torque, x thrust, y thrust, z thrust
'''
# Combined torque & thrust matrix
At = geom_to_thrust_matrix(geom)
Am = geom_to_torque_matrix(geom)
At = geometry_to_thrust_matrix(geometry)
Am = geometry_to_torque_matrix(geometry)
A = np.vstack([Am, At])
# Mix matrix computed as pseudoinverse of A
@ -214,7 +214,7 @@ def normalize_mix_px4(B):
return B_px
def generate_mixer_multirotor_header(geom_list, use_normalized_mix=False, use_6dof=False):
def generate_mixer_multirotor_header(geometries_list, use_normalized_mix=False, use_6dof=False):
'''
Generate C header file with same format as multi_tables.py
TODO: rewrite using templates (see generation of uORB headers)
@ -233,21 +233,21 @@ def generate_mixer_multirotor_header(geom_list, use_normalized_mix=False, use_6d
# Print enum
buf.write(u"enum class MultirotorGeometry : MultirotorGeometryUnderlyingType {\n")
for i, geom in enumerate(geom_list):
buf.write(u"\t{} = {},\n".format(geom['info']['name'].upper(), i))
for i, geometry in enumerate(geometries_list):
buf.write(u"\t{} = {},\n".format(geometry['info']['name'].upper(), i))
buf.write(u"\n\tMAX_GEOMETRY\n")
buf.write(u"}; // enum class MultirotorGeometry\n\n")
# Print mixer gains
buf.write(u"namespace {\n")
for geom in geom_list:
for geometry in geometries_list:
# Get desired mix matrix
if use_normalized_mix:
mix = geom['mix']['B_px']
mix = geometry['mix']['B_px']
else:
mix = geom['mix']['B']
mix = geometry['mix']['B']
buf.write(u"const MultirotorMixer::Rotor _config_{}[] = {{\n".format(geom['info']['name']))
buf.write(u"const MultirotorMixer::Rotor _config_{}[] = {{\n".format(geometry['info']['name']))
for row in mix:
if use_6dof:
@ -263,22 +263,22 @@ def generate_mixer_multirotor_header(geom_list, use_normalized_mix=False, use_6d
buf.write(u"};\n\n")
# Print geom indeces
# Print geometry indeces
buf.write(u"const MultirotorMixer::Rotor *_config_index[] = {\n")
for geom in geom_list:
buf.write(u"\t&_config_{}[0],\n".format(geom['info']['name']))
for geometry in geometries_list:
buf.write(u"\t&_config_{}[0],\n".format(geometry['info']['name']))
buf.write(u"};\n\n")
# Print geom rotor counts
# Print geometry rotor counts
buf.write(u"const unsigned _config_rotor_count[] = {\n")
for geom in geom_list:
buf.write(u"\t{}, /* {} */\n".format(len(geom['rotors']), geom['info']['name']))
for geometry in geometries_list:
buf.write(u"\t{}, /* {} */\n".format(len(geometry['rotors']), geometry['info']['name']))
buf.write(u"};\n\n")
# Print geom key
# Print geometry key
buf.write(u"const char* _config_key[] = {\n")
for geom in geom_list:
buf.write(u"\t\"{}\",\t/* {} */\n".format(geom['info']['key'], geom['info']['name']))
for geometry in geometries_list:
buf.write(u"\t\"{}\",\t/* {} */\n".format(geometry['info']['key'], geometry['info']['name']))
buf.write(u"};\n\n")
# Print footer
@ -295,9 +295,9 @@ if __name__ == '__main__':
# Parse arguments
parser = argparse.ArgumentParser(
description='Convert geom .toml files to mixer headers')
description='Convert geometry .toml files to mixer headers')
parser.add_argument('-d', dest='dir',
help='directory with geom files')
help='directory with geometry files')
parser.add_argument('-f', dest='files',
help="files to convert (use only without -d)",
nargs="+")
@ -321,29 +321,29 @@ if __name__ == '__main__':
raise Exception("Missing input directory (-d) or list of geometry files (-f)")
# List of geometries
geom_list = []
geometries_list = []
for filename in filenames:
# Parse geom file
geom = parse_geom_toml(filename)
# Parse geometry file
geometry = parse_geometry_toml(filename)
# Compute torque and thrust matrices
A, B = geom_to_mix(geom)
A, B = geometry_to_mix(geometry)
# Normalize mixer
B_px = normalize_mix_px4(B)
# Store matrices in geom
geom['mix'] = {'A': A, 'B': B, 'B_px': B_px}
# Store matrices in geometry
geometry['mix'] = {'A': A, 'B': B, 'B_px': B_px}
# Add to list
geom_list.append(geom)
geometries_list.append(geometry)
if args.verbose:
print('\nFilename')
print(filename)
print('\nGeometry')
print(geom)
print(geometry)
print('\nA:')
print(A.round(2))
print('\nB:')
@ -353,7 +353,7 @@ if __name__ == '__main__':
print('\n-----------------------------')
# Generate header file
header = generate_mixer_multirotor_header(geom_list,
header = generate_mixer_multirotor_header(geometries_list,
use_normalized_mix=args.normalize,
use_6dof=args.sixdof)