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构型配置文件获取相关构型信息,最终返回字典变量geometrygeometry
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矩阵
[ C M x : m 0 C M x : m 1 C M x : m 2 C M x : m 3 C M y : m 0 C M y : m 1 C M y : m 2 C M y : m 3 C M z : m 0 C M z : m 1 C M z : m 2 C M z : m 3 C F x : m 0 C F x : m 1 C F x : m 2 C F x : m 3 C F y : m 0 C F y : m 1 C F y : m 2 C F y : m 3 C F z : m 0 C F z : m 1 C F z : m 2 C F z : m 3 ] \left[ \begin{matrix} 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{matrix} \right] ⎣⎢⎢⎢⎢⎢⎢⎡CMx:m0CMy:m0CMz:m0CFx:m0CFy:m0CFz:m0CMx:m1CMy:m1CMz:m1CFx:m1CFy:m1CFz:m1CMx:m2CMy:m2CMz:m2CFx:m2CFy:m2CFz:m2CMx:m3CMy:m3CMz:m3CFx:m3CFy:m3CFz:m3⎦⎥⎥⎥⎥⎥⎥⎤
PS:此矩阵左侧乘以 [ ω 0 2 ω 1 2 ω 2 2 ω 3 2 ] ′ [\omega_0^2\ \omega_1^2\ \omega_2^2\ \omega_3^2]' [ω02 ω12 ω22 ω32]′ 电机转速平方即可得到作用于多旋翼体轴的三轴力和三轴力矩(矩阵中各值均根据构型文件提供的相关参数计算而得)B矩阵
[ E M x : m 0 E M y : m 0 E M z : m 0 E F x : m 0 E F y : m 0 E F z : m 0 E M x : m 1 E M y : m 1 E M z : m 1 E F x : m 1 E F y : m 1 E F z : m 1 E M x : m 2 E M y : m 2 E M z : m 2 E F x : m 2 E F y : m 2 E F z : m 2 E M x : m 3 E M y : m 3 E M z : m 3 E F x : m 3 E F y : m 3 E F z : m 3 ] \left[ \begin{matrix} 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{matrix} \right] ⎣⎢⎢⎡EMx:m0EMx:m1EMx:m2EMx:m3EMy:m0EMy:m1EMy:m2EMy:m3EMz:m0EMz:m1EMz:m2EMz:m3EFx:m0EFx:m1EFx:m2EFx:m3EFy:m0EFy:m1EFy:m2EFy:m3EFz:m0EFz:m1EFz:m2EFz:m3⎦⎥⎥⎤
PS:此矩阵左侧乘以 [ M x M y M z F x F y F z ] ′ [M_x\ M_y\ M_z\ F_x\ F_y\ F_z]' [Mx My Mz Fx Fy Fz]′ 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()
该函数根据构型生成三轴推力矩阵,返回三轴推力矩阵AtAt矩阵
[ C F x : m 0 C F x : m 1 C F x : m 2 C F x : m 3 C F y : m 0 C F y : m 1 C F y : m 2 C F y : m 3 C F z : m 0 C F z : m 1 C F z : m 2 C F z : m 3 ] \left[ \begin{matrix} 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{matrix} \right] ⎣⎡CFx:m0CFy:m0CFz:m0CFx:m1CFy:m1CFz:m1CFx:m2CFy:m2CFz:m2CFx:m3CFy:m3CFz:m3⎦⎤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()
该函数根据构型生成三轴推力矩阵,返回三轴推力矩阵thrustthrust矩阵
[ C F x : m 0 C F y : m 0 C F z : m 0 C F x : m 1 C F y : m 1 C F z : m 1 C F x : m 2 C F y : m 2 C F z : m 2 C F x : m 3 C F y : m 3 C F z : m 3 ] \left[ \begin{matrix} 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{matrix} \right] ⎣⎢⎢⎡CFx:m0CFx:m1CFx:m2CFx:m3CFy:m0CFy:m1CFy:m2CFy:m3CFz:m0CFz:m1CFz:m2CFz:m3⎦⎥⎥⎤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()
该函数根据构型生成三轴力矩矩阵,返回三轴力矩矩阵AmAm矩阵
[ C M x : m 0 C M x : m 1 C M x : m 2 C M x : m 3 C M y : m 0 C M y : m 1 C M y : m 2 C M y : m 3 C M z : m 0 C M z : m 1 C M z : m 2 C M z : m 3 ] \left[ \begin{matrix} 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{matrix} \right] ⎣⎡CMx:m0CMy:m0CMz:m0CMx:m1CMy:m1CMz:m1CMx:m2CMy:m2CMz:m2CMx:m3CMy:m3CMz:m3⎦⎤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()
该函数根据构型生成三轴力矩矩阵,返回三轴力矩矩阵torquetorque矩阵
[ C M x : m 0 C M x : m 1 C M x : m 2 C M x : m 3 C M y : m 0 C M y : m 1 C M y : m 2 C M y : m 3 C M z : m 0 C M z : m 1 C M z : m 2 C M z : m 3 ] \left[ \begin{matrix} 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{matrix} \right] ⎣⎡CMx:m0CMy:m0CMz:m0CMx:m1CMy:m1CMz:m1CMx:m2CMy:m2CMz:m2CMx:m3CMy:m3CMz:m3⎦⎤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矩阵
[ E p x : M x : m 0 E p x : M y : m 0 E p x : M z : m 0 E p x : F x : m 0 E p x : F y : m 0 E p x : F z : m 0 E p x : M x : m 1 E p x : M y : m 1 E p x : M z : m 1 E p x : F x : m 1 E p x : F y : m 1 E p x : F z : m 1 E p x : M x : m 2 E p x : M y : m 2 E p x : M z : m 2 E p x : F x : m 2 E p x : F y : m 2 E p x : F z : m 2 E p x : M x : m 3 E p x : M y : m 3 E p x : M z : m 3 E p x : F x : m 3 E p x : F y : m 3 E p x : F z : m 3 ] \left[ \begin{matrix} 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{matrix} \right] ⎣⎢⎢⎡Epx:Mx:m0Epx:Mx:m1Epx:Mx:m2Epx:Mx:m3Epx:My:m0Epx:My:m1Epx:My:m2Epx:My:m3Epx:Mz:m0Epx:Mz:m1Epx:Mz:m2Epx:Mz:m3Epx:Fx:m0Epx:Fx:m1Epx:Fx:m2Epx:Fx:m3Epx:Fy:m0Epx:Fy:m1Epx:Fy:m2Epx:Fy:m3Epx:Fz:m0Epx:Fz:m1Epx:Fz:m2Epx:Fz:m3⎦⎥⎥⎤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_pxFalse
-输出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)