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

Merge pull request #942 from PX4/mc_mixer_fix

Browse Source 
Multirotor mixer: more careful limiting
This commit is contained in:
Lorenz Meier 2014-05-17 04:53:57 -07:00
commit 98f05ea5c1
2 changed files with 54 additions and 49 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/modules/systemlib/mixer/mixer.h
View File

@ -516,7 +516,7 @@ private:
float _roll_scale;
float _pitch_scale;
float _yaw_scale;
float _deadband;
float _idle_speed;
unsigned _rotor_count;
const Rotor *_rotors;

101
src/modules/systemlib/mixer/mixer_multirotor.cpp
View File

@ -67,6 +67,11 @@
namespace
{
float constrain(float val, float min, float max)
{
return (val < min) ? min : ((val > max) ? max : val);
}
/*
* These tables automatically generated by multi_tables - do not edit.
*/
@ -171,12 +176,12 @@ MultirotorMixer::MultirotorMixer(ControlCallback control_cb,
float roll_scale,
float pitch_scale,
float yaw_scale,
float deadband) :
float idle_speed) :
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 */
_idle_speed(-1.0f + idle_speed * 2.0f), /* shift to output range here to avoid runtime calculation */
_rotor_count(_config_rotor_count[geometry]),
_rotors(_config_index[geometry])
{
@ -276,67 +281,67 @@ MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handl
unsigned
MultirotorMixer::mix(float *outputs, unsigned space)
{
float roll = get_control(0, 0) * _roll_scale;
float roll = constrain(get_control(0, 0) * _roll_scale, -1.0f, 1.0f);
//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);
float pitch = constrain(get_control(0, 1) * _pitch_scale, -1.0f, 1.0f);
float yaw = constrain(get_control(0, 2) * _yaw_scale, -1.0f, 1.0f);
float thrust = constrain(get_control(0, 3), 0.0f, 1.0f);
//lowsyslog("thrust: %d, get_control3: %d\n", (int)(thrust), (int)(get_control(0, 3)));
float max = 0.0f;
float fixup_scale;
float min_out = 0.0f;
float max_out = 0.0f;
float scale_yaw = 1.0f;
/* 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 */
/* perform initial mix pass yielding unbounded outputs, ignore yaw */
for (unsigned i = 0; i < _rotor_count; i++) {
float tmp = roll * _rotors[i].roll_scale +
float out = roll * _rotors[i].roll_scale +
pitch * _rotors[i].pitch_scale +
yaw * _rotors[i].yaw_scale +
thrust;
if (tmp > max)
max = tmp;
/* limit yaw if it causes outputs clipping */
if (out >= 0.0f && out < -yaw * _rotors[i].yaw_scale) {
yaw = -out / _rotors[i].yaw_scale;
}
outputs[i] = tmp;
/* calculate min and max output values */
if (out < min_out) {
min_out = out;
}
if (out > max_out) {
max_out = out;
}
outputs[i] = out;
}
/* scale values into the -1.0 - 1.0 range */
if (max > 1.0f) {
fixup_scale = 2.0f / max;
/* scale down roll/pitch controls if some outputs are negative, don't add yaw, keep total thrust */
if (min_out < 0.0) {
float scale_in = thrust / (thrust - min_out);
/* mix again with adjusted controls */
for (unsigned i = 0; i < _rotor_count; i++) {
outputs[i] = scale_in * (roll * _rotors[i].roll_scale + pitch * _rotors[i].pitch_scale) + thrust;
}
} else {
fixup_scale = 2.0f;
/* roll/pitch mixed without limiting, add yaw control */
for (unsigned i = 0; i < _rotor_count; i++) {
outputs[i] += yaw * _rotors[i].yaw_scale;
}
}
for (unsigned i = 0; i < _rotor_count; i++)
outputs[i] = -1.0f + (outputs[i] * fixup_scale);
/* scale down all outputs if some outputs are too large, reduce total thrust */
float scale_out;
if (max_out > 1.0f) {
scale_out = 1.0f / max_out;
/* ensure outputs are out of the deadband */
for (unsigned i = 0; i < _rotor_count; i++)
if (outputs[i] < _deadband)
outputs[i] = _deadband;
} else {
scale_out = 1.0f;
}
/* scale outputs to range _idle_speed..1 */
for (unsigned i = 0; i < _rotor_count; i++) {
outputs[i] = _idle_speed + (outputs[i] * (1.0f - _idle_speed) * scale_out);
}
return _rotor_count;
}