PX4多旋翼混控器生成算法

• 1. PX4多旋翼混控器生成
  • 1.1. 构型配置文件
  • 1.2. 混控器头文件生成工具

1. PX4多旋翼混控器生成

PX4多旋翼混控器采用伪逆法生成，在编译过程中根据构型配置文件自动生成混控器头文件，控制律解算控制期望后由混控器程序根据混控器头文件进行控制量分配。

1.1. 构型配置文件

构型配置文件位于 src/lib/mixer/MultirotorMixer/geometries 路径下，采用 .toml 格式语义化配置文件.

示例说明

名称：X构型四旋翼配置文件
示例文件路径：src/lib/mixer/MultirotorMixer/geometries/quad_x.toml
构型图片：

PS:图片引用自PX4用户手册

# X构型四旋翼配置文件
# src/lib/mixer/MultirotorMixer/geometries/quad_x.toml
# 构型图片：https://docs.px4.io/master/assets/airframes/types/QuadRotorX.svg

# Generic Quadcopter in X configuration

# 构型信息（主要提供混控器关键字，配置混控器类型时使用）
[info] # 构型信息参数组
key = "4x" # 构型关键字（在混控配置中作为识别该构型的关键字）
description = "Generic Quadcopter in X configuration" # 构型描述

# 动力单元默认参数，在单个动力单元未设置相关参数时采用
# 主要参数有：转向、转轴向量、升力系数、扭矩系数
[rotor_default] # 动力单元默认参数组
direction = "CW" # 动力单元转向 CW-顺时针 CCW-逆时针
axis      = [0.0, 0.0, -1.0] # 动力单元转轴向量（机体坐标系，Z轴向下）
Ct        = 1.0 # 动力单元升力系数
Cm        = 0.05 # 动力单元扭矩系数

# 动力单元参数
# 根据构型电机数量进行配置，按排列顺序对应输出编号
[[rotors]] # 0号动力单元
name      = "front_right" # 名称
position  = [0.707107, 0.707107, 0.0] # 安装位置，相对于重心（此处为X构型四旋翼，轴距作为未知量默认为1）
direction = "CCW" # 转向-逆时针

[[rotors]] # 1号动力单元
name      = "rear_left"
position  = [-0.707107, -0.707107, 0.0]
direction = "CCW"

[[rotors]] # 2号动力单元
name     = "front_left"
position = [0.707107, -0.707107, 0.0]

[[rotors]] # 3号动力单元
name     = "rear_right"
position = [-0.707107, 0.707107, 0.0]

# 各动力单元中未标明的参数均使用动力单元默认参数

1.2. 混控器头文件生成工具

PX4编译过程中调用混控器头文件生成工具自动根据构型配置文件生成头文件，该工具采用Python编写，主要使用NumPy库。

文件地址：src/lib/mixer/MultirotorMixer/geometries/tools/px_generate_mixers.py

文件主要函数解析

• parse_geometry_toml()
  该函数负责从toml构型配置文件获取相关构型信息，最终返回字典变量geometry

  geometry

  ---

  key	value
  ‘info’	d[‘info’]
  ‘rotors’	rotor_list

  d[‘info’]

  ---

  key	value
  ‘key’	“4x”
  ‘description’	Generic Quadcopter in X configuration
  ‘name’	‘quad_x’ 使用配置文件文件名称（小写） 新增项
  ‘filename’	‘src/lib/mixer/MultirotorMixer/geometries/quad_x.toml’ 文件全路径 新增项

  rotor_list

  ---

  key	value
  ‘name’	“front_right”
  ‘position’	[-0.707107, 0.707107, 0.0]
  ‘axis’	[0.0, 0.0, -1.0]
  ‘direction’	“CCW”
  ‘Ct’	1.0 使用默认值
  ‘Cm’	0.05 使用默认值

  PS：其他动力单元参数组织形式一致，根据配置文件填写各参数值

• geometry_to_mix()
  该函数根据构型生成混控矩阵，返回两个混控相关矩阵A、B
  A矩阵
  $$\left[\begin{array}{cccc}{C}_{Mx:m0}& C_{Mx:m1}& C_{Mx:m2}& C_{Mx:m3}\\ C_{My:m0}& C_{My:m1}& C_{My:m2}& C_{My:m3}\\ C_{Mz:m0}& C_{Mz:m1}& C_{Mz:m2}& C_{Mz:m3}\\ C_{Fx:m0}& C_{Fx:m1}& C_{Fx:m2}& C_{Fx:m3}\\ C_{Fy:m0}& C_{Fy:m1}& C_{Fy:m2}& C_{Fy:m3}\\ C_{Fz:m0}& C_{Fz:m1}& C_{Fz:m2}& C_{Fz:m3}\end{array}\right]$$
  PS:此矩阵左侧乘以 $[{\omega }_0^2{\omega }_1^2{\omega }_2^2{\omega }_3^2{]}^{\prime }$ 电机转速平方即可得到作用于多旋翼体轴的三轴力和三轴力矩（矩阵中各值均根据构型文件提供的相关参数计算而得）

  B矩阵
  $$\left[\begin{array}{cccccc}{E}_{Mx:m0}& E_{My:m0}& E_{Mz:m0}& E_{Fx:m0}& E_{Fy:m0}& E_{Fz:m0}\\ E_{Mx:m1}& E_{My:m1}& E_{Mz:m1}& E_{Fx:m1}& E_{Fy:m1}& E_{Fz:m1}\\ E_{Mx:m2}& E_{My:m2}& E_{Mz:m2}& E_{Fx:m2}& E_{Fy:m2}& E_{Fz:m2}\\ E_{Mx:m3}& E_{My:m3}& E_{Mz:m3}& E_{Fx:m3}& E_{Fy:m3}& E_{Fz:m3}\end{array}\right]$$
  PS:此矩阵左侧乘以 $[M_x{M}_y{M}_z{F}_x{F}_y{F}_z{]}^{\prime }$ 6dof期望的力和力矩即可求得各电机的期望转速平方

  def geometry_to_mix(geometry):
      '''
      Compute combined torque & thrust matrix A and mix matrix B from geometry dictionnary
  
      A is a 6xN matrix where N is the number of rotors
      Each column is the torque and thrust generated by one rotor
  
      B is a Nx6 matrix where N is the number of rotors
      Each column is the command to apply to the servos to get
      roll torque, pitch torque, yaw torque, x thrust, y thrust, z thrust
      '''
      # Combined torque & thrust matrix
      At = geometry_to_thrust_matrix(geometry) # 根据构型生成三轴力矩阵
      Am = geometry_to_torque_matrix(geometry) # 根据构型生成三轴力矩矩阵
      A = np.vstack([Am, At]) # 将三轴力矩阵和三轴力矩矩阵组合，生成六轴6dof驱动矩阵
  
      # Mix matrix computed as pseudoinverse of A
      B = np.linalg.pinv(A) # 生成6dof驱动矩阵的伪逆矩阵
  
      return A, B
  

• geometry_to_thrust_matrix()
  该函数根据构型生成三轴推力矩阵，返回三轴推力矩阵At

  At矩阵
  $$\left[\begin{array}{cccc}{C}_{Fx:m0}& C_{Fx:m1}& C_{Fx:m2}& C_{Fx:m3}\\ C_{Fy:m0}& C_{Fy:m1}& C_{Fy:m2}& C_{Fy:m3}\\ C_{Fz:m0}& C_{Fz:m1}& C_{Fz:m2}& C_{Fz:m3}\end{array}\right]$$

  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 geometry['rotors']]),
                       Ct=np.array([[rotor['Ct']] for rotor in geometry['rotors']])).T
  
    return At
  

• thrust_matrix()
  该函数根据构型生成三轴推力矩阵，返回三轴推力矩阵thrust

  thrust矩阵
  $$\left[\begin{array}{ccc}{C}_{Fx:m0}& C_{Fy:m0}& C_{Fz:m0}\\ C_{Fx:m1}& C_{Fy:m1}& C_{Fz:m1}\\ C_{Fx:m2}& C_{Fy:m2}& C_{Fz:m2}\\ C_{Fx:m3}& C_{Fy:m3}& C_{Fz:m3}\end{array}\right]$$

  def thrust_matrix(axis, Ct):
    '''
    Compute thrust generated by rotors
    '''
    # Normalize rotor axis
    ax = axis / np.linalg.norm(axis, axis=1)[:, np.newaxis] # 轴向量归一化
    thrust = Ct * ax # 使用Ct乘以轴向量，计算得出各动力单元在各轴上的推力分量
    return thrust
  

• geometry_to_torque_matrix()
  该函数根据构型生成三轴力矩矩阵，返回三轴力矩矩阵Am

  Am矩阵
  $$\left[\begin{array}{cccc}{C}_{Mx:m0}& C_{Mx:m1}& C_{Mx:m2}& C_{Mx:m3}\\ C_{My:m0}& C_{My:m1}& C_{My:m2}& C_{My:m3}\\ C_{Mz:m0}& C_{Mz:m1}& C_{Mz:m2}& C_{Mz:m3}\end{array}\right]$$

  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 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 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
  

• torque_matrix()
  该函数根据构型生成三轴力矩矩阵，返回三轴力矩矩阵torque

  torque矩阵
  $$\left[\begin{array}{cccc}{C}_{Mx:m0}& C_{Mx:m1}& C_{Mx:m2}& C_{Mx:m3}\\ C_{My:m0}& C_{My:m1}& C_{My:m2}& C_{My:m3}\\ C_{Mz:m0}& C_{Mz:m1}& C_{Mz:m2}& C_{Mz:m3}\end{array}\right]$$

  def torque_matrix(center, axis, dirs, Ct, Cm):
    '''
    Compute torque generated by rotors
    '''
    # normalize rotor axis
    ax = axis / np.linalg.norm(axis, axis=1)[:, np.newaxis] # 轴向量归一化
    torque = Ct * np.cross(center, ax) - Cm * ax * dirs # 求力矩矩阵
    # 前面部分动力单元位置向量叉乘轴向量，可得该动力单元三轴力矩分量向量（此力矩是由动力单元拉力乘以力臂产生的，所以乘以Ct），后面为航向方向的动力单元反扭力矩（也需要乘以轴向量和转向）
    return torque
  

• normalize_mix_px4()
  该函数对生成的混控矩阵根据px4规则归一化，返回B矩阵的px4形式B_px
  PS:当前代码中进行了注释，该函数仅为考虑兼容性，不建议使用

  B_px矩阵
  $$\left[\begin{array}{cccccc}{E}_{px:Mx:m0}& E_{px:My:m0}& E_{px:Mz:m0}& E_{px:Fx:m0}& E_{px:Fy:m0}& E_{px:Fz:m0}\\ E_{px:Mx:m1}& E_{px:My:m1}& E_{px:Mz:m1}& E_{px:Fx:m1}& E_{px:Fy:m1}& E_{px:Fz:m1}\\ E_{px:Mx:m2}& E_{px:My:m2}& E_{px:Mz:m2}& E_{px:Fx:m2}& E_{px:Fy:m2}& E_{px:Fz:m2}\\ E_{px:Mx:m3}& E_{px:My:m3}& E_{px:Mz:m3}& E_{px:Fx:m3}& E_{px:Fy:m3}& E_{px:Fz:m3}\end{array}\right]$$

  def normalize_mix_px4(B):
    '''
    Normalize mix for PX4
    This is for compatibility only and should ideally not be used
    '''
    B_norm = np.linalg.norm(B, axis=0) #B矩各列范数（按列）
    B_max = np.abs(B).max(axis=0) # B矩阵中取各列中的绝对值最大值（按列）
    B_sum = np.sum(B, axis=0) # B矩阵中各列的和（按列）
  
    # Same scale on roll and pitch
    # 保证构型中的各一半动力单元分别来控制俯仰和滚转，确保在一半动力单元控制量的平方和为1（困惑）
    B_norm[0] = max(B_norm[0], B_norm[1]) / np.sqrt(B.shape[0] / 2.0)
    B_norm[1] = B_norm[0]
  
    # Scale yaw separately
    # 保证偏航控制能力最强的动力单元偏航操纵系数为1，其他电机缩小偏航操纵比例
    B_norm[2] = B_max[2]
  
    # Same scale on x, y
    # 保证x，y方向力控制最强的动力单元操纵系数为1，使其发挥最大作用
    B_norm[3] = max(B_max[3], B_max[4])
    B_norm[4] = B_norm[3]
  
    # Scale z thrust separately
    # 保证Z轴力需求平均分配到能够产生Z轴推力的动力单元上
    B_norm[5] = - B_sum[5] / np.count_nonzero(B[:,5])
  
    # Normalize
    B_norm[np.abs(B_norm) < 1e-3] = 1 # 将B_norm中为0的项置为1，防止除0
    B_px = (B / B_norm) #将B矩阵按照B_norm矩阵进行PX归一化
  
    return B_px
  

• generate_mixer_multirotor_header()
  该函数生成最终的混控器头文件

  参数	说明
  use_normalized_mix	True-使用B_px输出混控头文件 False-使用B输出混控头文件
  use_6dof	True-输出6dof混控头文件，即B或B_px False-输出4dof混控头文件，将6dof中的Fx、Fy列去掉 （目前代码将Fz中添加了负号写进去了，但在6dof中无此操作，代码中写了要修复该处）

附录：完整代码（代码版本：9a96ca1）

#!/usr/bin/env python
#############################################################################
#
#   Copyright (C) 2013-2016 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
#    notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
#    notice, this list of conditions and the following disclaimer in
#    the documentation and/or other materials provided with the
#    distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
#    used to endorse or promote products derived from this software
#    without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
#############################################################################

"""
px_generate_mixers.py
Generates c/cpp header/source files for multirotor mixers
from geometry descriptions files (.toml format)
"""
import sys

try:
    import toml
except ImportError as e:
    print("Failed to import toml: " + str(e))
    print("")
    print("You may need to install it using:")
    print("    pip3 install --user toml")
    print("")
    sys.exit(1)

try:
    import numpy as np
except ImportError as e:
    print("Failed to import numpy: " + str(e))
    print("")
    print("You may need to install it using:")
    print("    pip3 install --user numpy")
    print("")
    sys.exit(1)

__author__ = "Julien Lecoeur"
__copyright__ = "Copyright (C) 2013-2017 PX4 Development Team."
__license__ = "BSD"
__email__ = "julien.lecoeur@gmail.com"


def parse_geometry_toml(filename):
    '''
    Parses toml geometry file and returns a dictionary with curated list of rotors
    '''
    import os

    # Load toml file
    d = toml.load(filename)

    # Check info section
    if 'info' not in d:
        raise AttributeError('{}: Error, missing info section'.format(filename))

    # Check info section
    for field in ['key', 'description']:
        if field not in d['info']:
            raise AttributeError('{}: Error, unspecified info field "{}"'.format(filename, field))

    # Use filename as mixer name
    d['info']['name'] = os.path.basename(filename).split('.')[0].lower()

    # Store filename
    d['info']['filename'] = filename

    # Check default rotor config
    if 'rotor_default' in d:
        default = d['rotor_default']
    else:
        default = {}

    # Convert rotors
    rotor_list = []
    if 'rotors' in d:
        for r in d['rotors']:
            # Make sure all fields are defined, fill missing with default
            for field in ['name', 'position', 'axis', 'direction', 'Ct', 'Cm']:
                if field not in r:
                    if field in default:
                        r[field] = default[field]
                    else:
                        raise AttributeError('{}: Error, unspecified field "{}" for rotor "{}"'
                                             .format(filename, field, r['name']))

            # Check direction field
            r['direction'] = r['direction'].upper()
            if r['direction'] not in ['CW', 'CCW']:
                raise AttributeError('{}: Error, invalid direction value "{}" for rotor "{}"'
                                     .format(filename, r['direction'], r['name']))

            # Check vector3 fields
            for field in ['position', 'axis']:
                if len(r[field]) != 3:
                    raise AttributeError('{}: Error, field "{}" for rotor "{}"'
                                         .format(filename, field, r['name']) +
                                         ' must be an array of length 3')

            # Add rotor to list
            rotor_list.append(r)

    # Clean dictionary
    geometry = {'info': d['info'],
            'rotors': rotor_list}

    return geometry

def torque_matrix(center, axis, dirs, Ct, Cm):
    '''
    Compute torque generated by rotors
    '''
    # normalize rotor axis
    ax = axis / np.linalg.norm(axis, axis=1)[:, np.newaxis]
    torque = Ct * np.cross(center, ax) - Cm * ax * dirs
    return torque

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 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 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):
    '''
    Compute thrust generated by rotors
    '''
    # Normalize rotor axis
    ax = axis / np.linalg.norm(axis, axis=1)[:, np.newaxis]
    thrust = Ct * ax
    return thrust

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 geometry['rotors']]),
                       Ct=np.array([[rotor['Ct']] for rotor in geometry['rotors']])).T

    return At

def geometry_to_mix(geometry):
    '''
    Compute combined torque & thrust matrix A and mix matrix B from geometry dictionnary

    A is a 6xN matrix where N is the number of rotors
    Each column is the torque and thrust generated by one rotor

    B is a Nx6 matrix where N is the number of rotors
    Each column is the command to apply to the servos to get
    roll torque, pitch torque, yaw torque, x thrust, y thrust, z thrust
    '''
    # Combined torque & thrust matrix
    At = geometry_to_thrust_matrix(geometry)
    Am = geometry_to_torque_matrix(geometry)
    A = np.vstack([Am, At])

    # Mix matrix computed as pseudoinverse of A
    B = np.linalg.pinv(A)

    return A, B

def normalize_mix_px4(B):
    '''
    Normalize mix for PX4
    This is for compatibility only and should ideally not be used
    '''
    B_norm = np.linalg.norm(B, axis=0)
    B_max = np.abs(B).max(axis=0)
    B_sum = np.sum(B, axis=0)

    # Same scale on roll and pitch
    B_norm[0] = max(B_norm[0], B_norm[1]) / np.sqrt(B.shape[0] / 2.0)
    B_norm[1] = B_norm[0]

    # Scale yaw separately
    B_norm[2] = B_max[2]

    # Same scale on x, y
    B_norm[3] = max(B_max[3], B_max[4])
    B_norm[4] = B_norm[3]

    # Scale z thrust separately
    B_norm[5] = - B_sum[5] / np.count_nonzero(B[:,5])

    # Normalize
    B_norm[np.abs(B_norm) < 1e-3] = 1
    B_px = (B / B_norm)

    return B_px

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)
    '''
    from io import StringIO
    buf = StringIO()

    # Print Header
    buf.write(u"/*\n")
    buf.write(u"* This file is automatically generated by px_generate_mixers.py - do not edit.\n")
    buf.write(u"*/\n")
    buf.write(u"\n")
    buf.write(u"#ifndef _MIXER_MULTI_TABLES\n")
    buf.write(u"#define _MIXER_MULTI_TABLES\n")
    buf.write(u"\n")

    # Print enum
    buf.write(u"enum class MultirotorGeometry : MultirotorGeometryUnderlyingType {\n")
    for i, geometry in enumerate(geometries_list):
        buf.write(u"\t{},{}// {} (text key {})\n".format(
            geometry['info']['name'].upper(), ' ' * (max(0, 30 - len(geometry['info']['name']))),
            geometry['info']['description'], geometry['info']['key']))
    buf.write(u"\n\tMAX_GEOMETRY\n")
    buf.write(u"}; // enum class MultirotorGeometry\n\n")

    # Print mixer gains
    buf.write(u"namespace {\n")
    for geometry in geometries_list:
        # Get desired mix matrix
        if use_normalized_mix:
            mix = geometry['mix']['B_px']
        else:
            mix = geometry['mix']['B']

        buf.write(u"static constexpr MultirotorMixer::Rotor _config_{}[] {{\n".format(geometry['info']['name']))

        for row in mix:
            if use_6dof:
            # 6dof mixer
                buf.write(u"\t{{ {:9f}, {:9f}, {:9f}, {:9f}, {:9f}, {:9f} }},\n".format(
                    row[0], row[1], row[2],
                    row[3], row[4], row[5]))
            else:
            # 4dof mixer
                buf.write(u"\t{{ {:9f}, {:9f}, {:9f}, {:9f} }},\n".format(
                    row[0], row[1], row[2],
                    -row[5]))  # Upward thrust is positive TODO: to remove this, adapt PX4 to use NED correctly

        buf.write(u"};\n\n")

    # Print geometry indeces
    buf.write(u"static constexpr const MultirotorMixer::Rotor *_config_index[] {\n")
    for geometry in geometries_list:
        buf.write(u"\t&_config_{}[0],\n".format(geometry['info']['name']))
    buf.write(u"};\n\n")

    # Print geometry rotor counts
    buf.write(u"static constexpr unsigned _config_rotor_count[] {\n")
    for geometry in geometries_list:
        buf.write(u"\t{}, /* {} */\n".format(len(geometry['rotors']), geometry['info']['name']))
    buf.write(u"};\n\n")

    # Print geometry key
    buf.write(u"const char* _config_key[] {\n")
    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
    buf.write(u"} // anonymous namespace\n\n")
    buf.write(u"#endif /* _MIXER_MULTI_TABLES */\n\n")

    return buf.getvalue()


if __name__ == '__main__':
    import argparse
    import glob
    import os

    # Parse arguments
    parser = argparse.ArgumentParser(
        description='Convert geometry .toml files to mixer headers')
    parser.add_argument('-d', dest='dir',
                        help='directory with geometry files')
    parser.add_argument('-f', dest='files',
                        help="files to convert (use only without -d)",
                        nargs="+")
    parser.add_argument('-o', dest='outputfile',
                        help='output header file')
    parser.add_argument('--verbose', help='Print details on standard output',
                        action='store_true')
    parser.add_argument('--normalize', help='Use normalized mixers (compatibility mode)',
                        action='store_true')
    parser.add_argument('--sixdof', help='Use 6dof mixers',
                        action='store_true')
    args = parser.parse_args()

    # Find toml files
    if args.files is not None:
        filenames = args.files
    elif args.dir is not None:
        filenames = glob.glob(os.path.join(args.dir, '*.toml'))
    else:
        parser.print_usage()
        raise Exception("Missing input directory (-d) or list of geometry files (-f)")

    # List of geometries
    geometries_list = []

    for filename in filenames:
        # Parse geometry file
        geometry = parse_geometry_toml(filename)

        # Compute torque and thrust matrices
        A, B = geometry_to_mix(geometry)

        # Normalize mixer
        B_px = normalize_mix_px4(B)

        # Store matrices in geometry
        geometry['mix'] = {'A': A, 'B': B, 'B_px': B_px}

        # Add to list
        geometries_list.append(geometry)

        if args.verbose:
            print('\nFilename')
            print(filename)
            print('\nGeometry')
            print(geometry)
            print('\nA:')
            print(A.round(2))
            print('\nB:')
            print(B.round(2))
            print('\nNormalized Mix (as in PX4):')
            print(B_px.round(2))
            print('\n-----------------------------')

    # Check that there are no duplicated mixer names or keys
    for i in range(len(geometries_list)):
        name_i = geometries_list[i]['info']['name']
        key_i = geometries_list[i]['info']['key']

        for j in range(i + 1, len(geometries_list)):
            name_j = geometries_list[j]['info']['name']
            key_j = geometries_list[j]['info']['key']

            # Mixers cannot share the same name
            if name_i == name_j:
                raise ValueError('Duplicated mixer name "{}" in files {} and {}'.format(
                    name_i,
                    geometries_list[i]['info']['filename'],
                    geometries_list[j]['info']['filename']))

            # Mixers cannot share the same key
            if key_i == key_j:
                raise ValueError('Duplicated mixer key "{}" for mixers "{}" and "{}"'.format(
                    key_i, name_i, name_j))

    # Generate header file
    header = generate_mixer_multirotor_header(geometries_list,
                                              use_normalized_mix=args.normalize,
                                              use_6dof=args.sixdof)

    if args.outputfile is not None:
        # Write header file
        with open(args.outputfile, 'w') as fd:
            fd.write(header)
    else:
        # Print to standard output
        print(header)