mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-05-02 05:04:08 +08:00
Omni Mixer: The mixer for 6-DoF vehicles is refactored to the MultirotorMixer now
This commit is contained in:
Azarakhsh Keipour
parent
d5ff0d8ea6
commit
69fe37e6be
@ -201,7 +201,7 @@ MixerGroup::load_from_buf(Mixer::ControlCallback control_cb, uintptr_t cb_handle
|
||||
break;
|
||||
|
||||
case 'S':
|
||||
m = MultirotorMixer6dof::from_text(control_cb, cb_handle, p, resid);
|
||||
m = MultirotorMixer::from_text(control_cb, cb_handle, p, resid);
|
||||
break;
|
||||
|
||||
case 'H':
|
||||
|
||||
@ -76,32 +76,16 @@ add_custom_command(
|
||||
)
|
||||
add_custom_target(mixer_gen DEPENDS mixer_multirotor.generated.h ${CMAKE_CURRENT_BINARY_DIR}/mixer_multirotor_normalized.generated.h)
|
||||
|
||||
# 6DOF mixers
|
||||
add_custom_command(
|
||||
OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/mixer_multirotor_6dof.generated.h
|
||||
COMMAND ${PYTHON_EXECUTABLE} ${MIXER_TOOLS}/px_generate_mixers.py --sixdof -f ${geometries_list} -o mixer_multirotor_6dof.generated.h
|
||||
DEPENDS ${MIXER_TOOLS}/px_generate_mixers.py ${geometries_list}
|
||||
)
|
||||
add_custom_command(
|
||||
OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/mixer_multirotor_6dof_normalized.generated.h
|
||||
COMMAND ${PYTHON_EXECUTABLE} ${MIXER_TOOLS}/px_generate_mixers.py --normalize --sixdof -f ${geometries_list} -o mixer_multirotor_6dof_normalized.generated.h
|
||||
DEPENDS ${MIXER_TOOLS}/px_generate_mixers.py ${geometries_list}
|
||||
)
|
||||
add_custom_target(mixer_gen_6dof DEPENDS mixer_multirotor_6dof.generated.h ${CMAKE_CURRENT_BINARY_DIR}/mixer_multirotor_6dof_normalized.generated.h)
|
||||
|
||||
add_library(MultirotorMixer
|
||||
MultirotorMixer.cpp
|
||||
MultirotorMixer.hpp
|
||||
MultirotorMixer6dof.cpp
|
||||
|
||||
${CMAKE_CURRENT_BINARY_DIR}/mixer_multirotor.generated.h
|
||||
${CMAKE_CURRENT_BINARY_DIR}/mixer_multirotor_normalized.generated.h
|
||||
${CMAKE_CURRENT_BINARY_DIR}/mixer_multirotor_6dof.generated.h
|
||||
${CMAKE_CURRENT_BINARY_DIR}/mixer_multirotor_6dof_normalized.generated.h
|
||||
)
|
||||
target_link_libraries(MultirotorMixer PRIVATE Mixer)
|
||||
target_include_directories(MultirotorMixer PRIVATE ${CMAKE_CURRENT_BINARY_DIR})
|
||||
add_dependencies(MultirotorMixer mixer_gen mixer_gen_6dof prebuild_targets)
|
||||
add_dependencies(MultirotorMixer mixer_gen prebuild_targets)
|
||||
|
||||
if(BUILD_TESTING)
|
||||
|
||||
|
||||
@ -1,6 +1,6 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2012-2018 PX4 Development Team. All rights reserved.
|
||||
* Copyright (c) 2012-2019 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
|
||||
@ -87,6 +87,22 @@ MultirotorMixer::MultirotorMixer(ControlCallback control_cb, uintptr_t cb_handle
|
||||
_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 */
|
||||
_is_6dof = false;
|
||||
}
|
||||
|
||||
MultirotorMixer::MultirotorMixer(ControlCallback control_cb, uintptr_t cb_handle, MultirotorGeometry geometry,
|
||||
float roll_scale, float pitch_scale, float yaw_scale,
|
||||
float x_scale, float y_scale, float z_scale, float idle_speed) :
|
||||
MultirotorMixer(control_cb, cb_handle, _config_index_6dof[(int)geometry], _config_rotor_count[(int)geometry])
|
||||
{
|
||||
_roll_scale = roll_scale;
|
||||
_pitch_scale = pitch_scale;
|
||||
_yaw_scale = yaw_scale;
|
||||
_x_scale = x_scale;
|
||||
_y_scale = y_scale;
|
||||
_z_scale = z_scale;
|
||||
_idle_speed = -1.0f + idle_speed * 2.0f; /* shift to output range here to avoid runtime calculation */
|
||||
_is_6dof = true;
|
||||
}
|
||||
|
||||
MultirotorMixer::MultirotorMixer(ControlCallback control_cb, uintptr_t cb_handle, const Rotor *rotors,
|
||||
@ -102,6 +118,19 @@ MultirotorMixer::MultirotorMixer(ControlCallback control_cb, uintptr_t cb_handle
|
||||
}
|
||||
}
|
||||
|
||||
MultirotorMixer::MultirotorMixer(ControlCallback control_cb, uintptr_t cb_handle, const Rotor6Dof *rotors,
|
||||
unsigned rotor_count) :
|
||||
Mixer(control_cb, cb_handle),
|
||||
_rotor_count(rotor_count),
|
||||
_rotors_6dof(rotors),
|
||||
_outputs_prev(new float[_rotor_count]),
|
||||
_tmp_array(new float[_rotor_count])
|
||||
{
|
||||
for (unsigned i = 0; i < _rotor_count; ++i) {
|
||||
_outputs_prev[i] = _idle_speed;
|
||||
}
|
||||
}
|
||||
|
||||
MultirotorMixer::~MultirotorMixer()
|
||||
{
|
||||
delete[] _outputs_prev;
|
||||
@ -113,7 +142,7 @@ MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handl
|
||||
{
|
||||
MultirotorGeometry geometry = MultirotorGeometry::MAX_GEOMETRY;
|
||||
char geomname[8];
|
||||
int s[4];
|
||||
int s[7];
|
||||
int used;
|
||||
|
||||
/* enforce that the mixer ends with a new line */
|
||||
@ -121,9 +150,20 @@ MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handl
|
||||
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;
|
||||
bool is_6dof = (buf[0] == 'S');
|
||||
|
||||
if (is_6dof) {
|
||||
if (sscanf(buf, "S: %7s %d %d %d %d %d %d %d%n", geomname, &s[0], &s[1], &s[2], &s[3], &s[4], &s[5], &s[6],
|
||||
&used) != 8) {
|
||||
debug("multirotor parse failed on '%s'", buf);
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
} else {
|
||||
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) {
|
||||
@ -155,14 +195,29 @@ MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handl
|
||||
|
||||
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);
|
||||
if (is_6dof) {
|
||||
return new MultirotorMixer(
|
||||
control_cb,
|
||||
cb_handle,
|
||||
geometry,
|
||||
s[0] / 10000.0f,
|
||||
s[1] / 10000.0f,
|
||||
s[2] / 10000.0f,
|
||||
s[3] / 10000.0f,
|
||||
s[4] / 10000.0f,
|
||||
s[5] / 10000.0f,
|
||||
s[6] / 10000.0f);
|
||||
|
||||
} else {
|
||||
return new MultirotorMixer(
|
||||
control_cb,
|
||||
cb_handle,
|
||||
geometry,
|
||||
s[0] / 10000.0f,
|
||||
s[1] / 10000.0f,
|
||||
s[2] / 10000.0f,
|
||||
s[3] / 10000.0f);
|
||||
}
|
||||
}
|
||||
|
||||
float
|
||||
@ -310,14 +365,52 @@ MultirotorMixer::mix_airmode_disabled(float roll, float pitch, float yaw, float
|
||||
mix_yaw(yaw, outputs);
|
||||
}
|
||||
|
||||
void
|
||||
MultirotorMixer::mix_airmode_disabled(float roll, float pitch, float yaw, float x_thrust, float y_thrust,
|
||||
float z_thrust, float *outputs)
|
||||
{
|
||||
// Airmode disabled: never allow to increase the thrust to unsaturate a motor
|
||||
|
||||
// Mix without yaw
|
||||
for (unsigned i = 0; i < _rotor_count; i++) {
|
||||
outputs[i] = roll * _rotors_6dof[i].roll_scale +
|
||||
pitch * _rotors_6dof[i].pitch_scale +
|
||||
x_thrust * _rotors_6dof[i].x_scale +
|
||||
y_thrust * _rotors_6dof[i].y_scale +
|
||||
z_thrust * _rotors_6dof[i].z_scale;
|
||||
|
||||
// Z thrust will be used to unsaturate if needed
|
||||
_tmp_array[i] = math::abs_t<float>(_rotors_6dof[i].z_scale);
|
||||
}
|
||||
|
||||
// only reduce thrust
|
||||
minimize_saturation(_tmp_array, outputs, _saturation_status, 0.f, 1.f, true);
|
||||
|
||||
// Reduce roll/pitch acceleration if needed to unsaturate
|
||||
for (unsigned i = 0; i < _rotor_count; i++) {
|
||||
_tmp_array[i] = _rotors_6dof[i].roll_scale;
|
||||
}
|
||||
|
||||
minimize_saturation(_tmp_array, outputs, _saturation_status);
|
||||
|
||||
for (unsigned i = 0; i < _rotor_count; i++) {
|
||||
_tmp_array[i] = _rotors_6dof[i].pitch_scale;
|
||||
}
|
||||
|
||||
minimize_saturation(_tmp_array, outputs, _saturation_status);
|
||||
|
||||
// Mix yaw independently
|
||||
mix_yaw(yaw, outputs);
|
||||
}
|
||||
|
||||
void MultirotorMixer::mix_yaw(float yaw, float *outputs)
|
||||
{
|
||||
// Add yaw to outputs
|
||||
for (unsigned i = 0; i < _rotor_count; i++) {
|
||||
outputs[i] += yaw * _rotors[i].yaw_scale;
|
||||
outputs[i] += yaw * (_is_6dof ? _rotors_6dof[i].yaw_scale : _rotors[i].yaw_scale);
|
||||
|
||||
// Yaw will be used to unsaturate if needed
|
||||
_tmp_array[i] = _rotors[i].yaw_scale;
|
||||
_tmp_array[i] = (_is_6dof ? _rotors_6dof[i].yaw_scale : _rotors[i].yaw_scale);
|
||||
}
|
||||
|
||||
// Change yaw acceleration to unsaturate the outputs if needed (do not change roll/pitch),
|
||||
@ -325,7 +418,7 @@ void MultirotorMixer::mix_yaw(float yaw, float *outputs)
|
||||
minimize_saturation(_tmp_array, outputs, _saturation_status, 0.f, 1.15f);
|
||||
|
||||
for (unsigned i = 0; i < _rotor_count; i++) {
|
||||
_tmp_array[i] = _rotors[i].thrust_scale;
|
||||
_tmp_array[i] = (_is_6dof ? _rotors_6dof[i].z_scale : _rotors[i].thrust_scale);
|
||||
}
|
||||
|
||||
// reduce thrust only
|
||||
@ -342,25 +435,33 @@ MultirotorMixer::mix(float *outputs, unsigned space)
|
||||
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);
|
||||
|
||||
// clean out class variable used to capture saturation
|
||||
_saturation_status.value = 0;
|
||||
|
||||
// Do the mixing using the strategy given by the current Airmode configuration
|
||||
switch (_airmode) {
|
||||
case Airmode::roll_pitch:
|
||||
mix_airmode_rp(roll, pitch, yaw, thrust, outputs);
|
||||
break;
|
||||
if (!_is_6dof)
|
||||
|
||||
case Airmode::roll_pitch_yaw:
|
||||
mix_airmode_rpy(roll, pitch, yaw, thrust, outputs);
|
||||
break;
|
||||
// Do the mixing using the strategy given by the current Airmode configuration
|
||||
switch (_airmode) {
|
||||
case Airmode::roll_pitch:
|
||||
mix_airmode_rp(roll, pitch, yaw, math::constrain(get_control(0, 3), 0.0f, 1.0f), outputs);
|
||||
break;
|
||||
|
||||
case Airmode::disabled:
|
||||
default: // just in case: default to disabled
|
||||
mix_airmode_disabled(roll, pitch, yaw, thrust, outputs);
|
||||
break;
|
||||
case Airmode::roll_pitch_yaw:
|
||||
mix_airmode_rpy(roll, pitch, yaw, math::constrain(get_control(0, 3), 0.0f, 1.0f), outputs);
|
||||
break;
|
||||
|
||||
case Airmode::disabled:
|
||||
default: // just in case: default to disabled
|
||||
mix_airmode_disabled(roll, pitch, yaw, math::constrain(get_control(0, 3), 0.0f, 1.0f), outputs);
|
||||
break;
|
||||
|
||||
} else { // if is 6-DoF vehicle
|
||||
float x_thrust = math::constrain(get_control(0, 8), -1.0f, 1.0f);
|
||||
float y_thrust = math::constrain(get_control(0, 9), -1.0f, 1.0f);
|
||||
float z_thrust = math::constrain(get_control(0, 10), -1.0f, 1.0f);
|
||||
|
||||
mix_airmode_disabled(roll, pitch, yaw, x_thrust, y_thrust, z_thrust, outputs);
|
||||
}
|
||||
|
||||
// Apply thrust model and scale outputs to range [idle_speed, 1].
|
||||
@ -438,87 +539,161 @@ void
|
||||
MultirotorMixer::update_saturation_status(unsigned index, bool clipping_high, bool clipping_low_roll_pitch,
|
||||
bool clipping_low_yaw)
|
||||
{
|
||||
float roll_scale = (_is_6dof) ? _rotors_6dof[index].roll_scale : _rotors[index].roll_scale;
|
||||
float pitch_scale = (_is_6dof) ? _rotors_6dof[index].pitch_scale : _rotors[index].pitch_scale;
|
||||
float yaw_scale = (_is_6dof) ? _rotors_6dof[index].yaw_scale : _rotors[index].yaw_scale;
|
||||
float x_scale = (_is_6dof) ? _rotors_6dof[index].x_scale : 0.f;
|
||||
float y_scale = (_is_6dof) ? _rotors_6dof[index].y_scale : 0.f;
|
||||
float z_scale = (_is_6dof) ? _rotors_6dof[index].z_scale : 0.f;
|
||||
|
||||
// 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) {
|
||||
if (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) {
|
||||
} else if (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) {
|
||||
if (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) {
|
||||
} else if (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) {
|
||||
if (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) {
|
||||
} else if (yaw_scale < 0.0f) {
|
||||
// A negative change in yaw will increase saturation
|
||||
_saturation_status.flags.yaw_neg = true;
|
||||
}
|
||||
|
||||
// A negative change in Z thrust will increase saturation
|
||||
_saturation_status.flags.z_thrust_neg = true;
|
||||
if (_is_6dof) {
|
||||
// check if the x input is saturating
|
||||
if (x_scale > 0.0f) {
|
||||
// A positive change in x will increase saturation
|
||||
_saturation_status.flags.x_thrust_pos = true;
|
||||
|
||||
} else if (x_scale < 0.0f) {
|
||||
// A negative change in x will increase saturation
|
||||
_saturation_status.flags.x_thrust_neg = true;
|
||||
}
|
||||
|
||||
// check if the y input is saturating
|
||||
if (y_scale > 0.0f) {
|
||||
// A positive change in y will increase saturation
|
||||
_saturation_status.flags.y_thrust_pos = true;
|
||||
|
||||
} else if (y_scale < 0.0f) {
|
||||
// A negative change in y will increase saturation
|
||||
_saturation_status.flags.y_thrust_neg = true;
|
||||
}
|
||||
|
||||
// check if the z input is saturating
|
||||
if (z_scale > 0.0f) {
|
||||
// A positive change in z will increase saturation
|
||||
_saturation_status.flags.z_thrust_pos = true;
|
||||
|
||||
} else if (z_scale < 0.0f) {
|
||||
// A negative change in z will increase saturation
|
||||
_saturation_status.flags.z_thrust_neg = true;
|
||||
}
|
||||
|
||||
} else {
|
||||
// A negative change in Z thrust (positive change in total thrust) will increase saturation
|
||||
_saturation_status.flags.z_thrust_neg = true;
|
||||
}
|
||||
}
|
||||
|
||||
// The motor is saturated at the lower limit
|
||||
// check which control axes and which directions are contributing
|
||||
if (clipping_low_roll_pitch) {
|
||||
// check if the roll input is saturating
|
||||
if (_rotors[index].roll_scale > 0.0f) {
|
||||
if (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) {
|
||||
} else if (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) {
|
||||
if (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) {
|
||||
} else if (pitch_scale < 0.0f) {
|
||||
// A positive change in pitch will increase saturation
|
||||
_saturation_status.flags.pitch_pos = true;
|
||||
}
|
||||
|
||||
// A positive change in Z thrust will increase saturation
|
||||
_saturation_status.flags.z_thrust_pos = true;
|
||||
if (_is_6dof) {
|
||||
// check if the x input is saturating
|
||||
if (x_scale > 0.0f) {
|
||||
// A negative change in x will increase saturation
|
||||
_saturation_status.flags.x_thrust_neg = true;
|
||||
|
||||
} else if (x_scale < 0.0f) {
|
||||
// A positive change in x will increase saturation
|
||||
_saturation_status.flags.x_thrust_pos = true;
|
||||
}
|
||||
|
||||
// check if the y input is saturating
|
||||
if (y_scale > 0.0f) {
|
||||
// A negative change in y will increase saturation
|
||||
_saturation_status.flags.y_thrust_neg = true;
|
||||
|
||||
} else if (y_scale < 0.0f) {
|
||||
// A positive change in y will increase saturation
|
||||
_saturation_status.flags.y_thrust_pos = true;
|
||||
}
|
||||
|
||||
// check if the z input is saturating
|
||||
if (z_scale > 0.0f) {
|
||||
// A negative change in z will increase saturation
|
||||
_saturation_status.flags.z_thrust_neg = true;
|
||||
|
||||
} else if (z_scale < 0.0f) {
|
||||
// A positive change in z will increase saturation
|
||||
_saturation_status.flags.z_thrust_pos = true;
|
||||
}
|
||||
|
||||
} else {
|
||||
// A positive change in Z thrust (negative change in total thrust) will increase saturation
|
||||
_saturation_status.flags.z_thrust_pos = true;
|
||||
}
|
||||
}
|
||||
|
||||
if (clipping_low_yaw) {
|
||||
// check if the yaw input is saturating
|
||||
if (_rotors[index].yaw_scale > 0.0f) {
|
||||
if (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) {
|
||||
} else if (yaw_scale < 0.0f) {
|
||||
// A positive change in yaw will increase saturation
|
||||
_saturation_status.flags.yaw_pos = true;
|
||||
}
|
||||
}
|
||||
|
||||
// X and Y thrusts are not controlled
|
||||
_saturation_status.flags.x_thrust_pos = true;
|
||||
_saturation_status.flags.x_thrust_neg = true;
|
||||
_saturation_status.flags.y_thrust_pos = true;
|
||||
_saturation_status.flags.y_thrust_neg = true;
|
||||
if (!_is_6dof) {
|
||||
// X and Y thrusts are not controlled
|
||||
_saturation_status.flags.x_thrust_pos = true;
|
||||
_saturation_status.flags.x_thrust_neg = true;
|
||||
_saturation_status.flags.y_thrust_pos = true;
|
||||
_saturation_status.flags.y_thrust_neg = true;
|
||||
}
|
||||
|
||||
_saturation_status.flags.valid = true;
|
||||
}
|
||||
|
||||
@ -46,22 +46,30 @@ enum class MultirotorGeometry : MultirotorGeometryUnderlyingType;
|
||||
/**
|
||||
* Multi-rotor mixer for pre-defined vehicle geometries.
|
||||
*
|
||||
* Collects four inputs (roll, pitch, yaw, thrust) and mixes them to
|
||||
* Collects either four inputs (roll, pitch, yaw, thrust) or six inputs
|
||||
* (roll, pitch, yaw, x thrust, y thrust and z thrust) and mixes them to
|
||||
* a set of outputs based on the configured geometry.
|
||||
*/
|
||||
class MultirotorMixer : public Mixer
|
||||
{
|
||||
public:
|
||||
/**
|
||||
|
||||
* Precalculated rotor mix.
|
||||
*/
|
||||
struct Rotor {
|
||||
float roll_scale; /**< scales roll for this 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 yaw_scale; /**< scales yaw for this rotor */
|
||||
float thrust_scale; /**< scales thrust for this rotor */
|
||||
};
|
||||
struct Rotor6Dof {
|
||||
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 x_scale; /**< scales x thrust for this rotor */
|
||||
float y_scale; /**< scales y thrust for this rotor */
|
||||
float z_scale; /**< scales z thrust for this rotor */
|
||||
};
|
||||
|
||||
/**
|
||||
* Constructor.
|
||||
@ -73,7 +81,7 @@ public:
|
||||
* 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
|
||||
* @param yaw_scale 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
|
||||
@ -82,6 +90,32 @@ public:
|
||||
MultirotorMixer(ControlCallback control_cb, uintptr_t cb_handle, MultirotorGeometry geometry,
|
||||
float roll_scale, float pitch_scale, float yaw_scale, float idle_speed);
|
||||
|
||||
/**
|
||||
* Constructor for 6-DoF.
|
||||
*
|
||||
* @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_scale Scaling factor applied to yaw inputs compared
|
||||
* to thrust.
|
||||
* @param x_scale Scaling factor applied to x thrust inputs
|
||||
* compared to thrust.
|
||||
* @param y_scale Scaling factor applied to y thrust inputs
|
||||
* compared to thrust.
|
||||
* @param z_scale Scaling factor applied to z thrust 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 x_scale, float y_scale, float z_scale, float idle_speed);
|
||||
|
||||
/**
|
||||
* Constructor (for testing).
|
||||
*
|
||||
@ -91,6 +125,17 @@ public:
|
||||
* @param rotor_count length of rotors array (= number of motors)
|
||||
*/
|
||||
MultirotorMixer(ControlCallback control_cb, uintptr_t cb_handle, const Rotor *rotors, unsigned rotor_count);
|
||||
|
||||
/**
|
||||
* Constructor (for testing 6-DoF).
|
||||
*
|
||||
* @param control_cb Callback invoked to read inputs.
|
||||
* @param cb_handle Passed to control_cb.
|
||||
* @param rotors control allocation matrix
|
||||
* @param rotor_count length of rotors array (= number of motors)
|
||||
*/
|
||||
MultirotorMixer(ControlCallback control_cb, uintptr_t cb_handle, const Rotor6Dof *rotors, unsigned rotor_count);
|
||||
|
||||
virtual ~MultirotorMixer();
|
||||
|
||||
// no copy, assignment, move, move assignment
|
||||
@ -186,14 +231,14 @@ private:
|
||||
* Minimize the saturation of the actuators by adding or substracting a fraction of desaturation_vector.
|
||||
* desaturation_vector is the vector that added to the output outputs, modifies the thrust or angular
|
||||
* acceleration on a specific axis.
|
||||
* For example, if desaturation_vector is given to slide along the vertical thrust axis (thrust_scale), the
|
||||
* For example, if desaturation_vector is given to slide along the vertical thrust axis (thrust_scale/z_scale), the
|
||||
* saturation will be minimized by shifting the vertical thrust setpoint, without changing the
|
||||
* roll/pitch/yaw accelerations.
|
||||
*
|
||||
* Note that as we only slide along the given axis, in extreme cases outputs can still contain values
|
||||
* outside of [min_output, max_output].
|
||||
*
|
||||
* @param desaturation_vector vector that is added to the outputs, e.g. thrust_scale
|
||||
* @param desaturation_vector vector that is added to the outputs, e.g. thrust_scale/z_scale
|
||||
* @param outputs output vector that is modified
|
||||
* @param sat_status saturation status output
|
||||
* @param min_output minimum desired value in outputs
|
||||
@ -232,232 +277,12 @@ private:
|
||||
inline void mix_airmode_disabled(float roll, float pitch, float yaw, float thrust, float *outputs);
|
||||
|
||||
/**
|
||||
* Mix yaw by updating an existing output vector (that already contains roll/pitch/thrust).
|
||||
*
|
||||
* Desaturation behavior: thrust is allowed to be decreased up to 15% in order to allow
|
||||
* some yaw control on the upper end. On the lower end thrust will never be increased,
|
||||
* but yaw is decreased as much as required.
|
||||
*
|
||||
* @param yaw demanded yaw
|
||||
* @param outputs output vector that is updated
|
||||
*/
|
||||
inline void mix_yaw(float yaw, float *outputs);
|
||||
|
||||
void update_saturation_status(unsigned index, bool clipping_high, bool clipping_low_roll_pitch, bool clipping_low_yaw);
|
||||
|
||||
float _roll_scale{1.0f};
|
||||
float _pitch_scale{1.0f};
|
||||
float _yaw_scale{1.0f};
|
||||
float _idle_speed{0.0f};
|
||||
float _delta_out_max{0.0f};
|
||||
float _thrust_factor{0.0f};
|
||||
|
||||
Airmode _airmode{Airmode::disabled};
|
||||
|
||||
saturation_status _saturation_status{};
|
||||
|
||||
unsigned _rotor_count;
|
||||
const Rotor *_rotors;
|
||||
|
||||
float *_outputs_prev{nullptr};
|
||||
float *_tmp_array{nullptr};
|
||||
};
|
||||
|
||||
/**
|
||||
* 6-DoF multi-rotor mixer for pre-defined vehicle geometries.
|
||||
*
|
||||
* Collects six inputs (roll, pitch, yaw, x thrust, y thrust and z thrust) and mixes them to
|
||||
* a set of outputs based on the configured geometry.
|
||||
*/
|
||||
class MultirotorMixer6dof : 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 x_scale; /**< scales x thrust for this rotor */
|
||||
float y_scale; /**< scales y thrust for this rotor */
|
||||
float z_scale; /**< scales z thrust 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_scale Scaling factor applied to yaw inputs compared
|
||||
* to thrust.
|
||||
* @param x_scale Scaling factor applied to x thrust inputs
|
||||
* compared to thrust.
|
||||
* @param y_scale Scaling factor applied to y thrust inputs
|
||||
* compared to thrust.
|
||||
* @param z_scale Scaling factor applied to z thrust 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.
|
||||
*/
|
||||
MultirotorMixer6dof(ControlCallback control_cb, uintptr_t cb_handle, MultirotorGeometry geometry,
|
||||
float roll_scale, float pitch_scale, float yaw_scale,
|
||||
float x_scale, float y_scale, float z_scale, float idle_speed);
|
||||
|
||||
/**
|
||||
* Constructor (for testing).
|
||||
*
|
||||
* @param control_cb Callback invoked to read inputs.
|
||||
* @param cb_handle Passed to control_cb.
|
||||
* @param rotors control allocation matrix
|
||||
* @param rotor_count length of rotors array (= number of motors)
|
||||
*/
|
||||
MultirotorMixer6dof(ControlCallback control_cb, uintptr_t cb_handle, const Rotor *rotors, unsigned rotor_count);
|
||||
|
||||
virtual ~MultirotorMixer6dof();
|
||||
|
||||
// no copy, assignment, move, move assignment
|
||||
MultirotorMixer6dof(const MultirotorMixer6dof &) = delete;
|
||||
MultirotorMixer6dof &operator=(const MultirotorMixer6dof &) = delete;
|
||||
MultirotorMixer6dof(MultirotorMixer6dof &&) = delete;
|
||||
MultirotorMixer6dof &operator=(MultirotorMixer6dof &&) = delete;
|
||||
|
||||
/**
|
||||
* 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 MultirotorMixer6dof instance, or nullptr
|
||||
* if the text format is bad.
|
||||
*/
|
||||
static MultirotorMixer6dof *from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, const char *buf,
|
||||
unsigned &buflen);
|
||||
|
||||
unsigned mix(float *outputs, unsigned space) override;
|
||||
|
||||
uint16_t get_saturation_status() override { return _saturation_status.value; }
|
||||
|
||||
void groups_required(uint32_t &groups) override { groups |= (1 << 0); }
|
||||
|
||||
/**
|
||||
* @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.
|
||||
*
|
||||
*/
|
||||
void set_max_delta_out_once(float delta_out_max) override { _delta_out_max = delta_out_max; }
|
||||
|
||||
unsigned set_trim(float trim) override { return _rotor_count; }
|
||||
unsigned get_trim(float *trim) override { return _rotor_count; }
|
||||
|
||||
/**
|
||||
* @brief Sets the thrust factor used to calculate mapping from desired thrust to motor control signal output.
|
||||
*
|
||||
* @param[in] val The value
|
||||
*/
|
||||
void set_thrust_factor(float val) override { _thrust_factor = math::constrain(val, 0.0f, 1.0f); }
|
||||
|
||||
void set_airmode(Airmode airmode) override { _airmode = airmode; }
|
||||
|
||||
unsigned get_multirotor_count() override { return _rotor_count; }
|
||||
|
||||
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 x_thrust_pos : 1; // 9 - true when a positive x thrust demand change will increase saturation
|
||||
uint16_t x_thrust_neg : 1; //10 - true when a negative x thrust demand change will increase saturation
|
||||
uint16_t y_thrust_pos : 1; //11 - true when a positive y thrust demand change will increase saturation
|
||||
uint16_t y_thrust_neg : 1; //12 - true when a negative y thrust demand change will increase saturation
|
||||
uint16_t z_thrust_pos : 1; //13 - true when a positive z thrust demand change will increase saturation
|
||||
uint16_t z_thrust_neg : 1; //14 - true when a negative z thrust demand change will increase saturation
|
||||
} flags;
|
||||
uint16_t value;
|
||||
};
|
||||
|
||||
private:
|
||||
/**
|
||||
* Computes the gain k by which desaturation_vector has to be multiplied
|
||||
* in order to unsaturate the output that has the greatest saturation.
|
||||
* @see also minimize_saturation().
|
||||
*
|
||||
* @return desaturation gain
|
||||
*/
|
||||
float compute_desaturation_gain(const float *desaturation_vector, const float *outputs, saturation_status &sat_status,
|
||||
float min_output, float max_output) const;
|
||||
|
||||
/**
|
||||
* Minimize the saturation of the actuators by adding or substracting a fraction of desaturation_vector.
|
||||
* desaturation_vector is the vector that added to the output outputs, modifies the thrust or angular
|
||||
* acceleration on a specific axis.
|
||||
* For example, if desaturation_vector is given to slide along the vertical thrust axis (z_scale), the
|
||||
* saturation will be minimized by shifting the vertical thrust setpoint, without changing the
|
||||
* roll/pitch/yaw accelerations.
|
||||
*
|
||||
* Note that as we only slide along the given axis, in extreme cases outputs can still contain values
|
||||
* outside of [min_output, max_output].
|
||||
*
|
||||
* @param desaturation_vector vector that is added to the outputs, e.g. z_scale
|
||||
* @param outputs output vector that is modified
|
||||
* @param sat_status saturation status output
|
||||
* @param min_output minimum desired value in outputs
|
||||
* @param max_output maximum desired value in outputs
|
||||
* @param reduce_only if true, only allow to reduce (substract) a fraction of desaturation_vector
|
||||
*/
|
||||
void minimize_saturation(const float *desaturation_vector, float *outputs, saturation_status &sat_status,
|
||||
float min_output = 0.f, float max_output = 1.f, bool reduce_only = false) const;
|
||||
|
||||
/**
|
||||
* Mix roll, pitch, yaw, thrust and set the outputs vector.
|
||||
*
|
||||
* Desaturation behavior: airmode for roll/pitch:
|
||||
* thrust is increased/decreased as much as required to meet the demanded roll/pitch.
|
||||
* Yaw is not allowed to increase the thrust, @see mix_yaw() for the exact behavior.
|
||||
*/
|
||||
inline void mix_airmode_rp(float roll, float pitch, float yaw, float x_thrust, float y_thrust, float z_thrust,
|
||||
float *outputs);
|
||||
|
||||
/**
|
||||
* Mix roll, pitch, yaw, thrust and set the outputs vector.
|
||||
*
|
||||
* Desaturation behavior: full airmode for roll/pitch/yaw:
|
||||
* thrust is increased/decreased as much as required to meet demanded the roll/pitch/yaw,
|
||||
* while giving priority to roll and pitch over yaw.
|
||||
*/
|
||||
inline void mix_airmode_rpy(float roll, float pitch, float yaw, float x_thrust, float y_thrust, float z_thrust,
|
||||
float *outputs);
|
||||
|
||||
/**
|
||||
* Mix roll, pitch, yaw, thrust and set the outputs vector.
|
||||
* Mix roll, pitch, yaw, x_thrust, y_thrust, z_thrust and set the outputs vector for 6-DoF vehicles.
|
||||
*
|
||||
* Desaturation behavior: no airmode, thrust is NEVER increased to meet the demanded
|
||||
* roll/pitch/yaw. Instead roll/pitch/yaw is reduced as much as needed.
|
||||
* Thrust can be reduced to unsaturate the upper side.
|
||||
* This is the only airmode available for 6-DoF vehicles.
|
||||
* @see mix_yaw() for the exact yaw behavior.
|
||||
*/
|
||||
inline void mix_airmode_disabled(float roll, float pitch, float yaw, float x_thrust, float y_thrust, float z_thrust,
|
||||
@ -477,6 +302,7 @@ private:
|
||||
|
||||
void update_saturation_status(unsigned index, bool clipping_high, bool clipping_low_roll_pitch, bool clipping_low_yaw);
|
||||
|
||||
bool _is_6dof{false};
|
||||
float _roll_scale{1.0f};
|
||||
float _pitch_scale{1.0f};
|
||||
float _yaw_scale{1.0f};
|
||||
@ -493,6 +319,7 @@ private:
|
||||
|
||||
unsigned _rotor_count;
|
||||
const Rotor *_rotors;
|
||||
const Rotor6Dof *_rotors_6dof;
|
||||
|
||||
float *_outputs_prev{nullptr};
|
||||
float *_tmp_array{nullptr};
|
||||
|
||||
@ -1,567 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2012-2019 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 "MultirotorMixer.hpp"
|
||||
|
||||
#include <float.h>
|
||||
#include <cstring>
|
||||
#include <cstdio>
|
||||
|
||||
#include <mathlib/mathlib.h>
|
||||
|
||||
// This file is generated by the px_generate_mixers.py script which is invoked during the build process
|
||||
// #include "mixer_multirotor.generated.h"
|
||||
#include "mixer_multirotor_6dof_normalized.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)
|
||||
|
||||
MultirotorMixer6dof::MultirotorMixer6dof(ControlCallback control_cb, uintptr_t cb_handle, MultirotorGeometry geometry,
|
||||
float roll_scale, float pitch_scale, float yaw_scale,
|
||||
float x_scale, float y_scale, float z_scale, float idle_speed) :
|
||||
MultirotorMixer6dof(control_cb, cb_handle, _config_index[(int)geometry], _config_rotor_count[(int)geometry])
|
||||
{
|
||||
_roll_scale = roll_scale;
|
||||
_pitch_scale = pitch_scale;
|
||||
_yaw_scale = yaw_scale;
|
||||
_x_scale = x_scale;
|
||||
_y_scale = y_scale;
|
||||
_z_scale = z_scale;
|
||||
_idle_speed = -1.0f + idle_speed * 2.0f; /* shift to output range here to avoid runtime calculation */
|
||||
}
|
||||
|
||||
MultirotorMixer6dof::MultirotorMixer6dof(ControlCallback control_cb, uintptr_t cb_handle, const Rotor *rotors,
|
||||
unsigned rotor_count) :
|
||||
Mixer(control_cb, cb_handle),
|
||||
_rotor_count(rotor_count),
|
||||
_rotors(rotors),
|
||||
_outputs_prev(new float[_rotor_count]),
|
||||
_tmp_array(new float[_rotor_count])
|
||||
{
|
||||
for (unsigned i = 0; i < _rotor_count; ++i) {
|
||||
_outputs_prev[i] = _idle_speed;
|
||||
}
|
||||
}
|
||||
|
||||
MultirotorMixer6dof::~MultirotorMixer6dof()
|
||||
{
|
||||
delete[] _outputs_prev;
|
||||
delete[] _tmp_array;
|
||||
}
|
||||
|
||||
MultirotorMixer6dof *
|
||||
MultirotorMixer6dof::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[7];
|
||||
int used;
|
||||
|
||||
/* enforce that the mixer ends with a new line */
|
||||
if (!string_well_formed(buf, buflen)) {
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
if (sscanf(buf, "S: %7s %d %d %d %d %d %d %d%n", geomname, &s[0], &s[1], &s[2], &s[3], &s[4], &s[5], &s[6],
|
||||
&used) != 8) {
|
||||
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 MultirotorMixer6dof(
|
||||
control_cb,
|
||||
cb_handle,
|
||||
geometry,
|
||||
s[0] / 10000.0f,
|
||||
s[1] / 10000.0f,
|
||||
s[2] / 10000.0f,
|
||||
s[3] / 10000.0f,
|
||||
s[4] / 10000.0f,
|
||||
s[5] / 10000.0f,
|
||||
s[6] / 10000.0f);
|
||||
}
|
||||
|
||||
float
|
||||
MultirotorMixer6dof::compute_desaturation_gain(const float *desaturation_vector, const float *outputs,
|
||||
saturation_status &sat_status, float min_output, float max_output) const
|
||||
{
|
||||
float k_min = 0.f;
|
||||
float k_max = 0.f;
|
||||
|
||||
for (unsigned i = 0; i < _rotor_count; i++) {
|
||||
// Avoid division by zero. If desaturation_vector[i] is zero, there's nothing we can do to unsaturate anyway
|
||||
if (fabsf(desaturation_vector[i]) < FLT_EPSILON) {
|
||||
continue;
|
||||
}
|
||||
|
||||
if (outputs[i] < min_output) {
|
||||
float k = (min_output - outputs[i]) / desaturation_vector[i];
|
||||
|
||||
if (k < k_min) { k_min = k; }
|
||||
|
||||
if (k > k_max) { k_max = k; }
|
||||
|
||||
sat_status.flags.motor_neg = true;
|
||||
}
|
||||
|
||||
if (outputs[i] > max_output) {
|
||||
float k = (max_output - outputs[i]) / desaturation_vector[i];
|
||||
|
||||
if (k < k_min) { k_min = k; }
|
||||
|
||||
if (k > k_max) { k_max = k; }
|
||||
|
||||
sat_status.flags.motor_pos = true;
|
||||
}
|
||||
}
|
||||
|
||||
// Reduce the saturation as much as possible
|
||||
return k_min + k_max;
|
||||
}
|
||||
|
||||
void
|
||||
MultirotorMixer6dof::minimize_saturation(const float *desaturation_vector, float *outputs,
|
||||
saturation_status &sat_status, float min_output, float max_output, bool reduce_only) const
|
||||
{
|
||||
float k1 = compute_desaturation_gain(desaturation_vector, outputs, sat_status, min_output, max_output);
|
||||
|
||||
if (reduce_only && k1 > 0.f) {
|
||||
return;
|
||||
}
|
||||
|
||||
for (unsigned i = 0; i < _rotor_count; i++) {
|
||||
outputs[i] += k1 * desaturation_vector[i];
|
||||
}
|
||||
|
||||
// Compute the desaturation gain again based on the updated outputs.
|
||||
// In most cases it will be zero. It won't be if max(outputs) - min(outputs) > max_output - min_output.
|
||||
// In that case adding 0.5 of the gain will equilibrate saturations.
|
||||
float k2 = 0.5f * compute_desaturation_gain(desaturation_vector, outputs, sat_status, min_output, max_output);
|
||||
|
||||
for (unsigned i = 0; i < _rotor_count; i++) {
|
||||
outputs[i] += k2 * desaturation_vector[i];
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
MultirotorMixer6dof::mix_airmode_rp(float roll, float pitch, float yaw, float x_thrust, float y_thrust, float z_thrust,
|
||||
float *outputs)
|
||||
{
|
||||
// Airmode for roll and pitch, but not yaw
|
||||
|
||||
// Mix without yaw
|
||||
for (unsigned i = 0; i < _rotor_count; i++) {
|
||||
outputs[i] = roll * _rotors[i].roll_scale +
|
||||
pitch * _rotors[i].pitch_scale +
|
||||
x_thrust * _rotors[i].x_scale +
|
||||
y_thrust * _rotors[i].y_scale +
|
||||
z_thrust * _rotors[i].z_scale;
|
||||
|
||||
// Thrust will be used to unsaturate if needed
|
||||
_tmp_array[i] = math::abs_t<float>(_rotors[i].z_scale);
|
||||
}
|
||||
|
||||
minimize_saturation(_tmp_array, outputs, _saturation_status);
|
||||
|
||||
// Mix yaw independently
|
||||
mix_yaw(yaw, outputs);
|
||||
}
|
||||
|
||||
void
|
||||
MultirotorMixer6dof::mix_airmode_rpy(float roll, float pitch, float yaw, float x_thrust, float y_thrust, float z_thrust,
|
||||
float *outputs)
|
||||
{
|
||||
// Airmode for roll, pitch and yaw
|
||||
|
||||
// Do full mixing
|
||||
for (unsigned i = 0; i < _rotor_count; i++) {
|
||||
outputs[i] = roll * _rotors[i].roll_scale +
|
||||
pitch * _rotors[i].pitch_scale +
|
||||
yaw * _rotors[i].yaw_scale +
|
||||
x_thrust * _rotors[i].x_scale +
|
||||
y_thrust * _rotors[i].y_scale +
|
||||
z_thrust * _rotors[i].z_scale;
|
||||
|
||||
// Z thrust will be used to unsaturate if needed
|
||||
_tmp_array[i] = math::abs_t<float>(_rotors[i].z_scale);
|
||||
}
|
||||
|
||||
minimize_saturation(_tmp_array, outputs, _saturation_status);
|
||||
|
||||
// Unsaturate yaw (in case upper and lower bounds are exceeded)
|
||||
// to prioritize roll/pitch over yaw.
|
||||
for (unsigned i = 0; i < _rotor_count; i++) {
|
||||
_tmp_array[i] = _rotors[i].yaw_scale;
|
||||
}
|
||||
|
||||
minimize_saturation(_tmp_array, outputs, _saturation_status);
|
||||
}
|
||||
|
||||
void
|
||||
MultirotorMixer6dof::mix_airmode_disabled(float roll, float pitch, float yaw, float x_thrust, float y_thrust,
|
||||
float z_thrust, float *outputs)
|
||||
{
|
||||
// Airmode disabled: never allow to increase the thrust to unsaturate a motor
|
||||
|
||||
// Mix without yaw
|
||||
for (unsigned i = 0; i < _rotor_count; i++) {
|
||||
outputs[i] = roll * _rotors[i].roll_scale +
|
||||
pitch * _rotors[i].pitch_scale +
|
||||
x_thrust * _rotors[i].x_scale +
|
||||
y_thrust * _rotors[i].y_scale +
|
||||
z_thrust * _rotors[i].z_scale;
|
||||
|
||||
// Z thrust will be used to unsaturate if needed
|
||||
_tmp_array[i] = math::abs_t<float>(_rotors[i].z_scale);
|
||||
}
|
||||
|
||||
// only reduce thrust
|
||||
minimize_saturation(_tmp_array, outputs, _saturation_status, 0.f, 1.f, true);
|
||||
|
||||
// Reduce roll/pitch acceleration if needed to unsaturate
|
||||
for (unsigned i = 0; i < _rotor_count; i++) {
|
||||
_tmp_array[i] = _rotors[i].roll_scale;
|
||||
}
|
||||
|
||||
minimize_saturation(_tmp_array, outputs, _saturation_status);
|
||||
|
||||
for (unsigned i = 0; i < _rotor_count; i++) {
|
||||
_tmp_array[i] = _rotors[i].pitch_scale;
|
||||
}
|
||||
|
||||
minimize_saturation(_tmp_array, outputs, _saturation_status);
|
||||
|
||||
// Mix yaw independently
|
||||
mix_yaw(yaw, outputs);
|
||||
}
|
||||
|
||||
void MultirotorMixer6dof::mix_yaw(float yaw, float *outputs)
|
||||
{
|
||||
// Add yaw to outputs
|
||||
for (unsigned i = 0; i < _rotor_count; i++) {
|
||||
outputs[i] += yaw * _rotors[i].yaw_scale;
|
||||
|
||||
// Yaw will be used to unsaturate if needed
|
||||
_tmp_array[i] = _rotors[i].yaw_scale;
|
||||
}
|
||||
|
||||
// Change yaw acceleration to unsaturate the outputs if needed (do not change roll/pitch),
|
||||
// and allow some yaw response at maximum thrust
|
||||
minimize_saturation(_tmp_array, outputs, _saturation_status, 0.f, 1.15f);
|
||||
|
||||
for (unsigned i = 0; i < _rotor_count; i++) {
|
||||
_tmp_array[i] = math::abs_t<float>(_rotors[i].z_scale);
|
||||
}
|
||||
|
||||
// reduce thrust only
|
||||
minimize_saturation(_tmp_array, outputs, _saturation_status, 0.f, 1.f, true);
|
||||
}
|
||||
|
||||
unsigned
|
||||
MultirotorMixer6dof::mix(float *outputs, unsigned space)
|
||||
{
|
||||
if (space < _rotor_count) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
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 x_thrust = math::constrain(get_control(0, 8), -1.0f, 1.0f);
|
||||
float y_thrust = math::constrain(get_control(0, 9), -1.0f, 1.0f);
|
||||
float z_thrust = math::constrain(get_control(0, 10), -1.0f, 1.0f);
|
||||
|
||||
// clean out class variable used to capture saturation
|
||||
_saturation_status.value = 0;
|
||||
|
||||
// Do the mixing using the strategy given by the current Airmode configuration
|
||||
switch (_airmode) {
|
||||
case Airmode::roll_pitch:
|
||||
mix_airmode_rp(roll, pitch, yaw, x_thrust, y_thrust, z_thrust, outputs);
|
||||
break;
|
||||
|
||||
case Airmode::roll_pitch_yaw:
|
||||
mix_airmode_rpy(roll, pitch, yaw, x_thrust, y_thrust, z_thrust, outputs);
|
||||
break;
|
||||
|
||||
case Airmode::disabled:
|
||||
default: // just in case: default to disabled
|
||||
mix_airmode_disabled(roll, pitch, yaw, x_thrust, y_thrust, z_thrust, outputs);
|
||||
break;
|
||||
}
|
||||
|
||||
// Apply thrust model and scale outputs to range [idle_speed, 1].
|
||||
// At this point the outputs are expected to be in [0, 1], but they can be outside, for example
|
||||
// if a roll command exceeds the motor band limit.
|
||||
for (unsigned i = 0; i < _rotor_count; i++) {
|
||||
// Implement simple model for static relationship between applied motor pwm and motor thrust
|
||||
// model: thrust = (1 - _thrust_factor) * PWM + _thrust_factor * PWM^2
|
||||
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_roll_pitch = false;
|
||||
bool clipping_low_yaw = false;
|
||||
|
||||
// Check for saturation against static limits.
|
||||
// We only check for low clipping if airmode is disabled (or yaw
|
||||
// clipping if airmode==roll/pitch), since in all other cases thrust will
|
||||
// be reduced or boosted and we can keep the integrators enabled, which
|
||||
// leads to better tracking performance.
|
||||
if (outputs[i] < _idle_speed + 0.01f) {
|
||||
if (_airmode == Airmode::disabled) {
|
||||
clipping_low_roll_pitch = true;
|
||||
clipping_low_yaw = true;
|
||||
|
||||
} else if (_airmode == Airmode::roll_pitch) {
|
||||
clipping_low_yaw = 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_roll_pitch = true;
|
||||
clipping_low_yaw = true;
|
||||
}
|
||||
}
|
||||
|
||||
_outputs_prev[i] = outputs[i];
|
||||
|
||||
// update the saturation status report
|
||||
update_saturation_status(i, clipping_high, clipping_low_roll_pitch, clipping_low_yaw);
|
||||
}
|
||||
|
||||
// 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_roll_pitch: true if the motor demand is being limited in the negative direction (roll/pitch)
|
||||
* clipping_low_yaw: true if the motor demand is being limited in the negative direction (yaw)
|
||||
*/
|
||||
void
|
||||
MultirotorMixer6dof::update_saturation_status(unsigned index, bool clipping_high, bool clipping_low_roll_pitch,
|
||||
bool clipping_low_yaw)
|
||||
{
|
||||
// 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;
|
||||
}
|
||||
|
||||
// check if the x input is saturating
|
||||
if (_rotors[index].x_scale > 0.0f) {
|
||||
// A positive change in x will increase saturation
|
||||
_saturation_status.flags.x_thrust_pos = true;
|
||||
|
||||
} else if (_rotors[index].x_scale < 0.0f) {
|
||||
// A negative change in x will increase saturation
|
||||
_saturation_status.flags.x_thrust_neg = true;
|
||||
}
|
||||
|
||||
// check if the y input is saturating
|
||||
if (_rotors[index].y_scale > 0.0f) {
|
||||
// A positive change in y will increase saturation
|
||||
_saturation_status.flags.y_thrust_pos = true;
|
||||
|
||||
} else if (_rotors[index].y_scale < 0.0f) {
|
||||
// A negative change in y will increase saturation
|
||||
_saturation_status.flags.y_thrust_neg = true;
|
||||
}
|
||||
|
||||
// check if the z input is saturating
|
||||
if (_rotors[index].z_scale > 0.0f) {
|
||||
// A positive change in z will increase saturation
|
||||
_saturation_status.flags.z_thrust_pos = true;
|
||||
|
||||
} else if (_rotors[index].z_scale < 0.0f) {
|
||||
// A negative change in z will increase saturation
|
||||
_saturation_status.flags.z_thrust_neg = true;
|
||||
}
|
||||
}
|
||||
|
||||
// The motor is saturated at the lower limit
|
||||
// check which control axes and which directions are contributing
|
||||
if (clipping_low_roll_pitch) {
|
||||
// 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 x input is saturating
|
||||
if (_rotors[index].x_scale > 0.0f) {
|
||||
// A negative change in x will increase saturation
|
||||
_saturation_status.flags.x_thrust_neg = true;
|
||||
|
||||
} else if (_rotors[index].x_scale < 0.0f) {
|
||||
// A positive change in x will increase saturation
|
||||
_saturation_status.flags.x_thrust_pos = true;
|
||||
}
|
||||
|
||||
// check if the y input is saturating
|
||||
if (_rotors[index].y_scale > 0.0f) {
|
||||
// A negative change in y will increase saturation
|
||||
_saturation_status.flags.y_thrust_neg = true;
|
||||
|
||||
} else if (_rotors[index].y_scale < 0.0f) {
|
||||
// A positive change in y will increase saturation
|
||||
_saturation_status.flags.y_thrust_pos = true;
|
||||
}
|
||||
|
||||
// check if the z input is saturating
|
||||
if (_rotors[index].z_scale > 0.0f) {
|
||||
// A negative change in z will increase saturation
|
||||
_saturation_status.flags.z_thrust_neg = true;
|
||||
|
||||
} else if (_rotors[index].z_scale < 0.0f) {
|
||||
// A positive change in z will increase saturation
|
||||
_saturation_status.flags.z_thrust_pos = true;
|
||||
}
|
||||
}
|
||||
|
||||
if (clipping_low_yaw) {
|
||||
// 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;
|
||||
}
|
||||
}
|
||||
|
||||
_saturation_status.flags.valid = true;
|
||||
}
|
||||
@ -217,7 +217,7 @@ def normalize_mix_px4(B):
|
||||
|
||||
return B_px
|
||||
|
||||
def generate_mixer_multirotor_header(geometries_list, use_normalized_mix=False, use_6dof=False):
|
||||
def generate_mixer_multirotor_header(geometries_list, use_normalized_mix=False):
|
||||
'''
|
||||
Generate C header file with same format as multi_tables.py
|
||||
TODO: rewrite using templates (see generation of uORB headers)
|
||||
@ -225,12 +225,6 @@ def generate_mixer_multirotor_header(geometries_list, use_normalized_mix=False,
|
||||
from io import StringIO
|
||||
buf = StringIO()
|
||||
|
||||
# Adapt fields based on options
|
||||
if use_6dof:
|
||||
mixer_class_name = "MultirotorMixer6dof"
|
||||
else:
|
||||
mixer_class_name = "MultirotorMixer"
|
||||
|
||||
# Print Header
|
||||
buf.write(u"/*\n")
|
||||
buf.write(u"* This file is automatically generated by px_generate_mixers.py - do not edit.\n")
|
||||
@ -258,28 +252,38 @@ def generate_mixer_multirotor_header(geometries_list, use_normalized_mix=False,
|
||||
else:
|
||||
mix = geometry['mix']['B']
|
||||
|
||||
buf.write(u"static constexpr {}::Rotor _config_{}[] {{\n".format(mixer_class_name, geometry['info']['name']))
|
||||
buf.write(u"static constexpr MultirotorMixer::Rotor _config_{}[] {{\n".format(geometry['info']['name']))
|
||||
|
||||
for row in mix:
|
||||
if use_6dof:
|
||||
# 6dof mixer
|
||||
buf.write(u"\t{{ {:9f}, {:9f}, {:9f}, {:9f}, {:9f}, {:9f} }},\n".format(
|
||||
row[0], row[1], row[2],
|
||||
row[3], row[4], row[5]))
|
||||
else:
|
||||
# 4dof mixer
|
||||
buf.write(u"\t{{ {:9f}, {:9f}, {:9f}, {:9f} }},\n".format(
|
||||
row[0], row[1], row[2],
|
||||
-row[5])) # Upward thrust is positive TODO: to remove this, adapt PX4 to use NED correctly
|
||||
buf.write(u"\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")
|
||||
|
||||
buf.write(u"static constexpr MultirotorMixer::Rotor6Dof _config_6dof_{}[] {{\n".format(geometry['info']['name']))
|
||||
|
||||
for row in mix:
|
||||
# 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]))
|
||||
|
||||
buf.write(u"};\n\n")
|
||||
|
||||
# Print geometry indices
|
||||
buf.write(u"static constexpr const {}::Rotor *_config_index[] {{\n".format(mixer_class_name))
|
||||
buf.write(u"static constexpr const MultirotorMixer::Rotor *_config_index[] {\n")
|
||||
for geometry in geometries_list:
|
||||
buf.write(u"\t&_config_{}[0],\n".format(geometry['info']['name']))
|
||||
buf.write(u"};\n\n")
|
||||
|
||||
# Print geometry indices for 6Dof
|
||||
buf.write(u"static constexpr const MultirotorMixer::Rotor6Dof *_config_index_6dof[] {\n")
|
||||
for geometry in geometries_list:
|
||||
buf.write(u"\t&_config_6dof_{}[0],\n".format(geometry['info']['name']))
|
||||
buf.write(u"};\n\n")
|
||||
|
||||
# Print geometry rotor counts
|
||||
buf.write(u"static constexpr unsigned _config_rotor_count[] {\n")
|
||||
for geometry in geometries_list:
|
||||
@ -318,8 +322,6 @@ if __name__ == '__main__':
|
||||
action='store_true')
|
||||
parser.add_argument('--normalize', help='Use normalized mixers (compatibility mode)',
|
||||
action='store_true')
|
||||
parser.add_argument('--sixdof', help='Use 6dof mixers',
|
||||
action='store_true')
|
||||
args = parser.parse_args()
|
||||
|
||||
# Find toml files
|
||||
@ -386,8 +388,7 @@ if __name__ == '__main__':
|
||||
|
||||
# Generate header file
|
||||
header = generate_mixer_multirotor_header(geometries_list,
|
||||
use_normalized_mix=args.normalize,
|
||||
use_6dof=args.sixdof)
|
||||
use_normalized_mix=args.normalize)
|
||||
|
||||
if args.outputfile is not None:
|
||||
# Write header file
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user