Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-26 01:17:51 +08:00
Code Issues Actions 5 Packages Projects Releases Wiki Activity

Omni Mixer: The mixer for 6-DoF vehicles is refactored to the MultirotorMixer now

Browse Source
This commit is contained in:
Azarakhsh Keipour
2020-01-06 17:55:11 -05:00
parent d5ff0d8ea6
commit 69fe37e6be
6 changed files with 306 additions and 886 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/mixer/MixerGroup.cpp
+1 -1
View File
@@ -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':
src/lib/mixer/MultirotorMixer/CMakeLists.txt
+1 -17
View File
@@ -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)
src/lib/mixer/MultirotorMixer/MultirotorMixer.cpp
+225 -50
View File
@@ -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;
}
src/lib/mixer/MultirotorMixer/MultirotorMixer.hpp
+56 -229
View File
@@ -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};
src/lib/mixer/MultirotorMixer/MultirotorMixer6dof.cpp
-567
View File
File diff suppressed because it is too large Load Diff
src/lib/mixer/MultirotorMixer/geometries/tools/px_generate_mixers.py
+23 -22
View File
@@ -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