mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-06-27 11:30:35 +08:00
Move src/module/systemlib/mixer to src/lib/mixer
This commit is contained in:
Julien Lecoeur
committed by
Beat Küng
Beat Küng
parent
d46c37be79
commit
89642a9203
@@ -1,111 +0,0 @@
|
||||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2015 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.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
include_directories(${CMAKE_CURRENT_BINARY_DIR})
|
||||
|
||||
set(geom_files
|
||||
quad_x.toml
|
||||
quad_h.toml
|
||||
quad_plus.toml
|
||||
quad_wide.toml
|
||||
quad_deadcat.toml
|
||||
quad_vtail.toml
|
||||
quad_y.toml
|
||||
quad_x_pusher.toml
|
||||
hex_x.toml
|
||||
hex_plus.toml
|
||||
hex_cox.toml
|
||||
hex_t.toml
|
||||
octa_x.toml
|
||||
octa_plus.toml
|
||||
octa_cox.toml
|
||||
octa_cox_wide.toml
|
||||
twin_engine.toml
|
||||
tri_y.toml
|
||||
dodeca_top_cox.toml
|
||||
dodeca_bottom_cox.toml
|
||||
)
|
||||
|
||||
set(geom_list)
|
||||
foreach(geom_file ${geom_files})
|
||||
list(APPEND geom_list ${CMAKE_CURRENT_SOURCE_DIR}/geoms/${geom_file})
|
||||
endforeach()
|
||||
|
||||
add_custom_command(OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/mixer_multirotor.generated.h
|
||||
COMMAND ${PYTHON_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/geoms/tools/px_generate_mixers.py -f ${geom_list} -o ${CMAKE_CURRENT_BINARY_DIR}/mixer_multirotor.generated.h
|
||||
DEPENDS ${CMAKE_CURRENT_SOURCE_DIR}/geoms/tools/px_generate_mixers.py ${geom_list}
|
||||
)
|
||||
add_custom_target(mixer_gen
|
||||
DEPENDS ${CMAKE_CURRENT_BINARY_DIR}/mixer_multirotor.generated.h
|
||||
DEPENDS ${geom_list})
|
||||
|
||||
add_custom_command(OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/mixer_multirotor_normalized.generated.h
|
||||
COMMAND ${PYTHON_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/geoms/tools/px_generate_mixers.py --normalize -f ${geom_list} -o ${CMAKE_CURRENT_BINARY_DIR}/mixer_multirotor_normalized.generated.h
|
||||
DEPENDS ${CMAKE_CURRENT_SOURCE_DIR}/geoms/tools/px_generate_mixers.py ${geom_list}
|
||||
)
|
||||
add_custom_target(mixer_gen_norm
|
||||
DEPENDS ${CMAKE_CURRENT_BINARY_DIR}/mixer_multirotor_normalized.generated.h
|
||||
DEPENDS ${geom_list})
|
||||
|
||||
add_custom_command(OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/mixer_multirotor_6dof.generated.h
|
||||
COMMAND ${PYTHON_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/geoms/tools/px_generate_mixers.py --sixdof -f ${geom_list} -o ${CMAKE_CURRENT_BINARY_DIR}/mixer_multirotor_6dof.generated.h
|
||||
DEPENDS ${CMAKE_CURRENT_SOURCE_DIR}/geoms/tools/px_generate_mixers.py ${geom_list}
|
||||
)
|
||||
add_custom_target(mixer_gen_6dof
|
||||
DEPENDS ${CMAKE_CURRENT_BINARY_DIR}/mixer_multirotor_6dof.generated.h
|
||||
DEPENDS ${geom_list})
|
||||
|
||||
add_custom_command(OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/mixer_multirotor_legacy.generated.h
|
||||
COMMAND ${PYTHON_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/multi_tables.py > ${CMAKE_CURRENT_BINARY_DIR}/mixer_multirotor_legacy.generated.h
|
||||
DEPENDS ${CMAKE_CURRENT_SOURCE_DIR}/multi_tables.py
|
||||
)
|
||||
add_custom_target(mixer_gen_legacy
|
||||
DEPENDS ${CMAKE_CURRENT_BINARY_DIR}/mixer_multirotor_legacy.generated.h)
|
||||
|
||||
px4_add_module(
|
||||
MODULE modules__systemlib__mixer
|
||||
SRCS
|
||||
mixer.cpp
|
||||
mixer_group.cpp
|
||||
mixer_helicopter.cpp
|
||||
mixer_load.c
|
||||
mixer_multirotor.cpp
|
||||
mixer_simple.cpp
|
||||
DEPENDS
|
||||
platforms__common
|
||||
mixer_gen
|
||||
mixer_gen_norm
|
||||
mixer_gen_6dof
|
||||
mixer_gen_legacy
|
||||
)
|
||||
# vim: set noet ft=cmake fenc=utf-8 ff=unix :
|
||||
@@ -1,41 +0,0 @@
|
||||
# Generic Dodecacopter in X coax configuration, bottom half
|
||||
|
||||
[info]
|
||||
name = "dodeca_bottom_cox"
|
||||
key = "6a"
|
||||
description = "Generic Dodecacopter in X coax configuration, bottom half"
|
||||
|
||||
[rotor_default]
|
||||
axis = [0.0, 0.0, -1.0]
|
||||
Ct = 1.0
|
||||
Cm = 0.05
|
||||
|
||||
[[rotors]]
|
||||
name = "mid_right_bottom"
|
||||
position = [0.0, 1.0, 0.1]
|
||||
direction = "CCW"
|
||||
|
||||
[[rotors]]
|
||||
name = "mid_left_bottom"
|
||||
position = [0.0, -1.0, 0.1]
|
||||
direction = "CW"
|
||||
|
||||
[[rotors]]
|
||||
name = "front_left_bottom"
|
||||
position = [0.866025, -0.5, 0.1]
|
||||
direction = "CCW"
|
||||
|
||||
[[rotors]]
|
||||
name = "rear_right_bottom"
|
||||
position = [-0.866025, 0.5, 0.1]
|
||||
direction = "CW"
|
||||
|
||||
[[rotors]]
|
||||
name = "front_right_bottom"
|
||||
position = [0.866025, 0.5, 0.1]
|
||||
direction = "CW"
|
||||
|
||||
[[rotors]]
|
||||
name = "rear_left_bottom"
|
||||
position = [-0.866025, -0.5, 0.1]
|
||||
direction = "CCW"
|
||||
@@ -1,41 +0,0 @@
|
||||
# Generic Dodecacopter in X coax configuration, top half
|
||||
|
||||
[info]
|
||||
name = "dodeca_top_cox"
|
||||
key = "6m"
|
||||
description = "Generic Dodecacopter in X coax configuration, top half"
|
||||
|
||||
[rotor_default]
|
||||
axis = [0.0, 0.0, -1.0]
|
||||
Ct = 1.0
|
||||
Cm = 0.05
|
||||
|
||||
[[rotors]]
|
||||
name = "mid_right_top"
|
||||
position = [0.0, 1.0, -0.1]
|
||||
direction = "CW"
|
||||
|
||||
[[rotors]]
|
||||
name = "mid_left_top"
|
||||
position = [0.0, -1.0, -0.1]
|
||||
direction = "CCW"
|
||||
|
||||
[[rotors]]
|
||||
name = "front_left_top"
|
||||
position = [0.866025, -0.5, -0.1]
|
||||
direction = "CW"
|
||||
|
||||
[[rotors]]
|
||||
name = "rear_right_top"
|
||||
position = [-0.866025, 0.5, -0.1]
|
||||
direction = "CCW"
|
||||
|
||||
[[rotors]]
|
||||
name = "front_right_top"
|
||||
position = [0.866025, 0.5, -0.1]
|
||||
direction = "CCW"
|
||||
|
||||
[[rotors]]
|
||||
name = "rear_left_top"
|
||||
position = [-0.866025, -0.5, -0.1]
|
||||
direction = "CW"
|
||||
@@ -1,41 +0,0 @@
|
||||
# Generic Hexacopter in coaxial configuration
|
||||
|
||||
[info]
|
||||
name = "hex_cox"
|
||||
key = "6c"
|
||||
description = "Generic Hexacopter in coaxial configuration"
|
||||
|
||||
[rotor_default]
|
||||
axis = [0.0, 0.0, -1.0]
|
||||
Ct = 1.0
|
||||
Cm = 0.05
|
||||
|
||||
[[rotors]]
|
||||
name = "front_right_top"
|
||||
position = [0.5, 0.866, -0.1]
|
||||
direction = "CW"
|
||||
|
||||
[[rotors]]
|
||||
name = "front_right_bottom"
|
||||
position = [0.5, 0.866, 0.1]
|
||||
direction = "CCW"
|
||||
|
||||
[[rotors]]
|
||||
name = "rear_top"
|
||||
position = [-1.0, 0.0, -0.1]
|
||||
direction = "CW"
|
||||
|
||||
[[rotors]]
|
||||
name = "rear_bottom"
|
||||
position = [-1.0, 0.0, 0.1]
|
||||
direction = "CCW"
|
||||
|
||||
[[rotors]]
|
||||
name = "front_left_top"
|
||||
position = [0.5, -0.866, -0.1]
|
||||
direction = "CW"
|
||||
|
||||
[[rotors]]
|
||||
name = "front_left_bottom"
|
||||
position = [0.5, -0.866, 0.1]
|
||||
direction = "CCW"
|
||||
@@ -1,41 +0,0 @@
|
||||
# Generic Hexacopter in + configuration
|
||||
|
||||
[info]
|
||||
name = "hex_plus"
|
||||
key = "6+"
|
||||
description = "Generic Hexacopter in + configuration"
|
||||
|
||||
[rotor_default]
|
||||
axis = [0.0, 0.0, -1.0]
|
||||
Ct = 1.0
|
||||
Cm = 0.05
|
||||
|
||||
[[rotors]]
|
||||
name = "front"
|
||||
position = [1.0, 0.0, 0.0]
|
||||
direction = "CW"
|
||||
|
||||
[[rotors]]
|
||||
name = "rear"
|
||||
position = [-1.0, 0.0, 0.0]
|
||||
direction = "CCW"
|
||||
|
||||
[[rotors]]
|
||||
name = "rear_left"
|
||||
position = [-0.5, -0.866025, 0.0]
|
||||
direction = "CW"
|
||||
|
||||
[[rotors]]
|
||||
name = "front_right"
|
||||
position = [0.5, 0.866025, 0.0]
|
||||
direction = "CCW"
|
||||
|
||||
[[rotors]]
|
||||
name = "front_left"
|
||||
position = [0.5, -0.866025, 0.0]
|
||||
direction = "CCW"
|
||||
|
||||
[[rotors]]
|
||||
name = "rear_right"
|
||||
position = [-0.5, 0.866025, 0.0]
|
||||
direction = "CW"
|
||||
@@ -1,41 +0,0 @@
|
||||
# Generic Hexacopter in T configuration
|
||||
|
||||
[info]
|
||||
name = "hex_t"
|
||||
key = "6t"
|
||||
description = "Generic Hexacopter in T configuration"
|
||||
|
||||
[rotor_default]
|
||||
axis = [0.0, 0.0, -1.0]
|
||||
Ct = 1.0
|
||||
Cm = 0.05
|
||||
|
||||
[[rotors]]
|
||||
name = "front_right_top"
|
||||
position = [0.729, 0.684, 0.1]
|
||||
direction = "CW"
|
||||
|
||||
[[rotors]]
|
||||
name = "front_right_bottom"
|
||||
position = [0.729, 0.684, -0.1]
|
||||
direction = "CCW"
|
||||
|
||||
[[rotors]]
|
||||
name = "rear_top"
|
||||
position = [-1.0, 0.0, 0.1]
|
||||
direction = "CW"
|
||||
|
||||
[[rotors]]
|
||||
name = "rear_bottom"
|
||||
position = [-1.0, 0.0, -0.1]
|
||||
direction = "CCW"
|
||||
|
||||
[[rotors]]
|
||||
name = "front_left_top"
|
||||
position = [0.729, -0.684, 0.1]
|
||||
direction = "CW"
|
||||
|
||||
[[rotors]]
|
||||
name = "front_left_bottom"
|
||||
position = [0.729, -0.684, -0.1]
|
||||
direction = "CCW"
|
||||
@@ -1,41 +0,0 @@
|
||||
# Generic Hexacopter in X configuration
|
||||
|
||||
[info]
|
||||
name = "hex_x"
|
||||
key = "6x"
|
||||
description = "Generic Hexacopter in X configuration"
|
||||
|
||||
[rotor_default]
|
||||
axis = [0.0, 0.0, -1.0]
|
||||
Ct = 1.0
|
||||
Cm = 0.05
|
||||
|
||||
[[rotors]]
|
||||
name = "mid_right"
|
||||
position = [0.0, 1.0, 0.0]
|
||||
direction = "CW"
|
||||
|
||||
[[rotors]]
|
||||
name = "mid_left"
|
||||
position = [0.0, -1.0, 0.0]
|
||||
direction = "CCW"
|
||||
|
||||
[[rotors]]
|
||||
name = "front_left"
|
||||
position = [0.866025, -0.5, 0.0]
|
||||
direction = "CW"
|
||||
|
||||
[[rotors]]
|
||||
name = "rear_right"
|
||||
position = [-0.866025, 0.5, 0.0]
|
||||
direction = "CCW"
|
||||
|
||||
[[rotors]]
|
||||
name = "front_right"
|
||||
position = [0.866025, 0.5, 0.0]
|
||||
direction = "CCW"
|
||||
|
||||
[[rotors]]
|
||||
name = "rear_left"
|
||||
position = [-0.866025, -0.5, 0.0]
|
||||
direction = "CW"
|
||||
@@ -1,51 +0,0 @@
|
||||
# Generic Octacopter in coax configuration
|
||||
|
||||
[info]
|
||||
name = "octa_cox"
|
||||
key = "8c"
|
||||
description = "GenericOctacopter in coax configuration"
|
||||
|
||||
[rotor_default]
|
||||
axis = [0.0, 0.0, -1.0]
|
||||
Ct = 1.0
|
||||
Cm = 0.05
|
||||
|
||||
[[rotors]]
|
||||
name = "front_right_top"
|
||||
position = [0.707107, 0.707107, -0.1]
|
||||
direction = "CCW"
|
||||
|
||||
[[rotors]]
|
||||
name = "front_left_top"
|
||||
position = [0.707107, -0.707107, -0.1]
|
||||
direction = "CW"
|
||||
|
||||
[[rotors]]
|
||||
name = "rear_left_top"
|
||||
position = [-0.707107, -0.707107, -0.1]
|
||||
direction = "CCW"
|
||||
|
||||
[[rotors]]
|
||||
name = "rear_right_top"
|
||||
position = [-0.707107, 0.707107, -0.1]
|
||||
direction = "CW"
|
||||
|
||||
[[rotors]]
|
||||
name = "front_left_bottom"
|
||||
position = [0.707107, -0.707107, 0.1]
|
||||
direction = "CCW"
|
||||
|
||||
[[rotors]]
|
||||
name = "front_right_bottom"
|
||||
position = [0.707107, 0.707107, 0.1]
|
||||
direction = "CW"
|
||||
|
||||
[[rotors]]
|
||||
name = "rear_right_bottom"
|
||||
position = [-0.707107, 0.707107, 0.1]
|
||||
direction = "CCW"
|
||||
|
||||
[[rotors]]
|
||||
name = "rear_left_bottom"
|
||||
position = [-0.707107, -0.707107, 0.1]
|
||||
direction = "CW"
|
||||
@@ -1,51 +0,0 @@
|
||||
# Generic Octacopter in wide coax configuration
|
||||
|
||||
[info]
|
||||
name = "octa_cox_wide"
|
||||
key = "8cw"
|
||||
description = "Generic Octacopter in wide coax configuration"
|
||||
|
||||
[rotor_default]
|
||||
axis = [0.0, 0.0, -1.0]
|
||||
Ct = 1.0
|
||||
Cm = 0.05
|
||||
|
||||
[[rotors]]
|
||||
name = "front_right_top"
|
||||
position = [0.3746066, 0.927184, -0.1]
|
||||
direction = "CCW"
|
||||
|
||||
[[rotors]]
|
||||
name = "front_left_top"
|
||||
position = [0.3746066, -0.927184, -0.1]
|
||||
direction = "CW"
|
||||
|
||||
[[rotors]]
|
||||
name = "rear_left_top"
|
||||
position = [-0.62932, -0.777146, -0.1]
|
||||
direction = "CCW"
|
||||
|
||||
[[rotors]]
|
||||
name = "rear_right_top"
|
||||
position = [-0.62932, 0.777146, -0.1]
|
||||
direction = "CW"
|
||||
|
||||
[[rotors]]
|
||||
name = "front_left_bottom"
|
||||
position = [0.3746066, -0.927184, 0.1]
|
||||
direction = "CCW"
|
||||
|
||||
[[rotors]]
|
||||
name = "front_right_bottom"
|
||||
position = [0.3746066, 0.927184, 0.1]
|
||||
direction = "CW"
|
||||
|
||||
[[rotors]]
|
||||
name = "rear_right_bottom"
|
||||
position = [-0.62932, 0.777146, 0.1]
|
||||
direction = "CCW"
|
||||
|
||||
[[rotors]]
|
||||
name = "rear_left_bottom"
|
||||
position = [-0.62932, -0.777146, 0.1]
|
||||
direction = "CW"
|
||||
@@ -1,51 +0,0 @@
|
||||
# Generic Octacopter in + configuration
|
||||
|
||||
[info]
|
||||
name = "octa_plus"
|
||||
key = "8+"
|
||||
description = "Generic Octacopter in + configuration"
|
||||
|
||||
[rotor_default]
|
||||
axis = [0.0, 0.0, -1.0]
|
||||
Ct = 1.0
|
||||
Cm = 0.05
|
||||
|
||||
[[rotors]]
|
||||
name = "front"
|
||||
position = [1.0, 0.0, 0.0]
|
||||
direction = "CW"
|
||||
|
||||
[[rotors]]
|
||||
name = "rear"
|
||||
position = [-1.0, 0.0, 0.0]
|
||||
direction = "CW"
|
||||
|
||||
[[rotors]]
|
||||
name = "front_right"
|
||||
position = [0.707107, 0.707107, 0.0]
|
||||
direction = "CCW"
|
||||
|
||||
[[rotors]]
|
||||
name = "rear_right"
|
||||
position = [-0.707107, 0.707107, 0.0]
|
||||
direction = "CCW"
|
||||
|
||||
[[rotors]]
|
||||
name = "front_left"
|
||||
position = [0.707107, -0.707107, 0.0]
|
||||
direction = "CCW"
|
||||
|
||||
[[rotors]]
|
||||
name = "rear_left"
|
||||
position = [-0.707107, -0.707107, 0.0]
|
||||
direction = "CCW"
|
||||
|
||||
[[rotors]]
|
||||
name = "left"
|
||||
position = [0.0, -1.0, 0.0]
|
||||
direction = "CW"
|
||||
|
||||
[[rotors]]
|
||||
name = "right"
|
||||
position = [0.0, 1.0, 0.0]
|
||||
direction = "CW"
|
||||
@@ -1,51 +0,0 @@
|
||||
# Generic Octacopter in X configuration
|
||||
|
||||
[info]
|
||||
name = "octa_x"
|
||||
key = "8x"
|
||||
description = "Generic Octacopter in X configuration"
|
||||
|
||||
[rotor_default]
|
||||
axis = [0.0, 0.0, -1.0]
|
||||
Ct = 1.0
|
||||
Cm = 0.05
|
||||
|
||||
[[rotors]]
|
||||
name = "front_right"
|
||||
position = [0.9238795, 0.3826834, 0.0]
|
||||
direction = "CW"
|
||||
|
||||
[[rotors]]
|
||||
name = "rear_left"
|
||||
position = [-0.9238795, -0.3826834, 0.0]
|
||||
direction = "CW"
|
||||
|
||||
[[rotors]]
|
||||
name = "mid_front_right"
|
||||
position = [0.3826834, 0.9238795, 0.0]
|
||||
direction = "CCW"
|
||||
|
||||
[[rotors]]
|
||||
name = "rear_right"
|
||||
position = [-0.9238795, 0.3826834, 0.0]
|
||||
direction = "CCW"
|
||||
|
||||
[[rotors]]
|
||||
name = "front_left"
|
||||
position = [0.9238795, -0.3826834, 0.0]
|
||||
direction = "CCW"
|
||||
|
||||
[[rotors]]
|
||||
name = "mid_rear_left"
|
||||
position = [-0.3826834, -0.9238795, 0.0]
|
||||
direction = "CCW"
|
||||
|
||||
[[rotors]]
|
||||
name = "mid_front_left"
|
||||
position = [0.3826834, -0.9238795, 0.0]
|
||||
direction = "CW"
|
||||
|
||||
[[rotors]]
|
||||
name = "mid_rear_right"
|
||||
position = [-0.3826834, 0.9238795, 0.0]
|
||||
direction = "CW"
|
||||
@@ -1,31 +0,0 @@
|
||||
# SK450 DeadCat Quadcopter.
|
||||
# Same geometry as quad_wide, except CG is located at intersection of rear arms, so front motors are more loaded.
|
||||
|
||||
[info]
|
||||
name = "quad_deadcat"
|
||||
key = "4dc"
|
||||
description = "SK450 DeadCat Quadcopter, CG at intersection of rear arms"
|
||||
|
||||
[rotor_default]
|
||||
direction = "CW"
|
||||
axis = [0.0, 0.0, -1.0]
|
||||
Ct = 1.0
|
||||
Cm = 0.01
|
||||
|
||||
[[rotors]]
|
||||
name = "front_right"
|
||||
position = [0.1155, 0.245, 0.0]
|
||||
direction = "CCW"
|
||||
|
||||
[[rotors]]
|
||||
name = "rear_left"
|
||||
position = [-0.1875, -0.1875, 0.0]
|
||||
direction = "CCW"
|
||||
|
||||
[[rotors]]
|
||||
name = "front_left"
|
||||
position = [0.1155, -0.245, 0.0]
|
||||
|
||||
[[rotors]]
|
||||
name = "rear_right"
|
||||
position = [-0.1875, 0.1875, 0.0]
|
||||
@@ -1,31 +0,0 @@
|
||||
# Generic Quadcopter in H configuration
|
||||
|
||||
[info]
|
||||
name = "quad_h"
|
||||
key = "4h"
|
||||
description = "Generic Quadcopter in H configuration"
|
||||
|
||||
[rotor_default]
|
||||
axis = [0.0, 0.0, -1.0]
|
||||
Ct = 1.0
|
||||
Cm = 0.05
|
||||
|
||||
[[rotors]]
|
||||
name = "front_right"
|
||||
position = [0.707107, 0.707107, 0.0]
|
||||
direction = "CW"
|
||||
|
||||
[[rotors]]
|
||||
name = "rear_left"
|
||||
position = [-0.707107, -0.707107, 0.0]
|
||||
direction = "CW"
|
||||
|
||||
[[rotors]]
|
||||
name = "front_left"
|
||||
position = [0.707107, -0.707107, 0.0]
|
||||
direction = "CCW"
|
||||
|
||||
[[rotors]]
|
||||
name = "rear_right"
|
||||
position = [-0.707107, 0.707107, 0.0]
|
||||
direction = "CCW"
|
||||
@@ -1,30 +0,0 @@
|
||||
# Generic Quadcopter in + configuration
|
||||
|
||||
[info]
|
||||
name = "quad_plus"
|
||||
key = "4+"
|
||||
description = "Generic Quadcopter in + configuration"
|
||||
|
||||
[rotor_default]
|
||||
direction = "CW"
|
||||
axis = [0.0, 0.0, -1.0]
|
||||
Ct = 1.0
|
||||
Cm = 0.05
|
||||
|
||||
[[rotors]]
|
||||
name = "right"
|
||||
position = [0.0, 1.0, 0.0]
|
||||
direction = "CCW"
|
||||
|
||||
[[rotors]]
|
||||
name = "left"
|
||||
position = [0.0, -1.0, 0.0]
|
||||
direction = "CCW"
|
||||
|
||||
[[rotors]]
|
||||
name = "front"
|
||||
position = [1.0, 0.0, 0.0]
|
||||
|
||||
[[rotors]]
|
||||
name = "rear_right"
|
||||
position = [-1.0, 0.0, 0.0]
|
||||
@@ -1,34 +0,0 @@
|
||||
# Quadcopter in Y configuration with rear props tilted at 45 degrees
|
||||
|
||||
[info]
|
||||
name = "quad_vtail"
|
||||
key = "4vt"
|
||||
description = "Quadcopter in Y configuration with rear props tilted at 45 degrees"
|
||||
|
||||
[rotor_default]
|
||||
direction = "CW"
|
||||
axis = [0.0, 0.0, -1.0]
|
||||
Ct = 1.0
|
||||
Cm = 0.05
|
||||
|
||||
[[rotors]]
|
||||
name = "front_right"
|
||||
position = [0.2, 0.2, 0.0]
|
||||
direction = "CW"
|
||||
|
||||
[[rotors]]
|
||||
name = "rear_left"
|
||||
position = [-0.3, -0.1, -0.1]
|
||||
axis = [0.0, 0.707106, -0.707106]
|
||||
direction = "CW"
|
||||
|
||||
[[rotors]]
|
||||
name = "front_left"
|
||||
position = [0.2, -0.2, 0.0]
|
||||
direction = "CCW"
|
||||
|
||||
[[rotors]]
|
||||
name = "rear_right"
|
||||
position = [-0.3, 0.1, -0.1]
|
||||
axis = [0.0, -0.707106, -0.707106]
|
||||
direction = "CCW"
|
||||
@@ -1,31 +0,0 @@
|
||||
# Generic Quadcopter in wide configuration
|
||||
|
||||
[info]
|
||||
name = "quad_wide"
|
||||
key = "4w"
|
||||
description = "Quadcopter in wide configuration. Same geometry as SK450 Deadcat except the CG is moved backward to load all motors equally"
|
||||
|
||||
[rotor_default]
|
||||
axis = [0.0, 0.0, -1.0]
|
||||
Ct = 1.0
|
||||
Cm = 0.05
|
||||
|
||||
[[rotors]]
|
||||
name = "front_right"
|
||||
position = [0.1515, 0.245, 0.0]
|
||||
direction = "CCW"
|
||||
|
||||
[[rotors]]
|
||||
name = "rear_left"
|
||||
position = [-0.1515, -0.1875, 0.0]
|
||||
direction = "CCW"
|
||||
|
||||
[[rotors]]
|
||||
name = "front_left"
|
||||
position = [0.1515, -0.245, 0.0]
|
||||
direction = "CW"
|
||||
|
||||
[[rotors]]
|
||||
name = "rear_right"
|
||||
position = [-0.1515, 0.1875, 0.0]
|
||||
direction = "CW"
|
||||
@@ -1,30 +0,0 @@
|
||||
# Generic Quadcopter in X configuration
|
||||
|
||||
[info]
|
||||
name = "quad_x"
|
||||
key = "4x"
|
||||
description = "Generic Quadcopter in X configuration"
|
||||
|
||||
[rotor_default]
|
||||
direction = "CW"
|
||||
axis = [0.0, 0.0, -1.0]
|
||||
Ct = 1.0
|
||||
Cm = 0.05
|
||||
|
||||
[[rotors]]
|
||||
name = "front_right"
|
||||
position = [0.707107, 0.707107]#, 0.0]
|
||||
direction = "CCW"
|
||||
|
||||
[[rotors]]
|
||||
name = "rear_left"
|
||||
position = [-0.707107, -0.707107, 0.0]
|
||||
direction = "CCW"
|
||||
|
||||
[[rotors]]
|
||||
name = "front_left"
|
||||
position = [0.707107, -0.707107, 0.0]
|
||||
|
||||
[[rotors]]
|
||||
name = "rear_right"
|
||||
position = [-0.707107, 0.707107, 0.0]
|
||||
@@ -1,38 +0,0 @@
|
||||
# Quadcopter in X configuration,
|
||||
# with added pusher motor in the back
|
||||
|
||||
[info]
|
||||
name = "quad_x_pusher"
|
||||
key = "4x1p"
|
||||
description = "Quadcopter in X configuration, with added pusher motor in the back"
|
||||
|
||||
[rotor_default]
|
||||
direction = "CW"
|
||||
axis = [0.0, 0.0, -1.0]
|
||||
Ct = 1.0
|
||||
Cm = 0.05
|
||||
|
||||
[[rotors]]
|
||||
name = "front_right"
|
||||
position = [1.0, 1.0, 0.0]
|
||||
direction = "CCW"
|
||||
|
||||
[[rotors]]
|
||||
name = "rear_left"
|
||||
position = [-1.0, -1.0, 0.0]
|
||||
direction = "CCW"
|
||||
|
||||
[[rotors]]
|
||||
name = "front_left"
|
||||
position = [1.0, -1.0, 0.0]
|
||||
|
||||
[[rotors]]
|
||||
name = "rear_right"
|
||||
position = [-1.0, 1.0, 0.0]
|
||||
|
||||
[[rotors]]
|
||||
name = "pusher"
|
||||
position = [-1.0, 0.0, 0.0]
|
||||
axis = [1.0, 0.0, 0.0]
|
||||
Ct = 2.0
|
||||
Cm = 0.0
|
||||
@@ -1,32 +0,0 @@
|
||||
# Quadcopter in Y configuration with coax rear props
|
||||
|
||||
[info]
|
||||
name = "quad_y"
|
||||
key = "4y"
|
||||
description = "Quadcopter in Y configuration with coax rear props"
|
||||
|
||||
[rotor_default]
|
||||
direction = "CW"
|
||||
axis = [0.0, 0.0, -1.0]
|
||||
Ct = 1.0
|
||||
Cm = 0.05
|
||||
|
||||
[[rotors]]
|
||||
name = "front_right"
|
||||
position = [0.2, 0.2, 0.0]
|
||||
direction = "CCW"
|
||||
|
||||
[[rotors]]
|
||||
name = "rear_top"
|
||||
position = [-0.2, 0.0, -0.1]
|
||||
direction = "CCW"
|
||||
|
||||
[[rotors]]
|
||||
name = "front_left"
|
||||
position = [0.2, -0.2, 0.0]
|
||||
direction = "CW"
|
||||
|
||||
[[rotors]]
|
||||
name = "rear_bottom"
|
||||
position = [-0.2, 0.0, 0.1]
|
||||
direction = "CW"
|
||||
@@ -1,356 +0,0 @@
|
||||
#!/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)
|
||||
"""
|
||||
|
||||
try:
|
||||
import toml
|
||||
import numpy as np
|
||||
except ImportError as e:
|
||||
print("python import error: ", e)
|
||||
print('''
|
||||
Required python packages not installed.
|
||||
|
||||
On a Debian/Ubuntu system please run:
|
||||
|
||||
sudo apt-get install python-toml python-numpy
|
||||
|
||||
On MacOS please run:
|
||||
sudo pip install numpy toml
|
||||
|
||||
On Windows please run:
|
||||
easy_install numpy toml
|
||||
''')
|
||||
exit(1)
|
||||
|
||||
__author__ = "Julien Lecoeur"
|
||||
__copyright__ = "Copyright (C) 2013-2017 PX4 Development Team."
|
||||
__license__ = "BSD"
|
||||
__email__ = "julien.lecoeur@gmail.com"
|
||||
|
||||
def parse_geom_toml(filename):
|
||||
'''
|
||||
Parses toml geometry file and returns a dictionary with curated list of rotors
|
||||
'''
|
||||
|
||||
# Load toml file
|
||||
d = toml.load(filename)
|
||||
|
||||
# Check default rotor config
|
||||
if 'rotor_default' in d:
|
||||
default = d['rotor_default']
|
||||
else:
|
||||
default = {}
|
||||
|
||||
# Check info section
|
||||
if 'info' not in d:
|
||||
raise AttributeError('{}: Error, missing info section'.format(filename))
|
||||
|
||||
# Check info section
|
||||
for field in ['name', 'key', 'description']:
|
||||
if field not in d['info']:
|
||||
raise AttributeError('{}: Error, unspecified info field "{}"'.format(filename, field))
|
||||
|
||||
|
||||
# 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
|
||||
geom = {'info': d['info'],
|
||||
'rotors': rotor_list}
|
||||
|
||||
return geom
|
||||
|
||||
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 geom_to_torque_matrix(geom):
|
||||
'''
|
||||
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']]),
|
||||
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
|
||||
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 geom_to_thrust_matrix(geom):
|
||||
'''
|
||||
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
|
||||
|
||||
return At
|
||||
|
||||
def geom_to_mix(geom):
|
||||
'''
|
||||
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 = geom_to_thrust_matrix(geom)
|
||||
Am = geom_to_torque_matrix(geom)
|
||||
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)
|
||||
|
||||
# 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_max[5]
|
||||
|
||||
# Normalize
|
||||
B_norm[np.abs(B_norm) < 1e-3] = 1
|
||||
B_px = (B / B_norm)
|
||||
|
||||
return B_px
|
||||
|
||||
def generate_mixer_multirotor_header(geom_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, geom in enumerate(geom_list):
|
||||
buf.write(u"\t{} = {},\n".format(geom['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:
|
||||
# Get desired mix matrix
|
||||
if use_normalized_mix:
|
||||
mix = geom['mix']['B_px']
|
||||
else:
|
||||
mix = geom['mix']['B']
|
||||
|
||||
buf.write(u"const MultirotorMixer::Rotor _config_{}[] = {{\n".format(geom['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 geom 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']))
|
||||
buf.write(u"};\n\n")
|
||||
|
||||
# Print geom 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']))
|
||||
buf.write(u"};\n\n")
|
||||
|
||||
# Print geom 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']))
|
||||
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
|
||||
|
||||
# Parse arguments
|
||||
parser = argparse.ArgumentParser(
|
||||
description='Convert geom .toml files to mixer headers')
|
||||
parser.add_argument('-d', dest='dir',
|
||||
help='directory with geom 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('--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
|
||||
else:
|
||||
filenames = glob.glob(args.dir + '/*.toml')
|
||||
|
||||
# List of geometries
|
||||
geom_list = []
|
||||
|
||||
for filename in filenames:
|
||||
# Parse geom file
|
||||
geom = parse_geom_toml(filename)
|
||||
|
||||
# Compute torque and thrust matrices
|
||||
A, B = geom_to_mix(geom)
|
||||
|
||||
# Normalize mixer
|
||||
B_px = normalize_mix_px4(B)
|
||||
|
||||
# Store matrices in geom
|
||||
geom['mix'] = {'A': A, 'B': B, 'B_px': B_px}
|
||||
|
||||
# Add to list
|
||||
geom_list.append(geom)
|
||||
|
||||
# print('\nFilename')
|
||||
# print(filename)
|
||||
# print('\nGeometry')
|
||||
# print(geom)
|
||||
# print('\nA:')
|
||||
# print(A.round(2))
|
||||
# print('\nB:')
|
||||
# print(B.round(2))
|
||||
# print('\nNormalized Mix (as in PX4):')
|
||||
# print(B_px)
|
||||
# print('\n-----------------------------')
|
||||
|
||||
# Generate header file
|
||||
header = generate_mixer_multirotor_header(geom_list,
|
||||
use_normalized_mix=args.normalize,
|
||||
use_6dof=args.sixdof)
|
||||
# print(header)
|
||||
|
||||
# Write header file
|
||||
with open(args.outputfile, 'w') as fd:
|
||||
fd.write(header)
|
||||
@@ -1,24 +0,0 @@
|
||||
# Tri Y
|
||||
|
||||
[info]
|
||||
name = "tri_y"
|
||||
key = "3y"
|
||||
description = "Tri Y"
|
||||
|
||||
[rotor_default]
|
||||
axis = [0.0, 0.0, -1.0]
|
||||
Ct = 1.0
|
||||
Cm = 0.0
|
||||
direction = "CW"
|
||||
|
||||
[[rotors]]
|
||||
name = "front_right"
|
||||
position = [0.5, 0.866025, 0.0]
|
||||
|
||||
[[rotors]]
|
||||
name = "front_left"
|
||||
position = [0.5, -0.866025, 0.0]
|
||||
|
||||
[[rotors]]
|
||||
name = "rear"
|
||||
position = [-1.0, 0.0, 0.0]
|
||||
@@ -1,20 +0,0 @@
|
||||
# Twin engine
|
||||
|
||||
[info]
|
||||
name = "twin_engine"
|
||||
key = "2-"
|
||||
description = "Twin engine"
|
||||
|
||||
[rotor_default]
|
||||
axis = [0.0, 0.0, -1.0]
|
||||
Ct = 1.0
|
||||
Cm = 0.0
|
||||
direction = "CW"
|
||||
|
||||
[[rotors]]
|
||||
name = "right"
|
||||
position = [0.0, 1.0, 0.0]
|
||||
|
||||
[[rotors]]
|
||||
name = "left"
|
||||
position = [0.0, -1.0, 0.0]
|
||||
@@ -1,225 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file mixer.cpp
|
||||
*
|
||||
* Programmable multi-channel mixer library.
|
||||
*/
|
||||
|
||||
#include <px4_config.h>
|
||||
|
||||
#include <sys/types.h>
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <fcntl.h>
|
||||
#include <poll.h>
|
||||
#include <errno.h>
|
||||
#include <stdio.h>
|
||||
#include <math.h>
|
||||
#include <unistd.h>
|
||||
#include <ctype.h>
|
||||
#include <systemlib/err.h>
|
||||
|
||||
#include "mixer.h"
|
||||
|
||||
#define debug(fmt, args...) do { } while(0)
|
||||
//#define debug(fmt, args...) do { printf("[mixer] " fmt "\n", ##args); } while(0)
|
||||
|
||||
Mixer::Mixer(ControlCallback control_cb, uintptr_t cb_handle) :
|
||||
_next(nullptr),
|
||||
_control_cb(control_cb),
|
||||
_cb_handle(cb_handle)
|
||||
{
|
||||
}
|
||||
|
||||
float
|
||||
Mixer::get_control(uint8_t group, uint8_t index)
|
||||
{
|
||||
float value;
|
||||
|
||||
_control_cb(_cb_handle, group, index, value);
|
||||
|
||||
return value;
|
||||
}
|
||||
|
||||
|
||||
float
|
||||
Mixer::scale(const mixer_scaler_s &scaler, float input)
|
||||
{
|
||||
float output;
|
||||
|
||||
if (input < 0.0f) {
|
||||
output = (input * scaler.negative_scale) + scaler.offset;
|
||||
|
||||
} else {
|
||||
output = (input * scaler.positive_scale) + scaler.offset;
|
||||
}
|
||||
|
||||
if (output > scaler.max_output) {
|
||||
output = scaler.max_output;
|
||||
|
||||
} else if (output < scaler.min_output) {
|
||||
output = scaler.min_output;
|
||||
}
|
||||
|
||||
return output;
|
||||
}
|
||||
|
||||
int
|
||||
Mixer::scale_check(struct mixer_scaler_s &scaler)
|
||||
{
|
||||
if (scaler.offset > 1.001f) {
|
||||
return 1;
|
||||
}
|
||||
|
||||
if (scaler.offset < -1.001f) {
|
||||
return 2;
|
||||
}
|
||||
|
||||
if (scaler.min_output > scaler.max_output) {
|
||||
return 3;
|
||||
}
|
||||
|
||||
if (scaler.min_output < -1.001f) {
|
||||
return 4;
|
||||
}
|
||||
|
||||
if (scaler.max_output > 1.001f) {
|
||||
return 5;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
const char *
|
||||
Mixer::findtag(const char *buf, unsigned &buflen, char tag)
|
||||
{
|
||||
while (buflen >= 2) {
|
||||
if ((buf[0] == tag) && (buf[1] == ':')) {
|
||||
return buf;
|
||||
}
|
||||
|
||||
buf++;
|
||||
buflen--;
|
||||
}
|
||||
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
const char *
|
||||
Mixer::skipline(const char *buf, unsigned &buflen)
|
||||
{
|
||||
const char *p;
|
||||
|
||||
/* if we can find a CR or NL in the buffer, skip up to it */
|
||||
if ((p = (const char *)memchr(buf, '\r', buflen)) || (p = (const char *)memchr(buf, '\n', buflen))) {
|
||||
/* skip up to it AND one beyond - could be on the NUL symbol now */
|
||||
buflen -= (p - buf) + 1;
|
||||
return p + 1;
|
||||
}
|
||||
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
bool
|
||||
Mixer::string_well_formed(const char *buf, unsigned &buflen)
|
||||
{
|
||||
/* enforce that the mixer ends with a new line */
|
||||
for (int i = buflen - 1; i >= 0; i--) {
|
||||
if (buf[i] == '\0') {
|
||||
continue;
|
||||
}
|
||||
|
||||
/* require a space or newline at the end of the buffer, fail on printable chars */
|
||||
if (buf[i] == '\n' || buf[i] == '\r') {
|
||||
/* found a line ending, so no split symbols / numbers. good. */
|
||||
return true;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
debug("pre-parser rejected: No newline in buf");
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
/****************************************************************************/
|
||||
|
||||
NullMixer::NullMixer() :
|
||||
Mixer(nullptr, 0)
|
||||
{
|
||||
}
|
||||
|
||||
unsigned
|
||||
NullMixer::mix(float *outputs, unsigned space)
|
||||
{
|
||||
if (space > 0) {
|
||||
*outputs = 0.0f;
|
||||
return 1;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
uint16_t
|
||||
NullMixer::get_saturation_status()
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
|
||||
void
|
||||
NullMixer::groups_required(uint32_t &groups)
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
NullMixer *
|
||||
NullMixer::from_text(const char *buf, unsigned &buflen)
|
||||
{
|
||||
NullMixer *nm = nullptr;
|
||||
|
||||
/* enforce that the mixer ends with a new line */
|
||||
if (!string_well_formed(buf, buflen)) {
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
if ((buflen >= 2) && (buf[0] == 'Z') && (buf[1] == ':')) {
|
||||
nm = new NullMixer;
|
||||
buflen -= 2;
|
||||
}
|
||||
|
||||
return nm;
|
||||
}
|
||||
@@ -1,743 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file mixer.h
|
||||
*
|
||||
* Generic, programmable, procedural control signal mixers.
|
||||
*
|
||||
* This library implements a generic mixer interface that can be used
|
||||
* by any driver or subsytem that wants to combine several control signals
|
||||
* into a single output.
|
||||
*
|
||||
* Terminology
|
||||
* ===========
|
||||
*
|
||||
* control value
|
||||
* A mixer input value, typically provided by some controlling
|
||||
* component of the system.
|
||||
*
|
||||
* control group
|
||||
* A collection of controls provided by a single controlling component.
|
||||
*
|
||||
* actuator
|
||||
* The mixer output value.
|
||||
*
|
||||
*
|
||||
* Mixing basics
|
||||
* =============
|
||||
*
|
||||
* An actuator derives its value from the combination of one or more
|
||||
* control values. Each of the control values is scaled according to
|
||||
* the actuator's configuration and then combined to produce the
|
||||
* actuator value, which may then be further scaled to suit the specific
|
||||
* output type.
|
||||
*
|
||||
* Internally, all scaling is performed using floating point values.
|
||||
* Inputs and outputs are clamped to the range -1.0 to 1.0.
|
||||
*
|
||||
* control control control
|
||||
* | | |
|
||||
* v v v
|
||||
* scale scale scale
|
||||
* | | |
|
||||
* | v |
|
||||
* +-------> mix <------+
|
||||
* |
|
||||
* scale
|
||||
* |
|
||||
* v
|
||||
* out
|
||||
*
|
||||
* Scaling
|
||||
* -------
|
||||
*
|
||||
* Each scaler allows the input value to be scaled independently for
|
||||
* inputs greater/less than zero. An offset can be applied to the output,
|
||||
* as well as lower and upper boundary constraints.
|
||||
* Negative scaling factors cause the output to be inverted (negative input
|
||||
* produces positive output).
|
||||
*
|
||||
* Scaler pseudocode:
|
||||
*
|
||||
* if (input < 0)
|
||||
* output = (input * NEGATIVE_SCALE) + OFFSET
|
||||
* else
|
||||
* output = (input * POSITIVE_SCALE) + OFFSET
|
||||
*
|
||||
* if (output < LOWER_LIMIT)
|
||||
* output = LOWER_LIMIT
|
||||
* if (output > UPPER_LIMIT)
|
||||
* output = UPPER_LIMIT
|
||||
*
|
||||
*
|
||||
* Mixing
|
||||
* ------
|
||||
*
|
||||
* Mixing behaviour varies based on the specific mixer class; each
|
||||
* mixer class describes its behaviour in more detail.
|
||||
*
|
||||
*
|
||||
* Controls
|
||||
* --------
|
||||
*
|
||||
* The precise assignment of controls may vary depending on the
|
||||
* application, but the following assignments should be used
|
||||
* when appropriate. Some mixer classes have specific assumptions
|
||||
* about the assignment of controls.
|
||||
*
|
||||
* control | standard meaning
|
||||
* --------+-----------------------
|
||||
* 0 | roll
|
||||
* 1 | pitch
|
||||
* 2 | yaw
|
||||
* 3 | primary thrust
|
||||
*/
|
||||
|
||||
|
||||
#ifndef _SYSTEMLIB_MIXER_MIXER_H
|
||||
#define _SYSTEMLIB_MIXER_MIXER_H value
|
||||
|
||||
#include <px4_config.h>
|
||||
#include "drivers/drv_mixer.h"
|
||||
|
||||
#include "mixer_load.h"
|
||||
|
||||
/**
|
||||
* Abstract class defining a mixer mixing zero or more inputs to
|
||||
* one or more outputs.
|
||||
*/
|
||||
class __EXPORT Mixer
|
||||
{
|
||||
public:
|
||||
/** next mixer in a list */
|
||||
Mixer *_next;
|
||||
|
||||
/**
|
||||
* Fetch a control value.
|
||||
*
|
||||
* @param handle Token passed when the callback is registered.
|
||||
* @param control_group The group to fetch the control from.
|
||||
* @param control_index The group-relative index to fetch the control from.
|
||||
* @param control The returned control
|
||||
* @return Zero if the value was fetched, nonzero otherwise.
|
||||
*/
|
||||
typedef int (* ControlCallback)(uintptr_t handle,
|
||||
uint8_t control_group,
|
||||
uint8_t control_index,
|
||||
float &control);
|
||||
|
||||
/**
|
||||
* Constructor.
|
||||
*
|
||||
* @param control_cb Callback invoked when reading controls.
|
||||
*/
|
||||
Mixer(ControlCallback control_cb, uintptr_t cb_handle);
|
||||
virtual ~Mixer() {}
|
||||
|
||||
/**
|
||||
* Perform the mixing function.
|
||||
*
|
||||
* @param outputs Array into which mixed output(s) should be placed.
|
||||
* @param space The number of available entries in the output array;
|
||||
* @return The number of entries in the output array that were populated.
|
||||
*/
|
||||
virtual unsigned mix(float *outputs, unsigned space) = 0;
|
||||
|
||||
/**
|
||||
* Get the saturation status.
|
||||
*
|
||||
* @return Integer bitmask containing saturation_status from multirotor_motor_limits.msg.
|
||||
*/
|
||||
virtual uint16_t get_saturation_status(void) = 0;
|
||||
|
||||
/**
|
||||
* Analyses the mix configuration and updates a bitmask of groups
|
||||
* that are required.
|
||||
*
|
||||
* @param groups A bitmask of groups (0-31) that the mixer requires.
|
||||
*/
|
||||
virtual void groups_required(uint32_t &groups) = 0;
|
||||
|
||||
/**
|
||||
* @brief Empty method, only implemented for MultirotorMixer and MixerGroup class.
|
||||
*
|
||||
* @param[in] delta_out_max Maximum delta output.
|
||||
*
|
||||
*/
|
||||
virtual void set_max_delta_out_once(float delta_out_max) {}
|
||||
|
||||
/**
|
||||
* @brief Set trim offset for this mixer
|
||||
*
|
||||
* @return the number of outputs this mixer feeds to
|
||||
*/
|
||||
virtual unsigned set_trim(float trim) = 0;
|
||||
|
||||
/*
|
||||
* @brief Sets the thrust factor used to calculate mapping from desired thrust to pwm.
|
||||
*
|
||||
* @param[in] val The value
|
||||
*/
|
||||
virtual void set_thrust_factor(float val) {}
|
||||
|
||||
protected:
|
||||
/** client-supplied callback used when fetching control values */
|
||||
ControlCallback _control_cb;
|
||||
uintptr_t _cb_handle;
|
||||
|
||||
/**
|
||||
* Invoke the client callback to fetch a control value.
|
||||
*
|
||||
* @param group Control group to fetch from.
|
||||
* @param index Control index to fetch.
|
||||
* @return The control value.
|
||||
*/
|
||||
float get_control(uint8_t group, uint8_t index);
|
||||
|
||||
/**
|
||||
* Perform simpler linear scaling.
|
||||
*
|
||||
* @param scaler The scaler configuration.
|
||||
* @param input The value to be scaled.
|
||||
* @return The scaled value.
|
||||
*/
|
||||
static float scale(const mixer_scaler_s &scaler, float input);
|
||||
|
||||
/**
|
||||
* Validate a scaler
|
||||
*
|
||||
* @param scaler The scaler to be validated.
|
||||
* @return Zero if good, nonzero otherwise.
|
||||
*/
|
||||
static int scale_check(struct mixer_scaler_s &scaler);
|
||||
|
||||
/**
|
||||
* Find a tag
|
||||
*
|
||||
* @param buf The buffer to operate on.
|
||||
* @param buflen length of the buffer.
|
||||
* @param tag character to search for.
|
||||
*/
|
||||
static const char *findtag(const char *buf, unsigned &buflen, char tag);
|
||||
|
||||
/**
|
||||
* Skip a line
|
||||
*
|
||||
* @param buf The buffer to operate on.
|
||||
* @param buflen length of the buffer.
|
||||
* @return 0 / OK if a line could be skipped, 1 else
|
||||
*/
|
||||
static const char *skipline(const char *buf, unsigned &buflen);
|
||||
|
||||
/**
|
||||
* Check wether the string is well formed and suitable for parsing
|
||||
*/
|
||||
static bool string_well_formed(const char *buf, unsigned &buflen);
|
||||
|
||||
private:
|
||||
|
||||
/* do not allow to copy due to pointer data members */
|
||||
Mixer(const Mixer &);
|
||||
Mixer &operator=(const Mixer &);
|
||||
};
|
||||
|
||||
/**
|
||||
* Group of mixers, built up from single mixers and processed
|
||||
* in order when mixing.
|
||||
*/
|
||||
class __EXPORT MixerGroup : public Mixer
|
||||
{
|
||||
public:
|
||||
MixerGroup(ControlCallback control_cb, uintptr_t cb_handle);
|
||||
~MixerGroup();
|
||||
|
||||
virtual unsigned mix(float *outputs, unsigned space);
|
||||
virtual uint16_t get_saturation_status(void);
|
||||
virtual void groups_required(uint32_t &groups);
|
||||
|
||||
/**
|
||||
* Add a mixer to the group.
|
||||
*
|
||||
* @param mixer The mixer to be added.
|
||||
*/
|
||||
void add_mixer(Mixer *mixer);
|
||||
|
||||
/**
|
||||
* Remove all the mixers from the group.
|
||||
*/
|
||||
void reset();
|
||||
|
||||
/**
|
||||
* Count the mixers in the group.
|
||||
*/
|
||||
unsigned count();
|
||||
|
||||
/**
|
||||
* Adds mixers to the group based on a text description in a buffer.
|
||||
*
|
||||
* Mixer definitions begin with a single capital letter and a colon.
|
||||
* The actual format of the mixer definition varies with the individual
|
||||
* mixers; they are summarised here, but see ROMFS/mixers/README for
|
||||
* more details.
|
||||
*
|
||||
* Null Mixer
|
||||
* ..........
|
||||
*
|
||||
* The null mixer definition has the form:
|
||||
*
|
||||
* Z:
|
||||
*
|
||||
* Simple Mixer
|
||||
* ............
|
||||
*
|
||||
* A simple mixer definition begins with:
|
||||
*
|
||||
* M: <control count>
|
||||
* O: <-ve scale> <+ve scale> <offset> <lower limit> <upper limit>
|
||||
*
|
||||
* The definition continues with <control count> entries describing the control
|
||||
* inputs and their scaling, in the form:
|
||||
*
|
||||
* S: <group> <index> <-ve scale> <+ve scale> <offset> <lower limit> <upper limit>
|
||||
*
|
||||
* Multirotor Mixer
|
||||
* ................
|
||||
*
|
||||
* The multirotor mixer definition is a single line of the form:
|
||||
*
|
||||
* R: <geometry> <roll scale> <pitch scale> <yaw scale> <deadband>
|
||||
*
|
||||
* Helicopter Mixer
|
||||
* ................
|
||||
*
|
||||
* The helicopter mixer includes throttle and pitch curves
|
||||
*
|
||||
* H: <swash plate servo count>
|
||||
* T: <0> <2500> <5000> <7500> <10000>
|
||||
* P: <-10000> <-5000> <0> <5000> <10000>
|
||||
*
|
||||
* The definition continues with <swash plate servo count> entries describing
|
||||
* the position of the servo, in the following form:
|
||||
*
|
||||
* S: <angle (deg)> <normalized arm length> <scale> <offset> <lower limit> <upper limit>
|
||||
*
|
||||
* @param buf The mixer configuration buffer.
|
||||
* @param buflen The length of the buffer, updated to reflect
|
||||
* bytes as they are consumed.
|
||||
* @return Zero on successful load, nonzero otherwise.
|
||||
*/
|
||||
int load_from_buf(const char *buf, unsigned &buflen);
|
||||
|
||||
/**
|
||||
* @brief Update slew rate parameter. This tells instances of the class MultirotorMixer
|
||||
* the maximum allowed change of the output values per cycle.
|
||||
* The value is only valid for one cycle, in order to have continuous
|
||||
* slew rate limiting this function needs to be called before every call
|
||||
* to mix().
|
||||
*
|
||||
* @param[in] delta_out_max Maximum delta output.
|
||||
*
|
||||
*/
|
||||
virtual void set_max_delta_out_once(float delta_out_max);
|
||||
|
||||
/*
|
||||
* Invoke the set_offset method of each mixer in the group
|
||||
* for each value in page r_page_servo_control_trim
|
||||
*/
|
||||
unsigned set_trims(int16_t *v, unsigned n);
|
||||
|
||||
unsigned set_trim(float trim)
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Sets the thrust factor used to calculate mapping from desired thrust to pwm.
|
||||
*
|
||||
* @param[in] val The value
|
||||
*/
|
||||
virtual void set_thrust_factor(float val);
|
||||
|
||||
private:
|
||||
Mixer *_first; /**< linked list of mixers */
|
||||
|
||||
/* do not allow to copy due to pointer data members */
|
||||
MixerGroup(const MixerGroup &);
|
||||
MixerGroup operator=(const MixerGroup &);
|
||||
};
|
||||
|
||||
/**
|
||||
* Null mixer; returns zero.
|
||||
*
|
||||
* Used as a placeholder for output channels that are unassigned in groups.
|
||||
*/
|
||||
class __EXPORT NullMixer : public Mixer
|
||||
{
|
||||
public:
|
||||
NullMixer();
|
||||
~NullMixer() {}
|
||||
|
||||
/**
|
||||
* Factory method.
|
||||
*
|
||||
* Given a pointer to a buffer containing a text description of the mixer,
|
||||
* returns a pointer to a new instance of the mixer.
|
||||
*
|
||||
* @param buf Buffer containing a text description of
|
||||
* the mixer.
|
||||
* @param buflen Length of the buffer in bytes, adjusted
|
||||
* to reflect the bytes consumed.
|
||||
* @return A new NullMixer instance, or nullptr
|
||||
* if the text format is bad.
|
||||
*/
|
||||
static NullMixer *from_text(const char *buf, unsigned &buflen);
|
||||
|
||||
virtual unsigned mix(float *outputs, unsigned space);
|
||||
virtual uint16_t get_saturation_status(void);
|
||||
virtual void groups_required(uint32_t &groups);
|
||||
virtual void set_offset(float trim) {}
|
||||
unsigned set_trim(float trim)
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
/**
|
||||
* Simple summing mixer.
|
||||
*
|
||||
* Collects zero or more inputs and mixes them to a single output.
|
||||
*/
|
||||
class __EXPORT SimpleMixer : public Mixer
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* Constructor
|
||||
*
|
||||
* @param mixinfo Mixer configuration. The pointer passed
|
||||
* becomes the property of the mixer and
|
||||
* will be freed when the mixer is deleted.
|
||||
*/
|
||||
SimpleMixer(ControlCallback control_cb,
|
||||
uintptr_t cb_handle,
|
||||
mixer_simple_s *mixinfo);
|
||||
~SimpleMixer();
|
||||
|
||||
/**
|
||||
* Factory method with full external configuration.
|
||||
*
|
||||
* Given a pointer to a buffer containing a text description of the mixer,
|
||||
* returns a pointer to a new instance of the mixer.
|
||||
*
|
||||
* @param control_cb The callback to invoke when fetching a
|
||||
* control value.
|
||||
* @param cb_handle Handle passed to the control callback.
|
||||
* @param buf Buffer containing a text description of
|
||||
* the mixer.
|
||||
* @param buflen Length of the buffer in bytes, adjusted
|
||||
* to reflect the bytes consumed.
|
||||
* @return A new SimpleMixer instance, or nullptr
|
||||
* if the text format is bad.
|
||||
*/
|
||||
static SimpleMixer *from_text(Mixer::ControlCallback control_cb,
|
||||
uintptr_t cb_handle,
|
||||
const char *buf,
|
||||
unsigned &buflen);
|
||||
|
||||
/**
|
||||
* Factory method for PWM/PPM input to internal float representation.
|
||||
*
|
||||
* @param control_cb The callback to invoke when fetching a
|
||||
* control value.
|
||||
* @param cb_handle Handle passed to the control callback.
|
||||
* @param input The control index used when fetching the input.
|
||||
* @param min The PWM/PPM value considered to be "minimum" (gives -1.0 out)
|
||||
* @param mid The PWM/PPM value considered to be the midpoint (gives 0.0 out)
|
||||
* @param max The PWM/PPM value considered to be "maximum" (gives 1.0 out)
|
||||
* @return A new SimpleMixer instance, or nullptr if one could not be
|
||||
* allocated.
|
||||
*/
|
||||
static SimpleMixer *pwm_input(Mixer::ControlCallback control_cb, uintptr_t cb_handle, unsigned input, uint16_t min,
|
||||
uint16_t mid, uint16_t max);
|
||||
|
||||
virtual unsigned mix(float *outputs, unsigned space);
|
||||
virtual uint16_t get_saturation_status(void);
|
||||
virtual void groups_required(uint32_t &groups);
|
||||
|
||||
/**
|
||||
* Check that the mixer configuration as loaded is sensible.
|
||||
*
|
||||
* Note that this function will call control_cb, but only cares about
|
||||
* error returns, not the input value.
|
||||
*
|
||||
* @return Zero if the mixer makes sense, nonzero otherwise.
|
||||
*/
|
||||
int check();
|
||||
|
||||
unsigned set_trim(float trim);
|
||||
|
||||
protected:
|
||||
|
||||
private:
|
||||
mixer_simple_s *_pinfo;
|
||||
|
||||
static int parse_output_scaler(const char *buf, unsigned &buflen, mixer_scaler_s &scaler);
|
||||
static int parse_control_scaler(const char *buf,
|
||||
unsigned &buflen,
|
||||
mixer_scaler_s &scaler,
|
||||
uint8_t &control_group,
|
||||
uint8_t &control_index);
|
||||
|
||||
/* do not allow to copy due to ptr data members */
|
||||
SimpleMixer(const SimpleMixer &);
|
||||
SimpleMixer operator=(const SimpleMixer &);
|
||||
};
|
||||
|
||||
/**
|
||||
* Supported multirotor geometries.
|
||||
*
|
||||
* Values are generated by the multi_tables script and placed to mixer_multirotor.generated.h
|
||||
*/
|
||||
typedef unsigned int MultirotorGeometryUnderlyingType;
|
||||
enum class MultirotorGeometry : MultirotorGeometryUnderlyingType;
|
||||
|
||||
/**
|
||||
* Multi-rotor mixer for pre-defined vehicle geometries.
|
||||
*
|
||||
* Collects four inputs (roll, pitch, yaw, thrust) and mixes them to
|
||||
* a set of outputs based on the configured geometry.
|
||||
*/
|
||||
class __EXPORT MultirotorMixer : public Mixer
|
||||
{
|
||||
public:
|
||||
/**
|
||||
|
||||
* Precalculated rotor mix.
|
||||
*/
|
||||
struct Rotor {
|
||||
float roll_scale; /**< scales roll for this rotor */
|
||||
float pitch_scale; /**< scales pitch for this rotor */
|
||||
float yaw_scale; /**< scales yaw for this rotor */
|
||||
float out_scale; /**< scales total out for this rotor */
|
||||
};
|
||||
|
||||
/**
|
||||
* Constructor.
|
||||
*
|
||||
* @param control_cb Callback invoked to read inputs.
|
||||
* @param cb_handle Passed to control_cb.
|
||||
* @param geometry The selected geometry.
|
||||
* @param roll_scale Scaling factor applied to roll inputs
|
||||
* compared to thrust.
|
||||
* @param pitch_scale Scaling factor applied to pitch inputs
|
||||
* compared to thrust.
|
||||
* @param yaw_wcale Scaling factor applied to yaw inputs compared
|
||||
* to thrust.
|
||||
* @param idle_speed Minimum rotor control output value; usually
|
||||
* tuned to ensure that rotors never stall at the
|
||||
* low end of their control range.
|
||||
*/
|
||||
MultirotorMixer(ControlCallback control_cb,
|
||||
uintptr_t cb_handle,
|
||||
MultirotorGeometry geometry,
|
||||
float roll_scale,
|
||||
float pitch_scale,
|
||||
float yaw_scale,
|
||||
float idle_speed);
|
||||
~MultirotorMixer();
|
||||
|
||||
/**
|
||||
* Factory method.
|
||||
*
|
||||
* Given a pointer to a buffer containing a text description of the mixer,
|
||||
* returns a pointer to a new instance of the mixer.
|
||||
*
|
||||
* @param control_cb The callback to invoke when fetching a
|
||||
* control value.
|
||||
* @param cb_handle Handle passed to the control callback.
|
||||
* @param buf Buffer containing a text description of
|
||||
* the mixer.
|
||||
* @param buflen Length of the buffer in bytes, adjusted
|
||||
* to reflect the bytes consumed.
|
||||
* @return A new MultirotorMixer instance, or nullptr
|
||||
* if the text format is bad.
|
||||
*/
|
||||
static MultirotorMixer *from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, const char *buf,
|
||||
unsigned &buflen);
|
||||
|
||||
virtual unsigned mix(float *outputs, unsigned space);
|
||||
virtual uint16_t get_saturation_status(void);
|
||||
virtual void groups_required(uint32_t &groups);
|
||||
|
||||
/**
|
||||
* @brief Update slew rate parameter. This tells the multicopter mixer
|
||||
* the maximum allowed change of the output values per cycle.
|
||||
* The value is only valid for one cycle, in order to have continuous
|
||||
* slew rate limiting this function needs to be called before every call
|
||||
* to mix().
|
||||
*
|
||||
* @param[in] delta_out_max Maximum delta output.
|
||||
*
|
||||
*/
|
||||
virtual void set_max_delta_out_once(float delta_out_max) { _delta_out_max = delta_out_max; }
|
||||
|
||||
unsigned set_trim(float trim)
|
||||
{
|
||||
return _rotor_count;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Sets the thrust factor used to calculate mapping from desired thrust to pwm.
|
||||
*
|
||||
* @param[in] val The value
|
||||
*/
|
||||
virtual void set_thrust_factor(float val) {_thrust_factor = val;}
|
||||
|
||||
union saturation_status {
|
||||
struct {
|
||||
uint16_t valid : 1; // 0 - true when the saturation status is used
|
||||
uint16_t motor_pos : 1; // 1 - true when any motor has saturated in the positive direction
|
||||
uint16_t motor_neg : 1; // 2 - true when any motor has saturated in the negative direction
|
||||
uint16_t roll_pos : 1; // 3 - true when a positive roll demand change will increase saturation
|
||||
uint16_t roll_neg : 1; // 4 - true when a negative roll demand change will increase saturation
|
||||
uint16_t pitch_pos : 1; // 5 - true when a positive pitch demand change will increase saturation
|
||||
uint16_t pitch_neg : 1; // 6 - true when a negative pitch demand change will increase saturation
|
||||
uint16_t yaw_pos : 1; // 7 - true when a positive yaw demand change will increase saturation
|
||||
uint16_t yaw_neg : 1; // 8 - true when a negative yaw demand change will increase saturation
|
||||
uint16_t thrust_pos : 1; // 9 - true when a positive thrust demand change will increase saturation
|
||||
uint16_t thrust_neg : 1; //10 - true when a negative thrust demand change will increase saturation
|
||||
} flags;
|
||||
uint16_t value;
|
||||
};
|
||||
|
||||
private:
|
||||
float _roll_scale;
|
||||
float _pitch_scale;
|
||||
float _yaw_scale;
|
||||
float _idle_speed;
|
||||
float _delta_out_max;
|
||||
float _thrust_factor;
|
||||
|
||||
void update_saturation_status(unsigned index, bool clipping_high, bool clipping_low);
|
||||
saturation_status _saturation_status;
|
||||
|
||||
unsigned _rotor_count;
|
||||
const Rotor *_rotors;
|
||||
|
||||
float *_outputs_prev = nullptr;
|
||||
|
||||
/* do not allow to copy due to ptr data members */
|
||||
MultirotorMixer(const MultirotorMixer &);
|
||||
MultirotorMixer operator=(const MultirotorMixer &);
|
||||
};
|
||||
|
||||
/** helicopter swash servo mixer */
|
||||
struct mixer_heli_servo_s {
|
||||
float angle;
|
||||
float arm_length;
|
||||
float scale;
|
||||
float offset;
|
||||
float min_output;
|
||||
float max_output;
|
||||
};
|
||||
|
||||
#define HELI_CURVES_NR_POINTS 5
|
||||
|
||||
/** helicopter swash plate mixer */
|
||||
struct mixer_heli_s {
|
||||
uint8_t control_count; /**< number of inputs */
|
||||
float throttle_curve[HELI_CURVES_NR_POINTS];
|
||||
float pitch_curve[HELI_CURVES_NR_POINTS];
|
||||
struct mixer_heli_servo_s servos[4]; /**< up to four inputs */
|
||||
};
|
||||
|
||||
/**
|
||||
* Generic helicopter mixer for helicopters with swash plate.
|
||||
*
|
||||
* Collects four inputs (roll, pitch, yaw, thrust) and mixes them to servo commands
|
||||
* for swash plate tilting and throttle- and pitch curves.
|
||||
*/
|
||||
class __EXPORT HelicopterMixer : public Mixer
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* Constructor.
|
||||
*
|
||||
* @param control_cb Callback invoked to read inputs.
|
||||
* @param cb_handle Passed to control_cb.
|
||||
* @param mixer_info Pointer to heli mixer configuration
|
||||
*/
|
||||
HelicopterMixer(ControlCallback control_cb,
|
||||
uintptr_t cb_handle,
|
||||
mixer_heli_s *mixer_info);
|
||||
~HelicopterMixer();
|
||||
|
||||
/**
|
||||
* Factory method.
|
||||
*
|
||||
* Given a pointer to a buffer containing a text description of the mixer,
|
||||
* returns a pointer to a new instance of the mixer.
|
||||
*
|
||||
* @param control_cb The callback to invoke when fetching a
|
||||
* control value.
|
||||
* @param cb_handle Handle passed to the control callback.
|
||||
* @param buf Buffer containing a text description of
|
||||
* the mixer.
|
||||
* @param buflen Length of the buffer in bytes, adjusted
|
||||
* to reflect the bytes consumed.
|
||||
* @return A new HelicopterMixer instance, or nullptr
|
||||
* if the text format is bad.
|
||||
*/
|
||||
static HelicopterMixer *from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, const char *buf,
|
||||
unsigned &buflen);
|
||||
|
||||
virtual unsigned mix(float *outputs, unsigned space);
|
||||
virtual void groups_required(uint32_t &groups);
|
||||
|
||||
virtual uint16_t get_saturation_status(void) { return 0; }
|
||||
|
||||
unsigned set_trim(float trim)
|
||||
{
|
||||
return 4;
|
||||
}
|
||||
|
||||
private:
|
||||
mixer_heli_s _mixer_info;
|
||||
|
||||
/* do not allow to copy */
|
||||
HelicopterMixer(const HelicopterMixer &);
|
||||
HelicopterMixer operator=(const HelicopterMixer &);
|
||||
};
|
||||
|
||||
#endif
|
||||
@@ -1,275 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file mixer_group.cpp
|
||||
*
|
||||
* Mixer collection.
|
||||
*/
|
||||
|
||||
#include <px4_config.h>
|
||||
|
||||
#include <sys/types.h>
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <fcntl.h>
|
||||
#include <poll.h>
|
||||
#include <errno.h>
|
||||
#include <stdio.h>
|
||||
#include <math.h>
|
||||
#include <unistd.h>
|
||||
|
||||
#include "mixer.h"
|
||||
|
||||
#define debug(fmt, args...) do { } while(0)
|
||||
//#define debug(fmt, args...) do { printf("[mixer] " fmt "\n", ##args); } while(0)
|
||||
//#include <debug.h>
|
||||
//#define debug(fmt, args...) syslog(fmt "\n", ##args)
|
||||
|
||||
MixerGroup::MixerGroup(ControlCallback control_cb, uintptr_t cb_handle) :
|
||||
Mixer(control_cb, cb_handle),
|
||||
_first(nullptr)
|
||||
{
|
||||
}
|
||||
|
||||
MixerGroup::~MixerGroup()
|
||||
{
|
||||
reset();
|
||||
}
|
||||
|
||||
void
|
||||
MixerGroup::add_mixer(Mixer *mixer)
|
||||
{
|
||||
Mixer **mpp;
|
||||
|
||||
mpp = &_first;
|
||||
|
||||
while (*mpp != nullptr) {
|
||||
mpp = &((*mpp)->_next);
|
||||
}
|
||||
|
||||
*mpp = mixer;
|
||||
mixer->_next = nullptr;
|
||||
}
|
||||
|
||||
void
|
||||
MixerGroup::reset()
|
||||
{
|
||||
Mixer *mixer;
|
||||
Mixer *next = _first;
|
||||
|
||||
/* flag mixer as invalid */
|
||||
_first = nullptr;
|
||||
|
||||
/* discard sub-mixers */
|
||||
while (next != nullptr) {
|
||||
mixer = next;
|
||||
next = mixer->_next;
|
||||
delete mixer;
|
||||
mixer = nullptr;
|
||||
}
|
||||
}
|
||||
|
||||
unsigned
|
||||
MixerGroup::mix(float *outputs, unsigned space)
|
||||
{
|
||||
Mixer *mixer = _first;
|
||||
unsigned index = 0;
|
||||
|
||||
while ((mixer != nullptr) && (index < space)) {
|
||||
index += mixer->mix(outputs + index, space - index);
|
||||
mixer = mixer->_next;
|
||||
}
|
||||
|
||||
return index;
|
||||
}
|
||||
|
||||
/*
|
||||
* set_trims() has no effect except for the SimpleMixer implementation for which set_trim()
|
||||
* always returns the value one.
|
||||
* The only other existing implementation is MultirotorMixer, which ignores the trim value
|
||||
* and returns _rotor_count.
|
||||
*/
|
||||
unsigned
|
||||
MixerGroup::set_trims(int16_t *values, unsigned n)
|
||||
{
|
||||
Mixer *mixer = _first;
|
||||
unsigned index = 0;
|
||||
|
||||
while ((mixer != nullptr) && (index < n)) {
|
||||
/* convert from integer to float */
|
||||
float offset = (float)values[index] / 10000;
|
||||
|
||||
/* to be safe, clamp offset to range of [-100, 100] usec */
|
||||
if (offset < -0.2f) { offset = -0.2f; }
|
||||
|
||||
if (offset > 0.2f) { offset = 0.2f; }
|
||||
|
||||
debug("set trim: %d, offset: %5.3f", values[index], (double)offset);
|
||||
index += mixer->set_trim(offset);
|
||||
mixer = mixer->_next;
|
||||
}
|
||||
|
||||
return index;
|
||||
}
|
||||
|
||||
void
|
||||
MixerGroup::set_thrust_factor(float val)
|
||||
{
|
||||
Mixer *mixer = _first;
|
||||
|
||||
while (mixer != nullptr) {
|
||||
mixer->set_thrust_factor(val);
|
||||
mixer = mixer->_next;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
uint16_t
|
||||
MixerGroup::get_saturation_status()
|
||||
{
|
||||
Mixer *mixer = _first;
|
||||
uint16_t sat = 0;
|
||||
|
||||
while (mixer != nullptr) {
|
||||
sat |= mixer->get_saturation_status();
|
||||
mixer = mixer->_next;
|
||||
}
|
||||
|
||||
return sat;
|
||||
}
|
||||
|
||||
unsigned
|
||||
MixerGroup::count()
|
||||
{
|
||||
Mixer *mixer = _first;
|
||||
unsigned index = 0;
|
||||
|
||||
while (mixer != nullptr) {
|
||||
mixer = mixer->_next;
|
||||
index++;
|
||||
}
|
||||
|
||||
return index;
|
||||
}
|
||||
|
||||
void
|
||||
MixerGroup::groups_required(uint32_t &groups)
|
||||
{
|
||||
Mixer *mixer = _first;
|
||||
|
||||
while (mixer != nullptr) {
|
||||
mixer->groups_required(groups);
|
||||
mixer = mixer->_next;
|
||||
}
|
||||
}
|
||||
|
||||
int
|
||||
MixerGroup::load_from_buf(const char *buf, unsigned &buflen)
|
||||
{
|
||||
int ret = -1;
|
||||
const char *end = buf + buflen;
|
||||
|
||||
/*
|
||||
* Loop until either we have emptied the buffer, or we have failed to
|
||||
* allocate something when we expected to.
|
||||
*/
|
||||
while (buflen > 0) {
|
||||
Mixer *m = nullptr;
|
||||
const char *p = end - buflen;
|
||||
unsigned resid = buflen;
|
||||
|
||||
/*
|
||||
* Use the next character as a hint to decide which mixer class to construct.
|
||||
*/
|
||||
switch (*p) {
|
||||
case 'Z':
|
||||
m = NullMixer::from_text(p, resid);
|
||||
break;
|
||||
|
||||
case 'M':
|
||||
m = SimpleMixer::from_text(_control_cb, _cb_handle, p, resid);
|
||||
break;
|
||||
|
||||
case 'R':
|
||||
m = MultirotorMixer::from_text(_control_cb, _cb_handle, p, resid);
|
||||
break;
|
||||
|
||||
case 'H':
|
||||
m = HelicopterMixer::from_text(_control_cb, _cb_handle, p, resid);
|
||||
break;
|
||||
|
||||
default:
|
||||
/* it's probably junk or whitespace, skip a byte and retry */
|
||||
buflen--;
|
||||
continue;
|
||||
}
|
||||
|
||||
/*
|
||||
* If we constructed something, add it to the group.
|
||||
*/
|
||||
if (m != nullptr) {
|
||||
add_mixer(m);
|
||||
|
||||
/* we constructed something */
|
||||
ret = 0;
|
||||
|
||||
/* only adjust buflen if parsing was successful */
|
||||
buflen = resid;
|
||||
debug("SUCCESS - buflen: %d", buflen);
|
||||
|
||||
} else {
|
||||
|
||||
/*
|
||||
* There is data in the buffer that we expected to parse, but it didn't,
|
||||
* so give up for now.
|
||||
*/
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
/* nothing more in the buffer for us now */
|
||||
return ret;
|
||||
}
|
||||
|
||||
void MixerGroup::set_max_delta_out_once(float delta_out_max)
|
||||
{
|
||||
Mixer *mixer = _first;
|
||||
|
||||
while (mixer != nullptr) {
|
||||
mixer->set_max_delta_out_once(delta_out_max);
|
||||
mixer = mixer->_next;
|
||||
}
|
||||
}
|
||||
@@ -1,285 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2012-2017 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file mixer_helicopter.cpp
|
||||
*
|
||||
* Helicopter mixers.
|
||||
*/
|
||||
#include <px4_config.h>
|
||||
#include <sys/types.h>
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <fcntl.h>
|
||||
#include <poll.h>
|
||||
#include <errno.h>
|
||||
#include <stdio.h>
|
||||
#include <math.h>
|
||||
#include <unistd.h>
|
||||
#include <math.h>
|
||||
|
||||
#include "mixer.h"
|
||||
|
||||
#define debug(fmt, args...) do { } while(0)
|
||||
//#define debug(fmt, args...) do { printf("[mixer] " fmt "\n", ##args); } while(0)
|
||||
//#include <debug.h>
|
||||
//#define debug(fmt, args...) lowsyslog(fmt "\n", ##args)
|
||||
|
||||
|
||||
namespace
|
||||
{
|
||||
|
||||
float constrain(float val, float min, float max)
|
||||
{
|
||||
return (val < min) ? min : ((val > max) ? max : val);
|
||||
}
|
||||
|
||||
} // anonymous namespace
|
||||
|
||||
HelicopterMixer::HelicopterMixer(ControlCallback control_cb,
|
||||
uintptr_t cb_handle,
|
||||
mixer_heli_s *mixer_info) :
|
||||
Mixer(control_cb, cb_handle),
|
||||
_mixer_info(*mixer_info)
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
HelicopterMixer::~HelicopterMixer()
|
||||
{
|
||||
}
|
||||
|
||||
HelicopterMixer *
|
||||
HelicopterMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, const char *buf, unsigned &buflen)
|
||||
{
|
||||
mixer_heli_s mixer_info;
|
||||
unsigned swash_plate_servo_count = 0;
|
||||
unsigned u[5];
|
||||
int s[5];
|
||||
int used;
|
||||
|
||||
/* enforce that the mixer ends with space or a new line */
|
||||
for (int i = buflen - 1; i >= 0; i--) {
|
||||
if (buf[i] == '\0') {
|
||||
continue;
|
||||
}
|
||||
|
||||
/* require a space or newline at the end of the buffer, fail on printable chars */
|
||||
if (buf[i] == ' ' || buf[i] == '\n' || buf[i] == '\r') {
|
||||
/* found a line ending or space, so no split symbols / numbers. good. */
|
||||
break;
|
||||
|
||||
} else {
|
||||
debug("simple parser rejected: No newline / space at end of buf. (#%d/%d: 0x%02x)", i, buflen - 1, buf[i]);
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
if (sscanf(buf, "H: %u%n", &swash_plate_servo_count, &used) != 1) {
|
||||
debug("helicopter parse failed on '%s'", buf);
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
if (swash_plate_servo_count < 3 || swash_plate_servo_count > 4) {
|
||||
debug("only supporting swash plate with 3 or 4 servos");
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
if (used > (int)buflen) {
|
||||
debug("OVERFLOW: helicopter spec used %d of %u", used, buflen);
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
buf = skipline(buf, buflen);
|
||||
|
||||
if (buf == nullptr) {
|
||||
debug("no line ending, line is incomplete");
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
buf = findtag(buf, buflen, 'T');
|
||||
|
||||
if ((buf == nullptr) || (buflen < 12)) {
|
||||
debug("control parser failed finding tag, ret: '%s'", buf);
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
if (sscanf(buf, "T: %u %u %u %u %u",
|
||||
&u[0], &u[1], &u[2], &u[3], &u[4]) != 5) {
|
||||
debug("control parse failed on '%s'", buf);
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
for (unsigned i = 0; i < HELI_CURVES_NR_POINTS; i++) {
|
||||
mixer_info.throttle_curve[i] = ((float) u[i]) / 10000.0f;
|
||||
}
|
||||
|
||||
buf = skipline(buf, buflen);
|
||||
|
||||
if (buf == nullptr) {
|
||||
debug("no line ending, line is incomplete");
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
buf = findtag(buf, buflen, 'P');
|
||||
|
||||
if ((buf == nullptr) || (buflen < 12)) {
|
||||
debug("control parser failed finding tag, ret: '%s'", buf);
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
if (sscanf(buf, "P: %d %d %d %d %d",
|
||||
&s[0], &s[1], &s[2], &s[3], &s[4]) != 5) {
|
||||
debug("control parse failed on '%s'", buf);
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
for (unsigned i = 0; i < HELI_CURVES_NR_POINTS; i++) {
|
||||
mixer_info.pitch_curve[i] = ((float) s[i]) / 10000.0f;
|
||||
}
|
||||
|
||||
buf = skipline(buf, buflen);
|
||||
|
||||
if (buf == nullptr) {
|
||||
debug("no line ending, line is incomplete");
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
mixer_info.control_count = swash_plate_servo_count;
|
||||
|
||||
/* Now loop through the servos */
|
||||
for (unsigned i = 0; i < mixer_info.control_count; i++) {
|
||||
|
||||
buf = findtag(buf, buflen, 'S');
|
||||
|
||||
if ((buf == nullptr) || (buflen < 12)) {
|
||||
debug("control parser failed finding tag, ret: '%s'", buf);
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
if (sscanf(buf, "S: %u %u %d %d %d %d",
|
||||
&u[0],
|
||||
&u[1],
|
||||
&s[0],
|
||||
&s[1],
|
||||
&s[2],
|
||||
&s[3]) != 6) {
|
||||
debug("control parse failed on '%s'", buf);
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
mixer_info.servos[i].angle = ((float) u[0]) * M_PI_F / 180.0f;
|
||||
mixer_info.servos[i].arm_length = ((float) u[1]) / 10000.0f;
|
||||
mixer_info.servos[i].scale = ((float) s[0]) / 10000.0f;
|
||||
mixer_info.servos[i].offset = ((float) s[1]) / 10000.0f;
|
||||
mixer_info.servos[i].min_output = ((float) s[2]) / 10000.0f;
|
||||
mixer_info.servos[i].max_output = ((float) s[3]) / 10000.0f;
|
||||
|
||||
buf = skipline(buf, buflen);
|
||||
|
||||
if (buf == nullptr) {
|
||||
debug("no line ending, line is incomplete");
|
||||
return nullptr;
|
||||
}
|
||||
}
|
||||
|
||||
debug("remaining in buf: %d, first char: %c", buflen, buf[0]);
|
||||
|
||||
HelicopterMixer *hm = new HelicopterMixer(
|
||||
control_cb,
|
||||
cb_handle,
|
||||
&mixer_info);
|
||||
|
||||
if (hm != nullptr) {
|
||||
debug("loaded heli mixer with %d swash plate input(s)", mixer_info.control_count);
|
||||
|
||||
} else {
|
||||
debug("could not allocate memory for mixer");
|
||||
}
|
||||
|
||||
return hm;
|
||||
}
|
||||
|
||||
unsigned
|
||||
HelicopterMixer::mix(float *outputs, unsigned space)
|
||||
{
|
||||
/* Find index to use for curves */
|
||||
float thrust_cmd = get_control(0, 3);
|
||||
int idx = (thrust_cmd / 0.25f);
|
||||
|
||||
/* Make sure idx is in range */
|
||||
if (idx < 0) {
|
||||
idx = 0;
|
||||
|
||||
} else if (idx > HELI_CURVES_NR_POINTS - 2) {
|
||||
/* We access idx + 1 below, so max legal index is (size - 2) */
|
||||
idx = HELI_CURVES_NR_POINTS - 2;
|
||||
}
|
||||
|
||||
/* Local throttle curve gradient and offset */
|
||||
float tg = (_mixer_info.throttle_curve[idx + 1] - _mixer_info.throttle_curve[idx]) / 0.25f;
|
||||
float to = (_mixer_info.throttle_curve[idx]) - (tg * idx * 0.25f);
|
||||
float throttle = constrain(2.0f * (tg * thrust_cmd + to) - 1.0f, -1.0f, 1.0f);
|
||||
|
||||
/* Local pitch curve gradient and offset */
|
||||
float pg = (_mixer_info.pitch_curve[idx + 1] - _mixer_info.pitch_curve[idx]) / 0.25f;
|
||||
float po = (_mixer_info.pitch_curve[idx]) - (pg * idx * 0.25f);
|
||||
float collective_pitch = constrain((pg * thrust_cmd + po), -0.5f, 0.5f);
|
||||
|
||||
float roll_cmd = get_control(0, 0);
|
||||
float pitch_cmd = get_control(0, 1);
|
||||
|
||||
outputs[0] = throttle;
|
||||
|
||||
for (unsigned i = 0; i < _mixer_info.control_count; i++) {
|
||||
outputs[i + 1] = collective_pitch
|
||||
+ cosf(_mixer_info.servos[i].angle) * pitch_cmd * _mixer_info.servos[i].arm_length
|
||||
- sinf(_mixer_info.servos[i].angle) * roll_cmd * _mixer_info.servos[i].arm_length;
|
||||
outputs[i + 1] *= _mixer_info.servos[i].scale;
|
||||
outputs[i + 1] += _mixer_info.servos[i].offset;
|
||||
outputs[i + 1] = constrain(outputs[i + 1], _mixer_info.servos[i].min_output, _mixer_info.servos[i].max_output);
|
||||
}
|
||||
|
||||
return _mixer_info.control_count + 1;
|
||||
}
|
||||
|
||||
void
|
||||
HelicopterMixer::groups_required(uint32_t &groups)
|
||||
{
|
||||
/* XXX for now, hardcoded to indexes 0-3 in control group zero */
|
||||
groups |= (1 << 0);
|
||||
}
|
||||
|
||||
@@ -1,115 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file mixer_load.c
|
||||
*
|
||||
* Programmable multi-channel mixer library.
|
||||
*/
|
||||
|
||||
#include <px4_config.h>
|
||||
#include <string.h>
|
||||
#include <stdio.h>
|
||||
#include <ctype.h>
|
||||
#include <systemlib/err.h>
|
||||
|
||||
#include "mixer_load.h"
|
||||
|
||||
int load_mixer_file(const char *fname, char *buf, unsigned maxlen)
|
||||
{
|
||||
FILE *fp;
|
||||
char line[120];
|
||||
|
||||
/* open the mixer definition file */
|
||||
fp = fopen(fname, "r");
|
||||
|
||||
if (fp == NULL) {
|
||||
warnx("file not found");
|
||||
return -1;
|
||||
}
|
||||
|
||||
/* read valid lines from the file into a buffer */
|
||||
buf[0] = '\0';
|
||||
|
||||
for (;;) {
|
||||
|
||||
/* get a line, bail on error/EOF */
|
||||
line[0] = '\0';
|
||||
|
||||
if (fgets(line, sizeof(line), fp) == NULL) {
|
||||
break;
|
||||
}
|
||||
|
||||
/* if the line doesn't look like a mixer definition line, skip it */
|
||||
if ((strlen(line) < 2) || !isupper(line[0]) || (line[1] != ':')) {
|
||||
continue;
|
||||
}
|
||||
|
||||
/* compact whitespace in the buffer */
|
||||
char *t, *f;
|
||||
|
||||
for (f = line; *f != '\0'; f++) {
|
||||
/* scan for space characters */
|
||||
if (*f == ' ') {
|
||||
/* look for additional spaces */
|
||||
t = f + 1;
|
||||
|
||||
while (*t == ' ') {
|
||||
t++;
|
||||
}
|
||||
|
||||
if (*t == '\0') {
|
||||
/* strip trailing whitespace */
|
||||
*f = '\0';
|
||||
|
||||
} else if (t > (f + 1)) {
|
||||
memmove(f + 1, t, strlen(t) + 1);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/* if the line is too long to fit in the buffer, bail */
|
||||
if ((strlen(line) + strlen(buf) + 1) >= maxlen) {
|
||||
warnx("line too long");
|
||||
fclose(fp);
|
||||
return -1;
|
||||
}
|
||||
|
||||
/* add the line to the buffer */
|
||||
strcat(buf, line);
|
||||
}
|
||||
|
||||
fclose(fp);
|
||||
return 0;
|
||||
}
|
||||
|
||||
@@ -1,51 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file mixer_load.h
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
#ifndef _SYSTEMLIB_MIXER_LOAD_H
|
||||
#define _SYSTEMLIB_MIXER_LOAD_H value
|
||||
|
||||
#include <px4_config.h>
|
||||
|
||||
__BEGIN_DECLS
|
||||
|
||||
__EXPORT int load_mixer_file(const char *fname, char *buf, unsigned maxlen);
|
||||
|
||||
__END_DECLS
|
||||
|
||||
#endif
|
||||
@@ -1,460 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2012-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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file mixer_multirotor.cpp
|
||||
*
|
||||
* Multi-rotor mixers.
|
||||
*/
|
||||
#include <px4_config.h>
|
||||
#include <sys/types.h>
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
#include <float.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <fcntl.h>
|
||||
#include <poll.h>
|
||||
#include <errno.h>
|
||||
#include <stdio.h>
|
||||
#include <math.h>
|
||||
#include <unistd.h>
|
||||
#include <math.h>
|
||||
|
||||
#include <mathlib/math/Limits.hpp>
|
||||
#include <drivers/drv_pwm_output.h>
|
||||
|
||||
#include "mixer.h"
|
||||
|
||||
// This file is generated by the multi_tables script which is invoked during the build process
|
||||
// #include "mixer_multirotor.generated.h"
|
||||
#include "mixer_multirotor_normalized.generated.h"
|
||||
// #include "mixer_multirotor_legacy.generated.h"
|
||||
|
||||
#define debug(fmt, args...) do { } while(0)
|
||||
//#define debug(fmt, args...) do { printf("[mixer] " fmt "\n", ##args); } while(0)
|
||||
//#include <debug.h>
|
||||
//#define debug(fmt, args...) syslog(fmt "\n", ##args)
|
||||
|
||||
/*
|
||||
* Clockwise: 1
|
||||
* Counter-clockwise: -1
|
||||
*/
|
||||
|
||||
MultirotorMixer::MultirotorMixer(ControlCallback control_cb,
|
||||
uintptr_t cb_handle,
|
||||
MultirotorGeometry geometry,
|
||||
float roll_scale,
|
||||
float pitch_scale,
|
||||
float yaw_scale,
|
||||
float idle_speed) :
|
||||
Mixer(control_cb, cb_handle),
|
||||
_roll_scale(roll_scale),
|
||||
_pitch_scale(pitch_scale),
|
||||
_yaw_scale(yaw_scale),
|
||||
_idle_speed(-1.0f + idle_speed * 2.0f), /* shift to output range here to avoid runtime calculation */
|
||||
_delta_out_max(0.0f),
|
||||
_thrust_factor(0.0f),
|
||||
_rotor_count(_config_rotor_count[(MultirotorGeometryUnderlyingType)geometry]),
|
||||
_rotors(_config_index[(MultirotorGeometryUnderlyingType)geometry]),
|
||||
_outputs_prev(new float[_rotor_count])
|
||||
{
|
||||
memset(_outputs_prev, _idle_speed, _rotor_count * sizeof(float));
|
||||
}
|
||||
|
||||
MultirotorMixer::~MultirotorMixer()
|
||||
{
|
||||
if (_outputs_prev != nullptr) {
|
||||
delete[] _outputs_prev;
|
||||
}
|
||||
}
|
||||
|
||||
MultirotorMixer *
|
||||
MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, const char *buf, unsigned &buflen)
|
||||
{
|
||||
MultirotorGeometry geometry = MultirotorGeometry::MAX_GEOMETRY;
|
||||
char geomname[8];
|
||||
int s[4];
|
||||
int used;
|
||||
|
||||
/* enforce that the mixer ends with a new line */
|
||||
if (!string_well_formed(buf, buflen)) {
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
if (sscanf(buf, "R: %7s %d %d %d %d%n", geomname, &s[0], &s[1], &s[2], &s[3], &used) != 5) {
|
||||
debug("multirotor parse failed on '%s'", buf);
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
if (used > (int)buflen) {
|
||||
debug("OVERFLOW: multirotor spec used %d of %u", used, buflen);
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
buf = skipline(buf, buflen);
|
||||
|
||||
if (buf == nullptr) {
|
||||
debug("no line ending, line is incomplete");
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
debug("remaining in buf: %d, first char: %c", buflen, buf[0]);
|
||||
|
||||
for (MultirotorGeometryUnderlyingType i = 0; i < (MultirotorGeometryUnderlyingType)MultirotorGeometry::MAX_GEOMETRY;
|
||||
i++) {
|
||||
if (!strcmp(geomname, _config_key[i])) {
|
||||
geometry = (MultirotorGeometry)i;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
if (geometry == MultirotorGeometry::MAX_GEOMETRY) {
|
||||
debug("unrecognised geometry '%s'", geomname);
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
debug("adding multirotor mixer '%s'", geomname);
|
||||
|
||||
return new MultirotorMixer(
|
||||
control_cb,
|
||||
cb_handle,
|
||||
geometry,
|
||||
s[0] / 10000.0f,
|
||||
s[1] / 10000.0f,
|
||||
s[2] / 10000.0f,
|
||||
s[3] / 10000.0f);
|
||||
}
|
||||
|
||||
unsigned
|
||||
MultirotorMixer::mix(float *outputs, unsigned space)
|
||||
{
|
||||
/* Summary of mixing strategy:
|
||||
1) mix roll, pitch and thrust without yaw.
|
||||
2) if some outputs violate range [0,1] then try to shift all outputs to minimize violation ->
|
||||
increase or decrease total thrust (boost). The total increase or decrease of thrust is limited
|
||||
(max_thrust_diff). If after the shift some outputs still violate the bounds then scale roll & pitch.
|
||||
In case there is violation at the lower and upper bound then try to shift such that violation is equal
|
||||
on both sides.
|
||||
3) mix in yaw and scale if it leads to limit violation.
|
||||
4) scale all outputs to range [idle_speed,1]
|
||||
*/
|
||||
|
||||
float roll = math::constrain(get_control(0, 0) * _roll_scale, -1.0f, 1.0f);
|
||||
float pitch = math::constrain(get_control(0, 1) * _pitch_scale, -1.0f, 1.0f);
|
||||
float yaw = math::constrain(get_control(0, 2) * _yaw_scale, -1.0f, 1.0f);
|
||||
float thrust = math::constrain(get_control(0, 3), 0.0f, 1.0f);
|
||||
float min_out = 1.0f;
|
||||
float max_out = 0.0f;
|
||||
|
||||
// clean out class variable used to capture saturation
|
||||
_saturation_status.value = 0;
|
||||
|
||||
// thrust boost parameters
|
||||
float thrust_increase_factor = 1.5f;
|
||||
float thrust_decrease_factor = 0.6f;
|
||||
|
||||
/* perform initial mix pass yielding unbounded outputs, ignore yaw */
|
||||
for (unsigned i = 0; i < _rotor_count; i++) {
|
||||
float out = roll * _rotors[i].roll_scale +
|
||||
pitch * _rotors[i].pitch_scale +
|
||||
thrust;
|
||||
|
||||
out *= _rotors[i].out_scale;
|
||||
|
||||
/* calculate min and max output values */
|
||||
if (out < min_out) {
|
||||
min_out = out;
|
||||
}
|
||||
|
||||
if (out > max_out) {
|
||||
max_out = out;
|
||||
}
|
||||
|
||||
outputs[i] = out;
|
||||
}
|
||||
|
||||
float boost = 0.0f; // value added to demanded thrust (can also be negative)
|
||||
float roll_pitch_scale = 1.0f; // scale for demanded roll and pitch
|
||||
|
||||
if (min_out < 0.0f && max_out < 1.0f && -min_out <= 1.0f - max_out) {
|
||||
float max_thrust_diff = thrust * thrust_increase_factor - thrust;
|
||||
|
||||
if (max_thrust_diff >= -min_out) {
|
||||
boost = -min_out;
|
||||
|
||||
} else {
|
||||
boost = max_thrust_diff;
|
||||
roll_pitch_scale = (thrust + boost) / (thrust - min_out);
|
||||
}
|
||||
|
||||
} else if (max_out > 1.0f && min_out > 0.0f && min_out >= max_out - 1.0f) {
|
||||
float max_thrust_diff = thrust - thrust_decrease_factor * thrust;
|
||||
|
||||
if (max_thrust_diff >= max_out - 1.0f) {
|
||||
boost = -(max_out - 1.0f);
|
||||
|
||||
} else {
|
||||
boost = -max_thrust_diff;
|
||||
roll_pitch_scale = (1 - (thrust + boost)) / (max_out - thrust);
|
||||
}
|
||||
|
||||
} else if (min_out < 0.0f && max_out < 1.0f && -min_out > 1.0f - max_out) {
|
||||
float max_thrust_diff = thrust * thrust_increase_factor - thrust;
|
||||
boost = math::constrain(-min_out - (1.0f - max_out) / 2.0f, 0.0f, max_thrust_diff);
|
||||
roll_pitch_scale = (thrust + boost) / (thrust - min_out);
|
||||
|
||||
} else if (max_out > 1.0f && min_out > 0.0f && min_out < max_out - 1.0f) {
|
||||
float max_thrust_diff = thrust - thrust_decrease_factor * thrust;
|
||||
boost = math::constrain(-(max_out - 1.0f - min_out) / 2.0f, -max_thrust_diff, 0.0f);
|
||||
roll_pitch_scale = (1 - (thrust + boost)) / (max_out - thrust);
|
||||
|
||||
} else if (min_out < 0.0f && max_out > 1.0f) {
|
||||
boost = math::constrain(-(max_out - 1.0f + min_out) / 2.0f, thrust_decrease_factor * thrust - thrust,
|
||||
thrust_increase_factor * thrust - thrust);
|
||||
roll_pitch_scale = (thrust + boost) / (thrust - min_out);
|
||||
}
|
||||
|
||||
// capture saturation
|
||||
if (min_out < 0.0f) {
|
||||
_saturation_status.flags.motor_neg = true;
|
||||
}
|
||||
|
||||
if (max_out > 1.0f) {
|
||||
_saturation_status.flags.motor_pos = true;
|
||||
}
|
||||
|
||||
// Thrust reduction is used to reduce the collective thrust if we hit
|
||||
// the upper throttle limit
|
||||
float thrust_reduction = 0.0f;
|
||||
|
||||
// mix again but now with thrust boost, scale roll/pitch and also add yaw
|
||||
for (unsigned i = 0; i < _rotor_count; i++) {
|
||||
float out = (roll * _rotors[i].roll_scale +
|
||||
pitch * _rotors[i].pitch_scale) * roll_pitch_scale +
|
||||
yaw * _rotors[i].yaw_scale +
|
||||
thrust + boost;
|
||||
|
||||
out *= _rotors[i].out_scale;
|
||||
|
||||
// scale yaw if it violates limits. inform about yaw limit reached
|
||||
if (out < 0.0f) {
|
||||
if (fabsf(_rotors[i].yaw_scale) <= FLT_EPSILON) {
|
||||
yaw = 0.0f;
|
||||
|
||||
} else {
|
||||
yaw = -((roll * _rotors[i].roll_scale + pitch * _rotors[i].pitch_scale) *
|
||||
roll_pitch_scale + thrust + boost) / _rotors[i].yaw_scale;
|
||||
}
|
||||
|
||||
} else if (out > 1.0f) {
|
||||
// allow to reduce thrust to get some yaw response
|
||||
float prop_reduction = fminf(0.15f, out - 1.0f);
|
||||
// keep the maximum requested reduction
|
||||
thrust_reduction = fmaxf(thrust_reduction, prop_reduction);
|
||||
|
||||
if (fabsf(_rotors[i].yaw_scale) <= FLT_EPSILON) {
|
||||
yaw = 0.0f;
|
||||
|
||||
} else {
|
||||
yaw = (1.0f - ((roll * _rotors[i].roll_scale + pitch * _rotors[i].pitch_scale) *
|
||||
roll_pitch_scale + (thrust - thrust_reduction) + boost)) / _rotors[i].yaw_scale;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Apply collective thrust reduction, the maximum for one prop
|
||||
thrust -= thrust_reduction;
|
||||
|
||||
// add yaw and scale outputs to range idle_speed...1
|
||||
for (unsigned i = 0; i < _rotor_count; i++) {
|
||||
outputs[i] = (roll * _rotors[i].roll_scale +
|
||||
pitch * _rotors[i].pitch_scale) * roll_pitch_scale +
|
||||
yaw * _rotors[i].yaw_scale +
|
||||
thrust + boost;
|
||||
|
||||
/*
|
||||
implement simple model for static relationship between applied motor pwm and motor thrust
|
||||
model: thrust = (1 - _thrust_factor) * PWM + _thrust_factor * PWM^2
|
||||
this model assumes normalized input / output in the range [0,1] so this is the right place
|
||||
to do it as at this stage the outputs are in that range.
|
||||
*/
|
||||
if (_thrust_factor > 0.0f) {
|
||||
outputs[i] = -(1.0f - _thrust_factor) / (2.0f * _thrust_factor) + sqrtf((1.0f - _thrust_factor) *
|
||||
(1.0f - _thrust_factor) / (4.0f * _thrust_factor * _thrust_factor) + (outputs[i] < 0.0f ? 0.0f : outputs[i] /
|
||||
_thrust_factor));
|
||||
}
|
||||
|
||||
outputs[i] = math::constrain(_idle_speed + (outputs[i] * (1.0f - _idle_speed)), _idle_speed, 1.0f);
|
||||
|
||||
}
|
||||
|
||||
/* slew rate limiting and saturation checking */
|
||||
for (unsigned i = 0; i < _rotor_count; i++) {
|
||||
bool clipping_high = false;
|
||||
bool clipping_low = false;
|
||||
|
||||
// check for saturation against static limits
|
||||
if (outputs[i] > 0.99f) {
|
||||
clipping_high = true;
|
||||
|
||||
} else if (outputs[i] < _idle_speed + 0.01f) {
|
||||
clipping_low = true;
|
||||
|
||||
}
|
||||
|
||||
// check for saturation against slew rate limits
|
||||
if (_delta_out_max > 0.0f) {
|
||||
float delta_out = outputs[i] - _outputs_prev[i];
|
||||
|
||||
if (delta_out > _delta_out_max) {
|
||||
outputs[i] = _outputs_prev[i] + _delta_out_max;
|
||||
clipping_high = true;
|
||||
|
||||
} else if (delta_out < -_delta_out_max) {
|
||||
outputs[i] = _outputs_prev[i] - _delta_out_max;
|
||||
clipping_low = true;
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
_outputs_prev[i] = outputs[i];
|
||||
|
||||
// update the saturation status report
|
||||
update_saturation_status(i, clipping_high, clipping_low);
|
||||
}
|
||||
|
||||
// this will force the caller of the mixer to always supply new slew rate values, otherwise no slew rate limiting will happen
|
||||
_delta_out_max = 0.0f;
|
||||
|
||||
return _rotor_count;
|
||||
}
|
||||
|
||||
/*
|
||||
* This function update the control saturation status report using the following inputs:
|
||||
*
|
||||
* index: 0 based index identifying the motor that is saturating
|
||||
* clipping_high: true if the motor demand is being limited in the positive direction
|
||||
* clipping_low: true if the motor demand is being limited in the negative direction
|
||||
*/
|
||||
void
|
||||
MultirotorMixer::update_saturation_status(unsigned index, bool clipping_high, bool clipping_low)
|
||||
{
|
||||
// The motor is saturated at the upper limit
|
||||
// check which control axes and which directions are contributing
|
||||
if (clipping_high) {
|
||||
if (_rotors[index].roll_scale > 0.0f) {
|
||||
// A positive change in roll will increase saturation
|
||||
_saturation_status.flags.roll_pos = true;
|
||||
|
||||
} else if (_rotors[index].roll_scale < 0.0f) {
|
||||
// A negative change in roll will increase saturation
|
||||
_saturation_status.flags.roll_neg = true;
|
||||
}
|
||||
|
||||
// check if the pitch input is saturating
|
||||
if (_rotors[index].pitch_scale > 0.0f) {
|
||||
// A positive change in pitch will increase saturation
|
||||
_saturation_status.flags.pitch_pos = true;
|
||||
|
||||
} else if (_rotors[index].pitch_scale < 0.0f) {
|
||||
// A negative change in pitch will increase saturation
|
||||
_saturation_status.flags.pitch_neg = true;
|
||||
}
|
||||
|
||||
// check if the yaw input is saturating
|
||||
if (_rotors[index].yaw_scale > 0.0f) {
|
||||
// A positive change in yaw will increase saturation
|
||||
_saturation_status.flags.yaw_pos = true;
|
||||
|
||||
} else if (_rotors[index].yaw_scale < 0.0f) {
|
||||
// A negative change in yaw will increase saturation
|
||||
_saturation_status.flags.yaw_neg = true;
|
||||
}
|
||||
|
||||
// A positive change in thrust will increase saturation
|
||||
_saturation_status.flags.thrust_pos = true;
|
||||
|
||||
}
|
||||
|
||||
// The motor is saturated at the lower limit
|
||||
// check which control axes and which directions are contributing
|
||||
if (clipping_low) {
|
||||
// check if the roll input is saturating
|
||||
if (_rotors[index].roll_scale > 0.0f) {
|
||||
// A negative change in roll will increase saturation
|
||||
_saturation_status.flags.roll_neg = true;
|
||||
|
||||
} else if (_rotors[index].roll_scale < 0.0f) {
|
||||
// A positive change in roll will increase saturation
|
||||
_saturation_status.flags.roll_pos = true;
|
||||
}
|
||||
|
||||
// check if the pitch input is saturating
|
||||
if (_rotors[index].pitch_scale > 0.0f) {
|
||||
// A negative change in pitch will increase saturation
|
||||
_saturation_status.flags.pitch_neg = true;
|
||||
|
||||
} else if (_rotors[index].pitch_scale < 0.0f) {
|
||||
// A positive change in pitch will increase saturation
|
||||
_saturation_status.flags.pitch_pos = true;
|
||||
}
|
||||
|
||||
// check if the yaw input is saturating
|
||||
if (_rotors[index].yaw_scale > 0.0f) {
|
||||
// A negative change in yaw will increase saturation
|
||||
_saturation_status.flags.yaw_neg = true;
|
||||
|
||||
} else if (_rotors[index].yaw_scale < 0.0f) {
|
||||
// A positive change in yaw will increase saturation
|
||||
_saturation_status.flags.yaw_pos = true;
|
||||
}
|
||||
|
||||
// A negative change in thrust will increase saturation
|
||||
_saturation_status.flags.thrust_neg = true;
|
||||
}
|
||||
|
||||
_saturation_status.flags.valid = true;
|
||||
}
|
||||
|
||||
void
|
||||
MultirotorMixer::groups_required(uint32_t &groups)
|
||||
{
|
||||
/* XXX for now, hardcoded to indexes 0-3 in control group zero */
|
||||
groups |= (1 << 0);
|
||||
}
|
||||
|
||||
uint16_t MultirotorMixer::get_saturation_status()
|
||||
{
|
||||
return _saturation_status.value;
|
||||
}
|
||||
@@ -1,369 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2012-2015 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file mixer_simple.cpp
|
||||
*
|
||||
* Simple summing mixer.
|
||||
*/
|
||||
|
||||
#include <px4_config.h>
|
||||
|
||||
#include <sys/types.h>
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <fcntl.h>
|
||||
#include <poll.h>
|
||||
#include <errno.h>
|
||||
#include <stdio.h>
|
||||
#include <math.h>
|
||||
#include <unistd.h>
|
||||
#include <ctype.h>
|
||||
|
||||
#include "mixer.h"
|
||||
|
||||
#define debug(fmt, args...) do { } while(0)
|
||||
//#define debug(fmt, args...) do { printf("[mixer] " fmt "\n", ##args); } while(0)
|
||||
|
||||
SimpleMixer::SimpleMixer(ControlCallback control_cb,
|
||||
uintptr_t cb_handle,
|
||||
mixer_simple_s *mixinfo) :
|
||||
Mixer(control_cb, cb_handle),
|
||||
_pinfo(mixinfo)
|
||||
{
|
||||
}
|
||||
|
||||
SimpleMixer::~SimpleMixer()
|
||||
{
|
||||
if (_pinfo != nullptr) {
|
||||
free(_pinfo);
|
||||
}
|
||||
}
|
||||
|
||||
unsigned SimpleMixer::set_trim(float trim)
|
||||
{
|
||||
_pinfo->output_scaler.offset = trim;
|
||||
return 1;
|
||||
}
|
||||
|
||||
int
|
||||
SimpleMixer::parse_output_scaler(const char *buf, unsigned &buflen, mixer_scaler_s &scaler)
|
||||
{
|
||||
int ret;
|
||||
int s[5];
|
||||
int n = -1;
|
||||
|
||||
buf = findtag(buf, buflen, 'O');
|
||||
|
||||
if ((buf == nullptr) || (buflen < 12)) {
|
||||
debug("output parser failed finding tag, ret: '%s'", buf);
|
||||
return -1;
|
||||
}
|
||||
|
||||
if ((ret = sscanf(buf, "O: %d %d %d %d %d %n",
|
||||
&s[0], &s[1], &s[2], &s[3], &s[4], &n)) != 5) {
|
||||
debug("out scaler parse failed on '%s' (got %d, consumed %d)", buf, ret, n);
|
||||
return -1;
|
||||
}
|
||||
|
||||
buf = skipline(buf, buflen);
|
||||
|
||||
if (buf == nullptr) {
|
||||
debug("no line ending, line is incomplete");
|
||||
return -1;
|
||||
}
|
||||
|
||||
scaler.negative_scale = s[0] / 10000.0f;
|
||||
scaler.positive_scale = s[1] / 10000.0f;
|
||||
scaler.offset = s[2] / 10000.0f;
|
||||
scaler.min_output = s[3] / 10000.0f;
|
||||
scaler.max_output = s[4] / 10000.0f;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
int
|
||||
SimpleMixer::parse_control_scaler(const char *buf, unsigned &buflen, mixer_scaler_s &scaler, uint8_t &control_group,
|
||||
uint8_t &control_index)
|
||||
{
|
||||
unsigned u[2];
|
||||
int s[5];
|
||||
|
||||
buf = findtag(buf, buflen, 'S');
|
||||
|
||||
if ((buf == nullptr) || (buflen < 16)) {
|
||||
debug("control parser failed finding tag, ret: '%s'", buf);
|
||||
return -1;
|
||||
}
|
||||
|
||||
if (sscanf(buf, "S: %u %u %d %d %d %d %d",
|
||||
&u[0], &u[1], &s[0], &s[1], &s[2], &s[3], &s[4]) != 7) {
|
||||
debug("control parse failed on '%s'", buf);
|
||||
return -1;
|
||||
}
|
||||
|
||||
buf = skipline(buf, buflen);
|
||||
|
||||
if (buf == nullptr) {
|
||||
debug("no line ending, line is incomplete");
|
||||
return -1;
|
||||
}
|
||||
|
||||
control_group = u[0];
|
||||
control_index = u[1];
|
||||
scaler.negative_scale = s[0] / 10000.0f;
|
||||
scaler.positive_scale = s[1] / 10000.0f;
|
||||
scaler.offset = s[2] / 10000.0f;
|
||||
scaler.min_output = s[3] / 10000.0f;
|
||||
scaler.max_output = s[4] / 10000.0f;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
SimpleMixer *
|
||||
SimpleMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, const char *buf, unsigned &buflen)
|
||||
{
|
||||
SimpleMixer *sm = nullptr;
|
||||
mixer_simple_s *mixinfo = nullptr;
|
||||
unsigned inputs;
|
||||
int used;
|
||||
const char *end = buf + buflen;
|
||||
|
||||
/* enforce that the mixer ends with a new line */
|
||||
if (!string_well_formed(buf, buflen)) {
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
/* get the base info for the mixer */
|
||||
if (sscanf(buf, "M: %u%n", &inputs, &used) != 1) {
|
||||
debug("simple parse failed on '%s'", buf);
|
||||
goto out;
|
||||
}
|
||||
|
||||
buf = skipline(buf, buflen);
|
||||
|
||||
if (buf == nullptr) {
|
||||
debug("no line ending, line is incomplete");
|
||||
goto out;
|
||||
}
|
||||
|
||||
mixinfo = (mixer_simple_s *)malloc(MIXER_SIMPLE_SIZE(inputs));
|
||||
|
||||
if (mixinfo == nullptr) {
|
||||
debug("could not allocate memory for mixer info");
|
||||
goto out;
|
||||
}
|
||||
|
||||
mixinfo->control_count = inputs;
|
||||
|
||||
if (parse_output_scaler(end - buflen, buflen, mixinfo->output_scaler)) {
|
||||
debug("simple mixer parser failed parsing out scaler tag, ret: '%s'", buf);
|
||||
goto out;
|
||||
}
|
||||
|
||||
for (unsigned i = 0; i < inputs; i++) {
|
||||
if (parse_control_scaler(end - buflen, buflen,
|
||||
mixinfo->controls[i].scaler,
|
||||
mixinfo->controls[i].control_group,
|
||||
mixinfo->controls[i].control_index)) {
|
||||
debug("simple mixer parser failed parsing ctrl scaler tag, ret: '%s'", buf);
|
||||
goto out;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
sm = new SimpleMixer(control_cb, cb_handle, mixinfo);
|
||||
|
||||
if (sm != nullptr) {
|
||||
mixinfo = nullptr;
|
||||
debug("loaded mixer with %d input(s)", inputs);
|
||||
|
||||
} else {
|
||||
debug("could not allocate memory for mixer");
|
||||
}
|
||||
|
||||
out:
|
||||
|
||||
if (mixinfo != nullptr) {
|
||||
free(mixinfo);
|
||||
}
|
||||
|
||||
return sm;
|
||||
}
|
||||
|
||||
SimpleMixer *
|
||||
SimpleMixer::pwm_input(Mixer::ControlCallback control_cb, uintptr_t cb_handle, unsigned input, uint16_t min,
|
||||
uint16_t mid, uint16_t max)
|
||||
{
|
||||
SimpleMixer *sm = nullptr;
|
||||
mixer_simple_s *mixinfo = nullptr;
|
||||
|
||||
mixinfo = (mixer_simple_s *)malloc(MIXER_SIMPLE_SIZE(1));
|
||||
|
||||
if (mixinfo == nullptr) {
|
||||
debug("could not allocate memory for mixer info");
|
||||
goto out;
|
||||
}
|
||||
|
||||
mixinfo->control_count = 1;
|
||||
|
||||
/*
|
||||
* Always pull from group 0, with the input value giving the channel.
|
||||
*/
|
||||
mixinfo->controls[0].control_group = 0;
|
||||
mixinfo->controls[0].control_index = input;
|
||||
|
||||
/*
|
||||
* Conversion uses both the input and output side of the mixer.
|
||||
*
|
||||
* The input side is used to slide the control value such that the min argument
|
||||
* results in a value of zero.
|
||||
*
|
||||
* The output side is used to apply the scaling for the min/max values so that
|
||||
* the resulting output is a -1.0 ... 1.0 value for the min...max range.
|
||||
*/
|
||||
mixinfo->controls[0].scaler.negative_scale = 1.0f;
|
||||
mixinfo->controls[0].scaler.positive_scale = 1.0f;
|
||||
mixinfo->controls[0].scaler.offset = -mid;
|
||||
mixinfo->controls[0].scaler.min_output = -(mid - min);
|
||||
mixinfo->controls[0].scaler.max_output = (max - mid);
|
||||
|
||||
mixinfo->output_scaler.negative_scale = 500.0f / (mid - min);
|
||||
mixinfo->output_scaler.positive_scale = 500.0f / (max - mid);
|
||||
mixinfo->output_scaler.offset = 0.0f;
|
||||
mixinfo->output_scaler.min_output = -1.0f;
|
||||
mixinfo->output_scaler.max_output = 1.0f;
|
||||
|
||||
sm = new SimpleMixer(control_cb, cb_handle, mixinfo);
|
||||
|
||||
if (sm != nullptr) {
|
||||
mixinfo = nullptr;
|
||||
debug("PWM input mixer for %d", input);
|
||||
|
||||
} else {
|
||||
debug("could not allocate memory for PWM input mixer");
|
||||
}
|
||||
|
||||
out:
|
||||
|
||||
if (mixinfo != nullptr) {
|
||||
free(mixinfo);
|
||||
}
|
||||
|
||||
return sm;
|
||||
}
|
||||
|
||||
unsigned
|
||||
SimpleMixer::mix(float *outputs, unsigned space)
|
||||
{
|
||||
float sum = 0.0f;
|
||||
|
||||
if (_pinfo == nullptr) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
if (space < 1) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
for (unsigned i = 0; i < _pinfo->control_count; i++) {
|
||||
float input = 0.0f;
|
||||
|
||||
_control_cb(_cb_handle,
|
||||
_pinfo->controls[i].control_group,
|
||||
_pinfo->controls[i].control_index,
|
||||
input);
|
||||
|
||||
sum += scale(_pinfo->controls[i].scaler, input);
|
||||
}
|
||||
|
||||
*outputs = scale(_pinfo->output_scaler, sum);
|
||||
return 1;
|
||||
}
|
||||
|
||||
uint16_t
|
||||
SimpleMixer::get_saturation_status()
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
|
||||
void
|
||||
SimpleMixer::groups_required(uint32_t &groups)
|
||||
{
|
||||
for (unsigned i = 0; i < _pinfo->control_count; i++) {
|
||||
groups |= 1 << _pinfo->controls[i].control_group;
|
||||
}
|
||||
}
|
||||
|
||||
int
|
||||
SimpleMixer::check()
|
||||
{
|
||||
int ret;
|
||||
float junk;
|
||||
|
||||
/* sanity that presumes that a mixer includes a control no more than once */
|
||||
/* max of 32 groups due to groups_required API */
|
||||
if (_pinfo->control_count > 32) {
|
||||
return -2;
|
||||
}
|
||||
|
||||
/* validate the output scaler */
|
||||
ret = scale_check(_pinfo->output_scaler);
|
||||
|
||||
if (ret != 0) {
|
||||
return ret;
|
||||
}
|
||||
|
||||
/* validate input scalers */
|
||||
for (unsigned i = 0; i < _pinfo->control_count; i++) {
|
||||
|
||||
/* verify that we can fetch the control */
|
||||
if (_control_cb(_cb_handle,
|
||||
_pinfo->controls[i].control_group,
|
||||
_pinfo->controls[i].control_index,
|
||||
junk) != 0) {
|
||||
return -3;
|
||||
}
|
||||
|
||||
/* validate the scaler */
|
||||
ret = scale_check(_pinfo->controls[i].scaler);
|
||||
|
||||
if (ret != 0) {
|
||||
return (10 * i + ret);
|
||||
}
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
@@ -1,284 +0,0 @@
|
||||
#!/usr/bin/env python
|
||||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2013, 2014 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.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
#
|
||||
# Generate multirotor mixer scale tables compatible with the ArduCopter layout
|
||||
#
|
||||
|
||||
# for python2.7 compatibility
|
||||
from __future__ import print_function
|
||||
|
||||
import math
|
||||
|
||||
print("/*")
|
||||
print("* This file is automatically generated by multi_tables - do not edit.")
|
||||
print("*/")
|
||||
print("")
|
||||
print("#ifndef _MIXER_MULTI_TABLES")
|
||||
print("#define _MIXER_MULTI_TABLES")
|
||||
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_h = [
|
||||
[ 45, CW],
|
||||
[-135, CW],
|
||||
[-45, CCW],
|
||||
[135, CCW],
|
||||
]
|
||||
|
||||
quad_plus = [
|
||||
[ 90, CCW],
|
||||
[ -90, CCW],
|
||||
[ 0, CW],
|
||||
[ 180, CW],
|
||||
]
|
||||
|
||||
quad_deadcat = [
|
||||
[ 60, CCW, 1.0],
|
||||
[-125, CCW, 0.92],
|
||||
[ -60, CW, 1.0],
|
||||
[ 125, CW, 0.92],
|
||||
]
|
||||
|
||||
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],
|
||||
]
|
||||
|
||||
quad_s250aq = [
|
||||
[ 59, CCW, 1.0 ],
|
||||
[ -139, CCW, 0.67],
|
||||
[ -59, CW, 1.0 ],
|
||||
[ 139, CW, 0.67],
|
||||
]
|
||||
|
||||
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],
|
||||
]
|
||||
|
||||
hex_t = [
|
||||
[ 43.21, CCW],
|
||||
[ 43.21, CW],
|
||||
[ 180, CW],
|
||||
[ 180, CCW],
|
||||
[ -43.21, CW],
|
||||
[ -43.21, 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],
|
||||
]
|
||||
|
||||
octa_cox_wide = [
|
||||
[ 68, CCW],
|
||||
[ -68, CW],
|
||||
[-129, CCW],
|
||||
[ 129, CW],
|
||||
[ -68, CCW],
|
||||
[ 68, CW],
|
||||
[ 129, CCW],
|
||||
[-129, CW],
|
||||
]
|
||||
|
||||
twin_engine = [
|
||||
[ 90, 0.0],
|
||||
[-90, 0.0],
|
||||
]
|
||||
|
||||
tri_y = [
|
||||
[ 60, 0.0],
|
||||
[ -60, 0.0],
|
||||
[ 180, 0.0],
|
||||
]
|
||||
|
||||
dodeca_top_cox = [
|
||||
[ 90, CW],
|
||||
[ -90, CCW],
|
||||
[ -30, CW],
|
||||
[ 150, CCW],
|
||||
[ 30, CCW],
|
||||
[-150, CW],
|
||||
]
|
||||
|
||||
dodeca_bottom_cox = [
|
||||
[ 90, CCW],
|
||||
[ -90, CW],
|
||||
[ -30, CCW],
|
||||
[ 150, CW],
|
||||
[ 30, CW],
|
||||
[-150, CCW],
|
||||
]
|
||||
|
||||
tables = [quad_x, quad_h, quad_plus, quad_v, quad_wide, quad_s250aq, quad_deadcat,
|
||||
hex_x, hex_plus, hex_cox, hex_t,
|
||||
octa_x, octa_plus, octa_cox, octa_cox_wide,
|
||||
twin_engine, tri_y,
|
||||
dodeca_top_cox, dodeca_bottom_cox]
|
||||
keys = ["4x", "4h", "4+", "4v", "4w", "4s", "4dc",
|
||||
"6x", "6+", "6c", "6t",
|
||||
"8x", "8+", "8c", "8cw",
|
||||
"2-", "3y",
|
||||
"6m", "6a"]
|
||||
|
||||
def variableName(variable):
|
||||
for variableName, value in list(globals().items()):
|
||||
if value is variable:
|
||||
return variableName
|
||||
|
||||
def unpackScales(scalesList):
|
||||
if len(scalesList) == 2:
|
||||
scalesList += [1.0] #Add thrust scale
|
||||
return scalesList
|
||||
|
||||
def printEnum():
|
||||
print("enum class MultirotorGeometry : MultirotorGeometryUnderlyingType {")
|
||||
for table in tables:
|
||||
print("\t{},".format(variableName(table).upper()))
|
||||
|
||||
print("\n\tMAX_GEOMETRY")
|
||||
print("}; // enum class MultirotorGeometry\n")
|
||||
|
||||
def printScaleTables():
|
||||
for table in tables:
|
||||
print("const MultirotorMixer::Rotor _config_{}[] = {{".format(variableName(table)))
|
||||
for row in table:
|
||||
angle, yawScale, thrustScale = unpackScales(row)
|
||||
rollScale = rcos(angle + 90)
|
||||
pitchScale = rcos(angle)
|
||||
print("\t{{ {:9f}, {:9f}, {:9f}, {:9f} }},".format(rollScale, pitchScale, yawScale, thrustScale))
|
||||
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")
|
||||
|
||||
def printScaleTablesKeys():
|
||||
print("const char* _config_key[] = {")
|
||||
for key, table in zip(keys, tables):
|
||||
print("\t\"{}\",\t/* {} */".format(key, variableName(table)))
|
||||
print("};\n")
|
||||
|
||||
printEnum()
|
||||
|
||||
print("namespace {")
|
||||
printScaleTables()
|
||||
printScaleTablesIndex()
|
||||
printScaleTablesCounts()
|
||||
printScaleTablesKeys()
|
||||
|
||||
print("} // anonymous namespace\n")
|
||||
print("#endif /* _MIXER_MULTI_TABLES */")
|
||||
print("")
|
||||
Reference in New Issue
Block a user