UAV-develop/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-25 21:40:07 +08:00
Code Issues Packages Projects Releases Wiki Activity

New multirotor mixer; builds, not yet tested.

Browse Source
This commit is contained in:
px4dev
2012-08-15 00:46:15 -07:00
parent 3edd6c86f2
commit 5198a9daf7
7 changed files with 361 additions and 30 deletions
Download Patch File Download Diff File Expand all files Collapse all files

23
ROMFS/mixers/FMU_multirotor.mix
View File

@@ -1,24 +1,7 @@
Multirotor mixer for PX4FMU
===========================
This file defines passthrough mixers suitable for driving ESCs over the full
input range.
Channel group 0, channels 0-3 values 0.0 - 1.0 are scaled to the full output range.
M: 1
O: 10000 10000 0 -10000 10000
S: 0 0 0 20000 -10000 -10000 10000
M: 1
O: 10000 10000 0 -10000 10000
S: 0 1 0 20000 -10000 -10000 10000
M: 1
O: 10000 10000 0 -10000 10000
S: 0 2 0 20000 -10000 -10000 10000
M: 1
O: 10000 10000 0 -10000 10000
S: 0 3 0 20000 -10000 -10000 10000
This file defines a single mixer for a quadrotor in the X configuration, with 10% contribution from
roll and pitch and 20% contribution from yaw and no deadband.
R: 4x 1000 1000 2000 0

28
ROMFS/mixers/README
View File

@@ -125,4 +125,30 @@ operations, the values stored in the definition file are scaled by a factor of
Multirotor Mixer
................
The multirotor mixer is not yet defined.
The multirotor mixer combines four control inputs (roll, pitch, yaw, thrust)
into a set of actuator outputs intended to drive motor speed controllers.
The mixer definition is a single line of the form:
R: <geometry> <roll scale> <pitch scale> <yaw scale> <deadband>
The supported geometries include:
4x - quadrotor in X configuration
4+ - quadrotor in + configuration
6x - hexcopter in X configuration
6+ - hexcopter in + configuration
8x - octocopter in X configuration
8+ - octocopter in + configuration
Each of the roll, pitch and yaw scale values determine scaling of the roll,
pitch and yaw controls relative to the thrust control. Whilst the calculations
are performed as floating-point operations, the values stored in the definition
file are scaled by a factor of 10000; i.e. an factor of 0.5 is encoded as 5000.
Roll, pitch and yaw inputs are expected to range from -1.0 to 1.0, whilst the
thrust input ranges from 0.0 to 1.0. Output for each actuator is in the
range -1.0 to 1.0.
In the case where an actuator saturates, all actuator values are rescaled so that
the saturating actuator is limited to 1.0.

11
apps/systemlib/mixer/mixer.cpp
View File

@@ -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)
{

67
apps/systemlib/mixer/mixer.h
View File

@@ -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

46
apps/systemlib/mixer/mixer_group.cpp
View File

@@ -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;

123
apps/systemlib/mixer/mixer_multirotor.cpp
View File

@@ -50,14 +50,100 @@
#include <stdio.h>
#include <math.h>
#include <unistd.h>
#include <math.h>
#include "mixer.h"
#define CW (-1.0f)
#define CCW (1.0f)
namespace
{
/*
* These tables automatically generated by multi_tables - do not edit.
*/
const MultirotorMixer::Rotor _config_quad_x[] = {
{ -0.707107, 0.707107, 1.00 },
{ 0.707107, -0.707107, 1.00 },
{ 0.707107, 0.707107, -1.00 },
{ -0.707107, -0.707107, -1.00 },
};
const MultirotorMixer::Rotor _config_quad_plus[] = {
{ -1.000000, 0.000000, 1.00 },
{ 1.000000, 0.000000, 1.00 },
{ 0.000000, 1.000000, -1.00 },
{ -0.000000, -1.000000, -1.00 },
};
const MultirotorMixer::Rotor _config_hex_x[] = {
{ -1.000000, 0.000000, -1.00 },
{ 1.000000, 0.000000, 1.00 },
{ 0.500000, 0.866025, -1.00 },
{ -0.500000, -0.866025, 1.00 },
{ -0.500000, 0.866025, 1.00 },
{ 0.500000, -0.866025, -1.00 },
};
const MultirotorMixer::Rotor _config_hex_plus[] = {
{ 0.000000, 1.000000, -1.00 },
{ -0.000000, -1.000000, 1.00 },
{ 0.866025, -0.500000, -1.00 },
{ -0.866025, 0.500000, 1.00 },
{ 0.866025, 0.500000, 1.00 },
{ -0.866025, -0.500000, -1.00 },
};
const MultirotorMixer::Rotor _config_octa_x[] = {
{ -0.382683, 0.923880, -1.00 },
{ 0.382683, -0.923880, -1.00 },
{ -0.923880, 0.382683, 1.00 },
{ -0.382683, -0.923880, 1.00 },
{ 0.382683, 0.923880, 1.00 },
{ 0.923880, -0.382683, 1.00 },
{ 0.923880, 0.382683, -1.00 },
{ -0.923880, -0.382683, -1.00 },
};
const MultirotorMixer::Rotor _config_octa_plus[] = {
{ 0.000000, 1.000000, -1.00 },
{ -0.000000, -1.000000, -1.00 },
{ -0.707107, 0.707107, 1.00 },
{ -0.707107, -0.707107, 1.00 },
{ 0.707107, 0.707107, 1.00 },
{ 0.707107, -0.707107, 1.00 },
{ 1.000000, 0.000000, -1.00 },
{ -1.000000, 0.000000, -1.00 },
};
const MultirotorMixer::Rotor *_config_index[MultirotorMixer::Geometry::MAX_GEOMETRY] = {
&_config_quad_x[0],
&_config_quad_plus[0],
&_config_hex_x[0],
&_config_hex_plus[0],
&_config_octa_x[0],
&_config_octa_plus[0],
};
const unsigned _config_rotor_count[MultirotorMixer::Geometry::MAX_GEOMETRY] = {
4, /* quad_x */
4, /* quad_plus */
6, /* hex_x */
6, /* hex_plus */
8, /* octa_x */
8, /* octa_plus */
};
}
MultirotorMixer::MultirotorMixer(ControlCallback control_cb,
uintptr_t cb_handle,
MultirotorMixer::Geometry geom) :
Geometry geometry,
float roll_scale,
float pitch_scale,
float yaw_scale,
float deadband) :
Mixer(control_cb, cb_handle),
_geometry(geom)
_roll_scale(roll_scale),
_pitch_scale(pitch_scale),
_yaw_scale(yaw_scale),
_deadband(-1.0 + deadband), /* shift to output range here to avoid runtime calculation */
_rotor_count(_config_rotor_count[geometry]),
_rotors(_config_index[geometry])
{
}
@@ -68,7 +154,38 @@ MultirotorMixer::~MultirotorMixer()
unsigned
MultirotorMixer::mix(float *outputs, unsigned space)
{
/* XXX implement this */
float roll = get_control(0, 0) * _roll_scale;
float pitch = get_control(0, 1) * _pitch_scale;
float yaw = get_control(0, 2) * _yaw_scale;
float thrust = get_control(0, 3);
float max = 0.0f;
float fixup_scale;
/* perform initial mix pass yielding un-bounded outputs */
for (unsigned i = 0; i < _rotor_count; i++) {
float tmp = roll * _rotors[i].roll_scale +
pitch * _rotors[i].pitch_scale +
yaw * _rotors[i].yaw_scale +
thrust;
if (tmp > max)
max = tmp;
outputs[i] = tmp;
}
/* scale values into the -1.0 - 1.0 range */
if (max > 1.0f) {
fixup_scale = 2.0f / max;
} else {
fixup_scale = 2.0f;
}
for (unsigned i = 0; i < _rotor_count; i++)
outputs[i] *= -1.0 + (outputs[i] * fixup_scale);
/* ensure outputs are out of the deadband */
for (unsigned i = 0; i < _rotor_count; i++)
if (outputs[i] < _deadband)
outputs[i] = _deadband;
return 0;
}

93
apps/systemlib/mixer/multi_tables Executable file
View File

@@ -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 "};"