From 5198a9daf798089b4f6f2112029260f614cf7702 Mon Sep 17 00:00:00 2001 From: px4dev Date: Wed, 15 Aug 2012 00:46:15 -0700 Subject: New multirotor mixer; builds, not yet tested. --- apps/systemlib/mixer/mixer.cpp | 11 +++ apps/systemlib/mixer/mixer.h | 67 ++++++++++++++-- apps/systemlib/mixer/mixer_group.cpp | 46 +++++++++++ apps/systemlib/mixer/mixer_multirotor.cpp | 123 +++++++++++++++++++++++++++++- apps/systemlib/mixer/multi_tables | 93 ++++++++++++++++++++++ 5 files changed, 331 insertions(+), 9 deletions(-) create mode 100755 apps/systemlib/mixer/multi_tables (limited to 'apps') diff --git a/apps/systemlib/mixer/mixer.cpp b/apps/systemlib/mixer/mixer.cpp index 49c8ed75a..b56226c03 100644 --- a/apps/systemlib/mixer/mixer.cpp +++ b/apps/systemlib/mixer/mixer.cpp @@ -59,6 +59,17 @@ Mixer::Mixer(ControlCallback control_cb, uintptr_t cb_handle) : { } +float +Mixer::get_control(uint8_t group, uint8_t index) +{ + float value; + + _control_cb(_cb_handle, group, index, value); + + return value; +} + + float Mixer::scale(const mixer_scaler_s &scaler, float input) { diff --git a/apps/systemlib/mixer/mixer.h b/apps/systemlib/mixer/mixer.h index 882acb33e..94c179eba 100644 --- a/apps/systemlib/mixer/mixer.h +++ b/apps/systemlib/mixer/mixer.h @@ -184,6 +184,15 @@ protected: ControlCallback _control_cb; uintptr_t _cb_handle; + /** + * Invoke the client callback to fetch a control value. + * + * @param group Control group to fetch from. + * @param index Control index to fetch. + * @return The control value. + */ + float get_control(uint8_t group, uint8_t index); + /** * Perform simpler linear scaling. * @@ -328,7 +337,7 @@ private: }; /** - * Multi-rotor mixer. + * Multi-rotor mixer for pre-defined vehicle geometries. * * Collects four inputs (roll, pitch, yaw, thrust) and mixes them to * a set of outputs based on the configured geometry. @@ -336,22 +345,68 @@ private: class __EXPORT MultirotorMixer : public Mixer { public: + /** + * Supported multirotor geometries. + * + * XXX add more + */ enum Geometry { - MULTIROTOR_QUAD_PLUS, - MULTIROTOR_QUAD_X - /* XXX add more here */ + QUAD_X = 0, /**< quad in X configuration */ + QUAD_PLUS, /**< quad in + configuration */ + HEX_X, /**< hex in X configuration */ + HEX_PLUS, /**< hex in + configuration */ + OCTA_X, + OCTA_PLUS, + + MAX_GEOMETRY + }; + + /** + * 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 */ }; + /** + * Constructor. + * + * @param control_cb Callback invoked to read inputs. + * @param cb_handle Passed to control_cb. + * @param geometry The selected geometry. + * @param roll_scale Scaling factor applied to roll inputs + * compared to thrust. + * @param pitch_scale Scaling factor applied to pitch inputs + * compared to thrust. + * @param yaw_wcale Scaling factor applied to yaw inputs compared + * to thrust. + * @param deadband Minumum 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, - Geometry geom); + Geometry geometry, + float roll_scale, + float pitch_scale, + float yaw_scale, + float deadband); ~MultirotorMixer(); virtual unsigned mix(float *outputs, unsigned space); virtual void groups_required(uint32_t &groups); private: - Geometry _geometry; + float _roll_scale; + float _pitch_scale; + float _yaw_scale; + float _deadband; + + unsigned _rotor_count; + const Rotor *_rotors; + }; #endif diff --git a/apps/systemlib/mixer/mixer_group.cpp b/apps/systemlib/mixer/mixer_group.cpp index 3f954c129..bc944ccd9 100644 --- a/apps/systemlib/mixer/mixer_group.cpp +++ b/apps/systemlib/mixer/mixer_group.cpp @@ -210,6 +210,45 @@ fail: return nullptr; } +MultirotorMixer * +mixer_load_multirotor(Mixer::ControlCallback control_cb, uintptr_t cb_handle, const char *buf) +{ + MultirotorMixer::Geometry geometry; + char geomname[8]; + int s[4]; + + if (sscanf(buf, "R: %7s %d %d %d %d", geomname, &s[0], &s[1], &s[2], &s[3]) != 5) { + debug("multirotor parse failed on '%s'", buf); + return nullptr; + } + + if (!strcmp(geomname, "4+")) { + geometry = MultirotorMixer::QUAD_PLUS; + } else if (!strcmp(geomname, "4x")) { + geometry = MultirotorMixer::QUAD_X; + } else if (!strcmp(geomname, "6+")) { + geometry = MultirotorMixer::HEX_PLUS; + } else if (!strcmp(geomname, "6x")) { + geometry = MultirotorMixer::HEX_X; + } else if (!strcmp(geomname, "8+")) { + geometry = MultirotorMixer::OCTA_PLUS; + } else if (!strcmp(geomname, "8x")) { + geometry = MultirotorMixer::OCTA_X; + } else { + debug("unrecognised geometry '%s'", geomname); + return nullptr; + } + + return new MultirotorMixer( + control_cb, + cb_handle, + geometry, + s[0] / 10000.0f, + s[1] / 10000.0f, + s[2] / 10000.0f, + s[3] / 10000.0f); +} + int mixer_load(Mixer::ControlCallback control_cb, uintptr_t cb_handle, int fd, Mixer *&mixer) { @@ -239,6 +278,13 @@ mixer_load(Mixer::ControlCallback control_cb, uintptr_t cb_handle, int fd, Mixer return (mixer == nullptr) ? -1 : 1; } + /* is it a multirotor mixer? */ + if (buf[0] == 'R') { + debug("got a multirotor mixer"); + mixer = mixer_load_multirotor(control_cb, cb_handle, buf); + return (mixer == nullptr) ? -1 : 1; + } + /* we don't recognise the mixer type */ debug("unrecognized mixer type '%c'", buf[0]); return -1; 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 #include #include +#include #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; } diff --git a/apps/systemlib/mixer/multi_tables b/apps/systemlib/mixer/multi_tables new file mode 100755 index 000000000..3da1a11ba --- /dev/null +++ b/apps/systemlib/mixer/multi_tables @@ -0,0 +1,93 @@ +#!/usr/bin/tclsh +# +# Generate multirotor mixer scale tables compatible with the ArduCopter layout +# + +proc rad {a} { expr ($a / 360.0) * 2 * acos(-1) } +proc rcos {a} { expr cos([rad $a])} + +set quad_x { + 45 CCW + -135 CCW + -45 CW + 135 CW +} + +set quad_plus { + 90 CCW + -90 CCW + 0 CW + 180 CW +} + +set hex_x { + 90 CW + -90 CCW + -30 CW + 150 CCW + 30 CCW + -150 CW +} + +set hex_plus { + 0 CW + 180 CCW + -120 CW + 60 CCW + -60 CCW + 120 CW +} + +set octa_x { + 22.5 CW + -157.5 CW + 67.5 CCW + 157.5 CCW + -22.5 CCW + -112.5 CCW + -67.5 CW + 112.5 CW +} + +set octa_plus { + 0 CW + 180 CW + 45 CCW + 135 CCW + -45 CCW + -135 CCW + -90 CW + 90 CW +} + +set tables {quad_x quad_plus hex_x hex_plus octa_x octa_plus} + +proc factors {a d} { puts [format "\t{ %9.6f, %9.6f, %5.2f }," [rcos [expr $a + 90]] [rcos $a] $d]} + +foreach table $tables { + puts [format "const MultirotorMixer::Rotor _config_%s\[\] = {" $table] + + upvar #0 $table angles + foreach {angle dir} $angles { + if {$dir == "CW"} { + set dd -1.0 + } else { + set dd 1.0 + } + factors $angle $dd + } + puts "};" +} + +puts "const MultirotorMixer::Rotor *_config_index\[MultirotorMixer::Geometry::MAX_GEOMETRY\] = {" +foreach table $tables { + puts [format "\t&_config_%s\[0\]," $table] +} +puts "};" + +puts "const unsigned _config_rotor_count\[MultirotorMixer::Geometry::MAX_GEOMETRY\] = {" +foreach table $tables { + upvar #0 $table angles + puts [format "\t%u, /* %s */" [expr [llength $angles] / 2] $table] +} +puts "};" -- cgit v1.2.3