fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-18 23:23:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

Moved last libs, drivers and headers, cleaned up IO build

Browse Source
This commit is contained in:
Lorenz Meier
2013-04-28 09:54:11 +02:00
parent f57439b90e
commit 13fc670386
423 changed files with 89 additions and 1024 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/systemlib/mixer/mixer.cpp
+152
View File
@@ -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 <nuttx/config.h>
#include <sys/types.h>
#include <stdint.h>
#include <stdbool.h>
#include <stdlib.h>
#include <string.h>
#include <fcntl.h>
#include <poll.h>
#include <errno.h>
#include <stdio.h>
#include <math.h>
#include <unistd.h>
#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;
}
src/modules/systemlib/mixer/mixer.h
+499
View File
@@ -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: <control count>
* O: <-ve scale> <+ve scale> <offset> <lower limit> <upper limit>
*
* The definition continues with <control count> entries describing the control
* inputs and their scaling, in the form:
*
* S: <group> <index> <-ve scale> <+ve scale> <offset> <lower limit> <upper limit>
*
* Multirotor Mixer
* ................
*
* The multirotor mixer definition is a single line of the form:
*
* R: <geometry> <roll scale> <pitch scale> <yaw scale> <deadband>
*
* @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
src/modules/systemlib/mixer/mixer_group.cpp
+185
View File
@@ -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 <nuttx/config.h>
#include <sys/types.h>
#include <stdint.h>
#include <stdbool.h>
#include <stdlib.h>
#include <string.h>
#include <fcntl.h>
#include <poll.h>
#include <errno.h>
#include <stdio.h>
#include <math.h>
#include <unistd.h>
#include "mixer.h"
#define debug(fmt, args...) do { } while(0)
//#define debug(fmt, args...) do { printf("[mixer] " fmt "\n", ##args); } while(0)
//#include <debug.h>
//#define debug(fmt, args...) 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;
}
src/modules/systemlib/mixer/mixer_multirotor.cpp
+301
View File
@@ -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 <nuttx/config.h>
#include <sys/types.h>
#include <stdint.h>
#include <stdbool.h>
#include <stdlib.h>
#include <string.h>
#include <fcntl.h>
#include <poll.h>
#include <errno.h>
#include <stdio.h>
#include <math.h>
#include <unistd.h>
#include <math.h>
#include "mixer.h"
#define debug(fmt, args...) do { } while(0)
//#define debug(fmt, args...) do { printf("[mixer] " fmt "\n", ##args); } while(0)
//#include <debug.h>
//#define debug(fmt, args...) 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);
}
src/modules/systemlib/mixer/mixer_simple.cpp
+334
View File
@@ -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 <nuttx/config.h>
#include <sys/types.h>
#include <stdint.h>
#include <stdbool.h>
#include <stdlib.h>
#include <string.h>
#include <fcntl.h>
#include <poll.h>
#include <errno.h>
#include <stdio.h>
#include <math.h>
#include <unistd.h>
#include <ctype.h>
#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;
}
src/modules/systemlib/mixer/module.mk
+42
View File
@@ -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
src/modules/systemlib/mixer/multi_tables
Executable
+100
View File
@@ -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 "};"