From 13fc6703862862f4263d8d5d085b7a16b87190e1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 28 Apr 2013 09:54:11 +0200 Subject: Moved last libs, drivers and headers, cleaned up IO build --- src/modules/systemlib/mixer/mixer.cpp | 152 +++++++ src/modules/systemlib/mixer/mixer.h | 499 +++++++++++++++++++++++ src/modules/systemlib/mixer/mixer_group.cpp | 185 +++++++++ src/modules/systemlib/mixer/mixer_multirotor.cpp | 301 ++++++++++++++ src/modules/systemlib/mixer/mixer_simple.cpp | 334 +++++++++++++++ src/modules/systemlib/mixer/module.mk | 42 ++ src/modules/systemlib/mixer/multi_tables | 100 +++++ 7 files changed, 1613 insertions(+) create mode 100644 src/modules/systemlib/mixer/mixer.cpp create mode 100644 src/modules/systemlib/mixer/mixer.h create mode 100644 src/modules/systemlib/mixer/mixer_group.cpp create mode 100644 src/modules/systemlib/mixer/mixer_multirotor.cpp create mode 100644 src/modules/systemlib/mixer/mixer_simple.cpp create mode 100644 src/modules/systemlib/mixer/module.mk create mode 100755 src/modules/systemlib/mixer/multi_tables (limited to 'src/modules/systemlib/mixer') diff --git a/src/modules/systemlib/mixer/mixer.cpp b/src/modules/systemlib/mixer/mixer.cpp new file mode 100644 index 000000000..df0dfe838 --- /dev/null +++ b/src/modules/systemlib/mixer/mixer.cpp @@ -0,0 +1,152 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mixer.cpp + * + * Programmable multi-channel mixer library. + */ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "mixer.h" + +Mixer::Mixer(ControlCallback control_cb, uintptr_t cb_handle) : + _next(nullptr), + _control_cb(control_cb), + _cb_handle(cb_handle) +{ +} + +float +Mixer::get_control(uint8_t group, uint8_t index) +{ + float value; + + _control_cb(_cb_handle, group, index, value); + + return value; +} + + +float +Mixer::scale(const mixer_scaler_s &scaler, float input) +{ + float output; + + if (input < 0.0f) { + output = (input * scaler.negative_scale) + scaler.offset; + + } else { + output = (input * scaler.positive_scale) + scaler.offset; + } + + if (output > scaler.max_output) { + output = scaler.max_output; + + } else if (output < scaler.min_output) { + output = scaler.min_output; + } + + return output; +} + +int +Mixer::scale_check(struct mixer_scaler_s &scaler) +{ + if (scaler.offset > 1.001f) + return 1; + + if (scaler.offset < -1.001f) + return 2; + + if (scaler.min_output > scaler.max_output) + return 3; + + if (scaler.min_output < -1.001f) + return 4; + + if (scaler.max_output > 1.001f) + return 5; + + return 0; +} + +/****************************************************************************/ + +NullMixer::NullMixer() : + Mixer(nullptr, 0) +{ +} + +unsigned +NullMixer::mix(float *outputs, unsigned space) +{ + if (space > 0) { + *outputs = 0.0f; + return 1; + } + + return 0; +} + +void +NullMixer::groups_required(uint32_t &groups) +{ + +} + +NullMixer * +NullMixer::from_text(const char *buf, unsigned &buflen) +{ + NullMixer *nm = nullptr; + + if ((buflen >= 2) && (buf[0] == 'Z') && (buf[1] == ':')) { + nm = new NullMixer; + buflen -= 2; + } + + return nm; +} diff --git a/src/modules/systemlib/mixer/mixer.h b/src/modules/systemlib/mixer/mixer.h new file mode 100644 index 000000000..40d37fce2 --- /dev/null +++ b/src/modules/systemlib/mixer/mixer.h @@ -0,0 +1,499 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mixer.h + * + * Generic, programmable, procedural control signal mixers. + * + * This library implements a generic mixer interface that can be used + * by any driver or subsytem that wants to combine several control signals + * into a single output. + * + * Terminology + * =========== + * + * control value + * A mixer input value, typically provided by some controlling + * component of the system. + * + * control group + * A collection of controls provided by a single controlling component. + * + * actuator + * The mixer output value. + * + * + * Mixing basics + * ============= + * + * An actuator derives its value from the combination of one or more + * control values. Each of the control values is scaled according to + * the actuator's configuration and then combined to produce the + * actuator value, which may then be further scaled to suit the specific + * output type. + * + * Internally, all scaling is performed using floating point values. + * Inputs and outputs are clamped to the range -1.0 to 1.0. + * + * control control control + * | | | + * v v v + * scale scale scale + * | | | + * | v | + * +-------> mix <------+ + * | + * scale + * | + * v + * out + * + * Scaling + * ------- + * + * Each scaler allows the input value to be scaled independently for + * inputs greater/less than zero. An offset can be applied to the output, + * as well as lower and upper boundary constraints. + * Negative scaling factors cause the output to be inverted (negative input + * produces positive output). + * + * Scaler pseudocode: + * + * if (input < 0) + * output = (input * NEGATIVE_SCALE) + OFFSET + * else + * output = (input * POSITIVE_SCALE) + OFFSET + * + * if (output < LOWER_LIMIT) + * output = LOWER_LIMIT + * if (output > UPPER_LIMIT) + * output = UPPER_LIMIT + * + * + * Mixing + * ------ + * + * Mixing behaviour varies based on the specific mixer class; each + * mixer class describes its behaviour in more detail. + * + * + * Controls + * -------- + * + * The precise assignment of controls may vary depending on the + * application, but the following assignments should be used + * when appropriate. Some mixer classes have specific assumptions + * about the assignment of controls. + * + * control | standard meaning + * --------+----------------------- + * 0 | roll + * 1 | pitch + * 2 | yaw + * 3 | primary thrust + */ + + +#ifndef _SYSTEMLIB_MIXER_MIXER_H +#define _SYSTEMLIB_MIXER_MIXER_H value + +#include "drivers/drv_mixer.h" + +/** + * Abstract class defining a mixer mixing zero or more inputs to + * one or more outputs. + */ +class __EXPORT Mixer +{ +public: + /** next mixer in a list */ + Mixer *_next; + + /** + * Fetch a control value. + * + * @param handle Token passed when the callback is registered. + * @param control_group The group to fetch the control from. + * @param control_index The group-relative index to fetch the control from. + * @param control The returned control + * @return Zero if the value was fetched, nonzero otherwise. + */ + typedef int (* ControlCallback)(uintptr_t handle, + uint8_t control_group, + uint8_t control_index, + float &control); + + /** + * Constructor. + * + * @param control_cb Callback invoked when reading controls. + */ + Mixer(ControlCallback control_cb, uintptr_t cb_handle); + virtual ~Mixer() {}; + + /** + * Perform the mixing function. + * + * @param outputs Array into which mixed output(s) should be placed. + * @param space The number of available entries in the output array; + * @return The number of entries in the output array that were populated. + */ + virtual unsigned mix(float *outputs, unsigned space) = 0; + + /** + * Analyses the mix configuration and updates a bitmask of groups + * that are required. + * + * @param groups A bitmask of groups (0-31) that the mixer requires. + */ + virtual void groups_required(uint32_t &groups) = 0; + +protected: + /** client-supplied callback used when fetching control values */ + ControlCallback _control_cb; + uintptr_t _cb_handle; + + /** + * Invoke the client callback to fetch a control value. + * + * @param group Control group to fetch from. + * @param index Control index to fetch. + * @return The control value. + */ + float get_control(uint8_t group, uint8_t index); + + /** + * Perform simpler linear scaling. + * + * @param scaler The scaler configuration. + * @param input The value to be scaled. + * @return The scaled value. + */ + static float scale(const mixer_scaler_s &scaler, float input); + + /** + * Validate a scaler + * + * @param scaler The scaler to be validated. + * @return Zero if good, nonzero otherwise. + */ + static int scale_check(struct mixer_scaler_s &scaler); + +private: +}; + +/** + * Group of mixers, built up from single mixers and processed + * in order when mixing. + */ +class __EXPORT MixerGroup : public Mixer +{ +public: + MixerGroup(ControlCallback control_cb, uintptr_t cb_handle); + ~MixerGroup(); + + virtual unsigned mix(float *outputs, unsigned space); + virtual void groups_required(uint32_t &groups); + + /** + * Add a mixer to the group. + * + * @param mixer The mixer to be added. + */ + void add_mixer(Mixer *mixer); + + /** + * Remove all the mixers from the group. + */ + void reset(); + + /** + * Adds mixers to the group based on a text description in a buffer. + * + * Mixer definitions begin with a single capital letter and a colon. + * The actual format of the mixer definition varies with the individual + * mixers; they are summarised here, but see ROMFS/mixers/README for + * more details. + * + * Null Mixer + * .......... + * + * The null mixer definition has the form: + * + * Z: + * + * Simple Mixer + * ............ + * + * A simple mixer definition begins with: + * + * M: + * O: <-ve scale> <+ve scale> + * + * The definition continues with entries describing the control + * inputs and their scaling, in the form: + * + * S: <-ve scale> <+ve scale> + * + * Multirotor Mixer + * ................ + * + * The multirotor mixer definition is a single line of the form: + * + * R: + * + * @param buf The mixer configuration buffer. + * @param buflen The length of the buffer, updated to reflect + * bytes as they are consumed. + * @return Zero on successful load, nonzero otherwise. + */ + int load_from_buf(const char *buf, unsigned &buflen); + +private: + Mixer *_first; /**< linked list of mixers */ +}; + +/** + * Null mixer; returns zero. + * + * Used as a placeholder for output channels that are unassigned in groups. + */ +class __EXPORT NullMixer : public Mixer +{ +public: + NullMixer(); + ~NullMixer() {}; + + /** + * Factory method. + * + * Given a pointer to a buffer containing a text description of the mixer, + * returns a pointer to a new instance of the mixer. + * + * @param buf Buffer containing a text description of + * the mixer. + * @param buflen Length of the buffer in bytes, adjusted + * to reflect the bytes consumed. + * @return A new NullMixer instance, or nullptr + * if the text format is bad. + */ + static NullMixer *from_text(const char *buf, unsigned &buflen); + + virtual unsigned mix(float *outputs, unsigned space); + virtual void groups_required(uint32_t &groups); +}; + +/** + * Simple summing mixer. + * + * Collects zero or more inputs and mixes them to a single output. + */ +class __EXPORT SimpleMixer : public Mixer +{ +public: + /** + * Constructor + * + * @param mixinfo Mixer configuration. The pointer passed + * becomes the property of the mixer and + * will be freed when the mixer is deleted. + */ + SimpleMixer(ControlCallback control_cb, + uintptr_t cb_handle, + mixer_simple_s *mixinfo); + ~SimpleMixer(); + + /** + * Factory method with full external configuration. + * + * Given a pointer to a buffer containing a text description of the mixer, + * returns a pointer to a new instance of the mixer. + * + * @param control_cb The callback to invoke when fetching a + * control value. + * @param cb_handle Handle passed to the control callback. + * @param buf Buffer containing a text description of + * the mixer. + * @param buflen Length of the buffer in bytes, adjusted + * to reflect the bytes consumed. + * @return A new SimpleMixer instance, or nullptr + * if the text format is bad. + */ + static SimpleMixer *from_text(Mixer::ControlCallback control_cb, + uintptr_t cb_handle, + const char *buf, + unsigned &buflen); + + /** + * Factory method for PWM/PPM input to internal float representation. + * + * @param control_cb The callback to invoke when fetching a + * control value. + * @param cb_handle Handle passed to the control callback. + * @param input The control index used when fetching the input. + * @param min The PWM/PPM value considered to be "minimum" (gives -1.0 out) + * @param mid The PWM/PPM value considered to be the midpoint (gives 0.0 out) + * @param max The PWM/PPM value considered to be "maximum" (gives 1.0 out) + * @return A new SimpleMixer instance, or nullptr if one could not be + * allocated. + */ + static SimpleMixer *pwm_input(Mixer::ControlCallback control_cb, + uintptr_t cb_handle, + unsigned input, + uint16_t min, + uint16_t mid, + uint16_t max); + + virtual unsigned mix(float *outputs, unsigned space); + virtual void groups_required(uint32_t &groups); + + /** + * Check that the mixer configuration as loaded is sensible. + * + * Note that this function will call control_cb, but only cares about + * error returns, not the input value. + * + * @return Zero if the mixer makes sense, nonzero otherwise. + */ + int check(); + +protected: + +private: + mixer_simple_s *_info; + + static int parse_output_scaler(const char *buf, unsigned &buflen, mixer_scaler_s &scaler); + static int parse_control_scaler(const char *buf, + unsigned &buflen, + mixer_scaler_s &scaler, + uint8_t &control_group, + uint8_t &control_index); +}; + +/** + * Multi-rotor mixer for pre-defined vehicle geometries. + * + * Collects four inputs (roll, pitch, yaw, thrust) and mixes them to + * a set of outputs based on the configured geometry. + */ +class __EXPORT MultirotorMixer : public Mixer +{ +public: + /** + * Supported multirotor geometries. + * + * XXX add more + */ + enum Geometry { + QUAD_X = 0, /**< quad in X configuration */ + QUAD_PLUS, /**< quad in + configuration */ + QUAD_V, /**< quad in V 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 geometry, + float roll_scale, + float pitch_scale, + float yaw_scale, + float deadband); + ~MultirotorMixer(); + + /** + * Factory method. + * + * Given a pointer to a buffer containing a text description of the mixer, + * returns a pointer to a new instance of the mixer. + * + * @param control_cb The callback to invoke when fetching a + * control value. + * @param cb_handle Handle passed to the control callback. + * @param buf Buffer containing a text description of + * the mixer. + * @param buflen Length of the buffer in bytes, adjusted + * to reflect the bytes consumed. + * @return A new MultirotorMixer instance, or nullptr + * if the text format is bad. + */ + static MultirotorMixer *from_text(Mixer::ControlCallback control_cb, + uintptr_t cb_handle, + const char *buf, + unsigned &buflen); + + virtual unsigned mix(float *outputs, unsigned space); + virtual void groups_required(uint32_t &groups); + +private: + float _roll_scale; + float _pitch_scale; + float _yaw_scale; + float _deadband; + + unsigned _rotor_count; + const Rotor *_rotors; + +}; + +#endif diff --git a/src/modules/systemlib/mixer/mixer_group.cpp b/src/modules/systemlib/mixer/mixer_group.cpp new file mode 100644 index 000000000..1dbc512cd --- /dev/null +++ b/src/modules/systemlib/mixer/mixer_group.cpp @@ -0,0 +1,185 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mixer_group.cpp + * + * Mixer collection. + */ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "mixer.h" + +#define debug(fmt, args...) do { } while(0) +//#define debug(fmt, args...) do { printf("[mixer] " fmt "\n", ##args); } while(0) +//#include +//#define debug(fmt, args...) lowsyslog(fmt "\n", ##args) + +MixerGroup::MixerGroup(ControlCallback control_cb, uintptr_t cb_handle) : + Mixer(control_cb, cb_handle), + _first(nullptr) +{ +} + +MixerGroup::~MixerGroup() +{ + reset(); +} + +void +MixerGroup::add_mixer(Mixer *mixer) +{ + Mixer **mpp; + + mpp = &_first; + + while (*mpp != nullptr) + mpp = &((*mpp)->_next); + + *mpp = mixer; + mixer->_next = nullptr; +} + +void +MixerGroup::reset() +{ + Mixer *mixer; + + /* discard sub-mixers */ + while (_first != nullptr) { + mixer = _first; + _first = mixer->_next; + delete mixer; + mixer = nullptr; + } +} + +unsigned +MixerGroup::mix(float *outputs, unsigned space) +{ + Mixer *mixer = _first; + unsigned index = 0; + + while ((mixer != nullptr) && (index < space)) { + index += mixer->mix(outputs + index, space - index); + mixer = mixer->_next; + } + + return index; +} + +void +MixerGroup::groups_required(uint32_t &groups) +{ + Mixer *mixer = _first; + + while (mixer != nullptr) { + mixer->groups_required(groups); + mixer = mixer->_next; + } +} + +int +MixerGroup::load_from_buf(const char *buf, unsigned &buflen) +{ + int ret = -1; + const char *end = buf + buflen; + + /* + * Loop until either we have emptied the buffer, or we have failed to + * allocate something when we expected to. + */ + while (buflen > 0) { + Mixer *m = nullptr; + const char *p = end - buflen; + unsigned resid = buflen; + + /* + * Use the next character as a hint to decide which mixer class to construct. + */ + switch (*p) { + case 'Z': + m = NullMixer::from_text(p, resid); + break; + + case 'M': + m = SimpleMixer::from_text(_control_cb, _cb_handle, p, resid); + break; + + case 'R': + m = MultirotorMixer::from_text(_control_cb, _cb_handle, p, resid); + break; + + default: + /* it's probably junk or whitespace, skip a byte and retry */ + buflen--; + continue; + } + + /* + * If we constructed something, add it to the group. + */ + if (m != nullptr) { + add_mixer(m); + + /* we constructed something */ + ret = 0; + + /* only adjust buflen if parsing was successful */ + buflen = resid; + } else { + + /* + * There is data in the buffer that we expected to parse, but it didn't, + * so give up for now. + */ + break; + } + } + + /* nothing more in the buffer for us now */ + return ret; +} diff --git a/src/modules/systemlib/mixer/mixer_multirotor.cpp b/src/modules/systemlib/mixer/mixer_multirotor.cpp new file mode 100644 index 000000000..d79811c0f --- /dev/null +++ b/src/modules/systemlib/mixer/mixer_multirotor.cpp @@ -0,0 +1,301 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mixer_multirotor.cpp + * + * Multi-rotor mixers. + */ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "mixer.h" + +#define debug(fmt, args...) do { } while(0) +//#define debug(fmt, args...) do { printf("[mixer] " fmt "\n", ##args); } while(0) +//#include +//#define debug(fmt, args...) lowsyslog(fmt "\n", ##args) + +/* + * Clockwise: 1 + * Counter-clockwise: -1 + */ + +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_quad_v[] = { + { -0.927184, 0.374607, 1.00 }, + { 0.694658, -0.719340, 1.00 }, + { 0.927184, 0.374607, -1.00 }, + { -0.694658, -0.719340, -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_quad_v[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 */ + 4, /* quad_v */ + 6, /* hex_x */ + 6, /* hex_plus */ + 8, /* octa_x */ + 8, /* octa_plus */ +}; + +} + +MultirotorMixer::MultirotorMixer(ControlCallback control_cb, + uintptr_t cb_handle, + Geometry geometry, + float roll_scale, + float pitch_scale, + float yaw_scale, + float deadband) : + Mixer(control_cb, cb_handle), + _roll_scale(roll_scale), + _pitch_scale(pitch_scale), + _yaw_scale(yaw_scale), + _deadband(-1.0f + deadband), /* shift to output range here to avoid runtime calculation */ + _rotor_count(_config_rotor_count[geometry]), + _rotors(_config_index[geometry]) +{ +} + +MultirotorMixer::~MultirotorMixer() +{ +} + +MultirotorMixer * +MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, const char *buf, unsigned &buflen) +{ + MultirotorMixer::Geometry geometry; + char geomname[8]; + int s[4]; + int used; + + if (sscanf(buf, "R: %s %d %d %d %d%n", geomname, &s[0], &s[1], &s[2], &s[3], &used) != 5) { + debug("multirotor parse failed on '%s'", buf); + return nullptr; + } + + if (used > (int)buflen) { + debug("multirotor spec used %d of %u", used, buflen); + return nullptr; + } + + buflen -= used; + + if (!strcmp(geomname, "4+")) { + geometry = MultirotorMixer::QUAD_PLUS; + + } else if (!strcmp(geomname, "4x")) { + geometry = MultirotorMixer::QUAD_X; + + } else if (!strcmp(geomname, "4v")) { + geometry = MultirotorMixer::QUAD_V; + + } 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; + } + + debug("adding multirotor mixer '%s'", geomname); + + return new MultirotorMixer( + control_cb, + cb_handle, + geometry, + s[0] / 10000.0f, + s[1] / 10000.0f, + s[2] / 10000.0f, + s[3] / 10000.0f); +} + +unsigned +MultirotorMixer::mix(float *outputs, unsigned space) +{ + float roll = get_control(0, 0) * _roll_scale; + //lowsyslog("roll: %d, get_control0: %d, %d\n", (int)(roll), (int)(get_control(0, 0)), (int)(_roll_scale)); + float pitch = get_control(0, 1) * _pitch_scale; + float yaw = get_control(0, 2) * _yaw_scale; + float thrust = get_control(0, 3); + //lowsyslog("thrust: %d, get_control3: %d\n", (int)(thrust), (int)(get_control(0, 3))); + float max = 0.0f; + float fixup_scale; + + /* use an output factor to prevent too strong control signals at low throttle */ + float min_thrust = 0.05f; + float max_thrust = 1.0f; + float startpoint_full_control = 0.40f; + float output_factor; + + /* keep roll, pitch and yaw control to 0 below min thrust */ + if (thrust <= min_thrust) { + output_factor = 0.0f; + /* linearly increase the output factor from 0 to 1 between min_thrust and startpoint_full_control */ + + } else if (thrust < startpoint_full_control && thrust > min_thrust) { + output_factor = (thrust / max_thrust) / (startpoint_full_control - min_thrust); + /* and then stay at full control */ + + } else { + output_factor = max_thrust; + } + + roll *= output_factor; + pitch *= output_factor; + yaw *= output_factor; + + + /* 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.0f + (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 _rotor_count; +} + +void +MultirotorMixer::groups_required(uint32_t &groups) +{ + /* XXX for now, hardcoded to indexes 0-3 in control group zero */ + groups |= (1 << 0); +} + diff --git a/src/modules/systemlib/mixer/mixer_simple.cpp b/src/modules/systemlib/mixer/mixer_simple.cpp new file mode 100644 index 000000000..07dc5f37f --- /dev/null +++ b/src/modules/systemlib/mixer/mixer_simple.cpp @@ -0,0 +1,334 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mixer_simple.cpp + * + * Simple summing mixer. + */ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "mixer.h" + +#define debug(fmt, args...) do { } while(0) +//#define debug(fmt, args...) do { printf("[mixer] " fmt "\n", ##args); } while(0) + +SimpleMixer::SimpleMixer(ControlCallback control_cb, + uintptr_t cb_handle, + mixer_simple_s *mixinfo) : + Mixer(control_cb, cb_handle), + _info(mixinfo) +{ +} + +SimpleMixer::~SimpleMixer() +{ + if (_info != nullptr) + free(_info); +} + +static const char * +findtag(const char *buf, unsigned &buflen, char tag) +{ + while (buflen >= 2) { + if ((buf[0] == tag) && (buf[1] == ':')) + return buf; + buf++; + buflen--; + } + return nullptr; +} + +static void +skipline(const char *buf, unsigned &buflen) +{ + const char *p; + + /* if we can find a CR or NL in the buffer, skip up to it */ + if ((p = (const char *)memchr(buf, '\r', buflen)) || (p = (const char *)memchr(buf, '\n', buflen))) + buflen -= (p - buf); +} + +int +SimpleMixer::parse_output_scaler(const char *buf, unsigned &buflen, mixer_scaler_s &scaler) +{ + int ret; + int s[5]; + + buf = findtag(buf, buflen, 'O'); + if ((buf == nullptr) || (buflen < 12)) + return -1; + + if ((ret = sscanf(buf, "O: %d %d %d %d %d", + &s[0], &s[1], &s[2], &s[3], &s[4])) != 5) { + debug("scaler parse failed on '%s' (got %d)", buf, ret); + return -1; + } + skipline(buf, buflen); + + scaler.negative_scale = s[0] / 10000.0f; + scaler.positive_scale = s[1] / 10000.0f; + scaler.offset = s[2] / 10000.0f; + scaler.min_output = s[3] / 10000.0f; + scaler.max_output = s[4] / 10000.0f; + + return 0; +} + +int +SimpleMixer::parse_control_scaler(const char *buf, unsigned &buflen, mixer_scaler_s &scaler, uint8_t &control_group, uint8_t &control_index) +{ + unsigned u[2]; + int s[5]; + + buf = findtag(buf, buflen, 'S'); + if ((buf == nullptr) || (buflen < 16)) + return -1; + + if (sscanf(buf, "S: %u %u %d %d %d %d %d", + &u[0], &u[1], &s[0], &s[1], &s[2], &s[3], &s[4]) != 7) { + debug("control parse failed on '%s'", buf); + return -1; + } + skipline(buf, buflen); + + control_group = u[0]; + control_index = u[1]; + scaler.negative_scale = s[0] / 10000.0f; + scaler.positive_scale = s[1] / 10000.0f; + scaler.offset = s[2] / 10000.0f; + scaler.min_output = s[3] / 10000.0f; + scaler.max_output = s[4] / 10000.0f; + + return 0; +} + +SimpleMixer * +SimpleMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, const char *buf, unsigned &buflen) +{ + SimpleMixer *sm = nullptr; + mixer_simple_s *mixinfo = nullptr; + unsigned inputs; + int used; + const char *end = buf + buflen; + + /* get the base info for the mixer */ + if (sscanf(buf, "M: %u%n", &inputs, &used) != 1) { + debug("simple parse failed on '%s'", buf); + goto out; + } + + buflen -= used; + + mixinfo = (mixer_simple_s *)malloc(MIXER_SIMPLE_SIZE(inputs)); + + if (mixinfo == nullptr) { + debug("could not allocate memory for mixer info"); + goto out; + } + + mixinfo->control_count = inputs; + + if (parse_output_scaler(end - buflen, buflen, mixinfo->output_scaler)) + goto out; + + for (unsigned i = 0; i < inputs; i++) { + if (parse_control_scaler(end - buflen, buflen, + mixinfo->controls[i].scaler, + mixinfo->controls[i].control_group, + mixinfo->controls[i].control_index)) + goto out; + } + + sm = new SimpleMixer(control_cb, cb_handle, mixinfo); + + if (sm != nullptr) { + mixinfo = nullptr; + debug("loaded mixer with %d inputs", inputs); + + } else { + debug("could not allocate memory for mixer"); + } + +out: + + if (mixinfo != nullptr) + free(mixinfo); + + return sm; +} + +SimpleMixer * +SimpleMixer::pwm_input(Mixer::ControlCallback control_cb, uintptr_t cb_handle, unsigned input, uint16_t min, uint16_t mid, uint16_t max) +{ + SimpleMixer *sm = nullptr; + mixer_simple_s *mixinfo = nullptr; + + mixinfo = (mixer_simple_s *)malloc(MIXER_SIMPLE_SIZE(1)); + + if (mixinfo == nullptr) { + debug("could not allocate memory for mixer info"); + goto out; + } + + mixinfo->control_count = 1; + + /* + * Always pull from group 0, with the input value giving the channel. + */ + mixinfo->controls[0].control_group = 0; + mixinfo->controls[0].control_index = input; + + /* + * Conversion uses both the input and output side of the mixer. + * + * The input side is used to slide the control value such that the min argument + * results in a value of zero. + * + * The output side is used to apply the scaling for the min/max values so that + * the resulting output is a -1.0 ... 1.0 value for the min...max range. + */ + mixinfo->controls[0].scaler.negative_scale = 1.0f; + mixinfo->controls[0].scaler.positive_scale = 1.0f; + mixinfo->controls[0].scaler.offset = -mid; + mixinfo->controls[0].scaler.min_output = -(mid - min); + mixinfo->controls[0].scaler.max_output = (max - mid); + + mixinfo->output_scaler.negative_scale = 500.0f / (mid - min); + mixinfo->output_scaler.positive_scale = 500.0f / (max - mid); + mixinfo->output_scaler.offset = 0.0f; + mixinfo->output_scaler.min_output = -1.0f; + mixinfo->output_scaler.max_output = 1.0f; + + sm = new SimpleMixer(control_cb, cb_handle, mixinfo); + + if (sm != nullptr) { + mixinfo = nullptr; + debug("PWM input mixer for %d", input); + + } else { + debug("could not allocate memory for PWM input mixer"); + } + +out: + + if (mixinfo != nullptr) + free(mixinfo); + + return sm; +} + +unsigned +SimpleMixer::mix(float *outputs, unsigned space) +{ + float sum = 0.0f; + + if (_info == nullptr) + return 0; + + if (space < 1) + return 0; + + for (unsigned i = 0; i < _info->control_count; i++) { + float input; + + _control_cb(_cb_handle, + _info->controls[i].control_group, + _info->controls[i].control_index, + input); + + sum += scale(_info->controls[i].scaler, input); + } + + *outputs = scale(_info->output_scaler, sum); + return 1; +} + +void +SimpleMixer::groups_required(uint32_t &groups) +{ + for (unsigned i = 0; i < _info->control_count; i++) + groups |= 1 << _info->controls[i].control_group; +} + +int +SimpleMixer::check() +{ + int ret; + float junk; + + /* sanity that presumes that a mixer includes a control no more than once */ + /* max of 32 groups due to groups_required API */ + if (_info->control_count > 32) + return -2; + + /* validate the output scaler */ + ret = scale_check(_info->output_scaler); + + if (ret != 0) + return ret; + + /* validate input scalers */ + for (unsigned i = 0; i < _info->control_count; i++) { + + /* verify that we can fetch the control */ + if (_control_cb(_cb_handle, + _info->controls[i].control_group, + _info->controls[i].control_index, + junk) != 0) { + return -3; + } + + /* validate the scaler */ + ret = scale_check(_info->controls[i].scaler); + + if (ret != 0) + return (10 * i + ret); + } + + return 0; +} diff --git a/src/modules/systemlib/mixer/module.mk b/src/modules/systemlib/mixer/module.mk new file mode 100644 index 000000000..4d45e1c50 --- /dev/null +++ b/src/modules/systemlib/mixer/module.mk @@ -0,0 +1,42 @@ +############################################################################ +# +# Copyright (c) 2012, 2013 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. +# +############################################################################ + +# +# mixer library +# +LIBNAME = mixerlib + +SRCS = mixer.cpp \ + mixer_group.cpp \ + mixer_multirotor.cpp \ + mixer_simple.cpp diff --git a/src/modules/systemlib/mixer/multi_tables b/src/modules/systemlib/mixer/multi_tables new file mode 100755 index 000000000..19a8239a6 --- /dev/null +++ b/src/modules/systemlib/mixer/multi_tables @@ -0,0 +1,100 @@ +#!/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 quad_v { + 68 CCW + -136 CCW + -68 CW + 136 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 quad_v 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] [expr -$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