fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-03 14:58:53 +08:00
Code Issues Packages Projects Releases Wiki Activity

Mixers: Rename geom -> geometry(ies)

Browse Source
This commit is contained in:
Julien Lecoeur
2017-11-15 09:56:10 +01:00
committed by Beat Küng
parent c95229faab
commit b6911c2266
1 changed files with 42 additions and 42 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/mixer/geoms/tools/px_generate_mixers.py
+42 -42
View File
@@ -63,7 +63,7 @@ __copyright__ = "Copyright (C) 2013-2017 PX4 Development Team."
__license__ = "BSD" __license__ = "BSD"
__email__ = "julien.lecoeur@gmail.com" __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 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) rotor_list.append(r)
# Clean dictionary # Clean dictionary
geom = {'info': d['info'], geometry = {'info': d['info'],
'rotors': rotor_list} 'rotors': rotor_list}
return geom return geometry
def torque_matrix(center, axis, dirs, Ct, Cm): 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 torque = Ct * np.cross(center, ax) - Cm * ax * dirs
return torque return torque
def geom_to_torque_matrix(geom): def geometry_to_torque_matrix(geometry):
''' '''
Compute torque matrix Am and Bm from geometry dictionnary Compute torque matrix Am and Bm from geometry dictionnary
Am is a 3xN matrix where N is the number of rotors Am is a 3xN matrix where N is the number of rotors
Each column is the torque generated by one rotor Each column is the torque generated by one rotor
''' '''
Am = torque_matrix(center=np.array([rotor['position'] 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 geom['rotors']]), axis=np.array([rotor['axis'] for rotor in geometry['rotors']]),
dirs=np.array([[1.0 if rotor['direction'] == 'CCW' else -1.0] dirs=np.array([[1.0 if rotor['direction'] == 'CCW' else -1.0]
for rotor in geom['rotors']]), for rotor in geometry['rotors']]),
Ct=np.array([[rotor['Ct']] for rotor in geom['rotors']]), Ct=np.array([[rotor['Ct']] for rotor in geometry['rotors']]),
Cm=np.array([[rotor['Cm']] for rotor in geom['rotors']])).T Cm=np.array([[rotor['Cm']] for rotor in geometry['rotors']])).T
return Am return Am
def thrust_matrix(axis, Ct): def thrust_matrix(axis, Ct):
@@ -154,18 +154,18 @@ def thrust_matrix(axis, Ct):
thrust = Ct * ax thrust = Ct * ax
return thrust return thrust
def geom_to_thrust_matrix(geom): def geometry_to_thrust_matrix(geometry):
''' '''
Compute thrust matrix At from geometry dictionnary Compute thrust matrix At from geometry dictionnary
At is a 3xN matrix where N is the number of rotors At is a 3xN matrix where N is the number of rotors
Each column is the thrust generated by one rotor Each column is the thrust generated by one rotor
''' '''
At = thrust_matrix(axis=np.array([rotor['axis'] for rotor in geom['rotors']]), At = thrust_matrix(axis=np.array([rotor['axis'] for rotor in geometry['rotors']]),
Ct=np.array([[rotor['Ct']] for rotor in geom['rotors']])).T Ct=np.array([[rotor['Ct']] for rotor in geometry['rotors']])).T
return At 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 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 roll torque, pitch torque, yaw torque, x thrust, y thrust, z thrust
''' '''
# Combined torque & thrust matrix # Combined torque & thrust matrix
At = geom_to_thrust_matrix(geom) At = geometry_to_thrust_matrix(geometry)
Am = geom_to_torque_matrix(geom) Am = geometry_to_torque_matrix(geometry)
A = np.vstack([Am, At]) A = np.vstack([Am, At])
# Mix matrix computed as pseudoinverse of A # Mix matrix computed as pseudoinverse of A
@@ -214,7 +214,7 @@ def normalize_mix_px4(B):
return B_px 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 Generate C header file with same format as multi_tables.py
TODO: rewrite using templates (see generation of uORB headers) 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 # Print enum
buf.write(u"enum class MultirotorGeometry : MultirotorGeometryUnderlyingType {\n") buf.write(u"enum class MultirotorGeometry : MultirotorGeometryUnderlyingType {\n")
for i, geom in enumerate(geom_list): for i, geometry in enumerate(geometries_list):
buf.write(u"\t{} = {},\n".format(geom['info']['name'].upper(), i)) buf.write(u"\t{} = {},\n".format(geometry['info']['name'].upper(), i))
buf.write(u"\n\tMAX_GEOMETRY\n") buf.write(u"\n\tMAX_GEOMETRY\n")
buf.write(u"}; // enum class MultirotorGeometry\n\n") buf.write(u"}; // enum class MultirotorGeometry\n\n")
# Print mixer gains # Print mixer gains
buf.write(u"namespace {\n") buf.write(u"namespace {\n")
for geom in geom_list: for geometry in geometries_list:
# Get desired mix matrix # Get desired mix matrix
if use_normalized_mix: if use_normalized_mix:
mix = geom['mix']['B_px'] mix = geometry['mix']['B_px']
else: 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: for row in mix:
if use_6dof: 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") buf.write(u"};\n\n")
# Print geom indeces # Print geometry indeces
buf.write(u"const MultirotorMixer::Rotor *_config_index[] = {\n") buf.write(u"const MultirotorMixer::Rotor *_config_index[] = {\n")
for geom in geom_list: for geometry in geometries_list:
buf.write(u"\t&_config_{}[0],\n".format(geom['info']['name'])) buf.write(u"\t&_config_{}[0],\n".format(geometry['info']['name']))
buf.write(u"};\n\n") buf.write(u"};\n\n")
# Print geom rotor counts # Print geometry rotor counts
buf.write(u"const unsigned _config_rotor_count[] = {\n") buf.write(u"const unsigned _config_rotor_count[] = {\n")
for geom in geom_list: for geometry in geometries_list:
buf.write(u"\t{}, /* {} */\n".format(len(geom['rotors']), geom['info']['name'])) buf.write(u"\t{}, /* {} */\n".format(len(geometry['rotors']), geometry['info']['name']))
buf.write(u"};\n\n") buf.write(u"};\n\n")
# Print geom key # Print geometry key
buf.write(u"const char* _config_key[] = {\n") buf.write(u"const char* _config_key[] = {\n")
for geom in geom_list: for geometry in geometries_list:
buf.write(u"\t\"{}\",\t/* {} */\n".format(geom['info']['key'], geom['info']['name'])) buf.write(u"\t\"{}\",\t/* {} */\n".format(geometry['info']['key'], geometry['info']['name']))
buf.write(u"};\n\n") buf.write(u"};\n\n")
# Print footer # Print footer
@@ -295,9 +295,9 @@ if __name__ == '__main__':
# Parse arguments # Parse arguments
parser = argparse.ArgumentParser( 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', parser.add_argument('-d', dest='dir',
help='directory with geom files') help='directory with geometry files')
parser.add_argument('-f', dest='files', parser.add_argument('-f', dest='files',
help="files to convert (use only without -d)", help="files to convert (use only without -d)",
nargs="+") nargs="+")
@@ -321,29 +321,29 @@ if __name__ == '__main__':
raise Exception("Missing input directory (-d) or list of geometry files (-f)") raise Exception("Missing input directory (-d) or list of geometry files (-f)")
# List of geometries # List of geometries
geom_list = [] geometries_list = []
for filename in filenames: for filename in filenames:
# Parse geom file # Parse geometry file
geom = parse_geom_toml(filename) geometry = parse_geometry_toml(filename)
# Compute torque and thrust matrices # Compute torque and thrust matrices
A, B = geom_to_mix(geom) A, B = geometry_to_mix(geometry)
# Normalize mixer # Normalize mixer
B_px = normalize_mix_px4(B) B_px = normalize_mix_px4(B)
# Store matrices in geom # Store matrices in geometry
geom['mix'] = {'A': A, 'B': B, 'B_px': B_px} geometry['mix'] = {'A': A, 'B': B, 'B_px': B_px}
# Add to list # Add to list
geom_list.append(geom) geometries_list.append(geometry)
if args.verbose: if args.verbose:
print('\nFilename') print('\nFilename')
print(filename) print(filename)
print('\nGeometry') print('\nGeometry')
print(geom) print(geometry)
print('\nA:') print('\nA:')
print(A.round(2)) print(A.round(2))
print('\nB:') print('\nB:')
@@ -353,7 +353,7 @@ if __name__ == '__main__':
print('\n-----------------------------') print('\n-----------------------------')
# Generate header file # Generate header file
header = generate_mixer_multirotor_header(geom_list, header = generate_mixer_multirotor_header(geometries_list,
use_normalized_mix=args.normalize, use_normalized_mix=args.normalize,
use_6dof=args.sixdof) use_6dof=args.sixdof)