fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-24 01:53:35 +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
apps/systemlib/mixer/mixer_multirotor.cpp
+120 -3
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;
}