diff options
author | px4dev <px4@purgatory.org> | 2012-08-15 00:46:15 -0700 |
---|---|---|
committer | px4dev <px4@purgatory.org> | 2012-08-15 00:46:15 -0700 |
commit | 5198a9daf798089b4f6f2112029260f614cf7702 (patch) | |
tree | 97040f964cc58ac4f86c65b02f8eea973c3448d4 /apps/systemlib/mixer/mixer_multirotor.cpp | |
parent | 3edd6c86f20a768eee049fa2e9410c32bb13c382 (diff) | |
download | px4-firmware-5198a9daf798089b4f6f2112029260f614cf7702.tar.gz px4-firmware-5198a9daf798089b4f6f2112029260f614cf7702.tar.bz2 px4-firmware-5198a9daf798089b4f6f2112029260f614cf7702.zip |
New multirotor mixer; builds, not yet tested.
Diffstat (limited to 'apps/systemlib/mixer/mixer_multirotor.cpp')
-rw-r--r-- | apps/systemlib/mixer/mixer_multirotor.cpp | 123 |
1 files changed, 120 insertions, 3 deletions
diff --git a/apps/systemlib/mixer/mixer_multirotor.cpp b/apps/systemlib/mixer/mixer_multirotor.cpp index 713b1e25d..990c067fd 100644 --- a/apps/systemlib/mixer/mixer_multirotor.cpp +++ b/apps/systemlib/mixer/mixer_multirotor.cpp @@ -50,14 +50,100 @@ #include <stdio.h> #include <math.h> #include <unistd.h> +#include <math.h> #include "mixer.h" +#define CW (-1.0f) +#define CCW (1.0f) + +namespace +{ + +/* + * These tables automatically generated by multi_tables - do not edit. + */ +const MultirotorMixer::Rotor _config_quad_x[] = { + { -0.707107, 0.707107, 1.00 }, + { 0.707107, -0.707107, 1.00 }, + { 0.707107, 0.707107, -1.00 }, + { -0.707107, -0.707107, -1.00 }, +}; +const MultirotorMixer::Rotor _config_quad_plus[] = { + { -1.000000, 0.000000, 1.00 }, + { 1.000000, 0.000000, 1.00 }, + { 0.000000, 1.000000, -1.00 }, + { -0.000000, -1.000000, -1.00 }, +}; +const MultirotorMixer::Rotor _config_hex_x[] = { + { -1.000000, 0.000000, -1.00 }, + { 1.000000, 0.000000, 1.00 }, + { 0.500000, 0.866025, -1.00 }, + { -0.500000, -0.866025, 1.00 }, + { -0.500000, 0.866025, 1.00 }, + { 0.500000, -0.866025, -1.00 }, +}; +const MultirotorMixer::Rotor _config_hex_plus[] = { + { 0.000000, 1.000000, -1.00 }, + { -0.000000, -1.000000, 1.00 }, + { 0.866025, -0.500000, -1.00 }, + { -0.866025, 0.500000, 1.00 }, + { 0.866025, 0.500000, 1.00 }, + { -0.866025, -0.500000, -1.00 }, +}; +const MultirotorMixer::Rotor _config_octa_x[] = { + { -0.382683, 0.923880, -1.00 }, + { 0.382683, -0.923880, -1.00 }, + { -0.923880, 0.382683, 1.00 }, + { -0.382683, -0.923880, 1.00 }, + { 0.382683, 0.923880, 1.00 }, + { 0.923880, -0.382683, 1.00 }, + { 0.923880, 0.382683, -1.00 }, + { -0.923880, -0.382683, -1.00 }, +}; +const MultirotorMixer::Rotor _config_octa_plus[] = { + { 0.000000, 1.000000, -1.00 }, + { -0.000000, -1.000000, -1.00 }, + { -0.707107, 0.707107, 1.00 }, + { -0.707107, -0.707107, 1.00 }, + { 0.707107, 0.707107, 1.00 }, + { 0.707107, -0.707107, 1.00 }, + { 1.000000, 0.000000, -1.00 }, + { -1.000000, 0.000000, -1.00 }, +}; +const MultirotorMixer::Rotor *_config_index[MultirotorMixer::Geometry::MAX_GEOMETRY] = { + &_config_quad_x[0], + &_config_quad_plus[0], + &_config_hex_x[0], + &_config_hex_plus[0], + &_config_octa_x[0], + &_config_octa_plus[0], +}; +const unsigned _config_rotor_count[MultirotorMixer::Geometry::MAX_GEOMETRY] = { + 4, /* quad_x */ + 4, /* quad_plus */ + 6, /* hex_x */ + 6, /* hex_plus */ + 8, /* octa_x */ + 8, /* octa_plus */ +}; + +} + MultirotorMixer::MultirotorMixer(ControlCallback control_cb, uintptr_t cb_handle, - MultirotorMixer::Geometry geom) : + Geometry geometry, + float roll_scale, + float pitch_scale, + float yaw_scale, + float deadband) : Mixer(control_cb, cb_handle), - _geometry(geom) + _roll_scale(roll_scale), + _pitch_scale(pitch_scale), + _yaw_scale(yaw_scale), + _deadband(-1.0 + deadband), /* shift to output range here to avoid runtime calculation */ + _rotor_count(_config_rotor_count[geometry]), + _rotors(_config_index[geometry]) { } @@ -68,7 +154,38 @@ MultirotorMixer::~MultirotorMixer() unsigned MultirotorMixer::mix(float *outputs, unsigned space) { - /* XXX implement this */ + float roll = get_control(0, 0) * _roll_scale; + float pitch = get_control(0, 1) * _pitch_scale; + float yaw = get_control(0, 2) * _yaw_scale; + float thrust = get_control(0, 3); + float max = 0.0f; + float fixup_scale; + + /* perform initial mix pass yielding un-bounded outputs */ + for (unsigned i = 0; i < _rotor_count; i++) { + float tmp = roll * _rotors[i].roll_scale + + pitch * _rotors[i].pitch_scale + + yaw * _rotors[i].yaw_scale + + thrust; + if (tmp > max) + max = tmp; + outputs[i] = tmp; + } + + /* scale values into the -1.0 - 1.0 range */ + if (max > 1.0f) { + fixup_scale = 2.0f / max; + } else { + fixup_scale = 2.0f; + } + for (unsigned i = 0; i < _rotor_count; i++) + outputs[i] *= -1.0 + (outputs[i] * fixup_scale); + + /* ensure outputs are out of the deadband */ + for (unsigned i = 0; i < _rotor_count; i++) + if (outputs[i] < _deadband) + outputs[i] = _deadband; + return 0; } |