From b6441d29661be76d6790a4c492114065424174a3 Mon Sep 17 00:00:00 2001 From: tumbili Date: Sat, 11 Apr 2015 19:58:53 +0200 Subject: implemented new mixer strategy --- src/modules/systemlib/mixer/mixer_multirotor.cpp | 125 +++++++++++++++-------- 1 file changed, 84 insertions(+), 41 deletions(-) diff --git a/src/modules/systemlib/mixer/mixer_multirotor.cpp b/src/modules/systemlib/mixer/mixer_multirotor.cpp index e9a8a87ca..4d9d60c2d 100644 --- a/src/modules/systemlib/mixer/mixer_multirotor.cpp +++ b/src/modules/systemlib/mixer/mixer_multirotor.cpp @@ -200,18 +200,31 @@ MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handl unsigned MultirotorMixer::mix(float *outputs, unsigned space, uint16_t *status_reg) { + /* Summary of mixing strategy: + 1) mix roll, pitch and thrust without yaw. + 2) if some outputs violate range [0,1] then try to shift all outputs to minimize violation -> + increase or decrease total thrust (boost). The total increase or decrease of thrust is limited + (max_thrust_diff). If after the shift some outputs still violate the bounds then scale roll & pitch. + In case there is violation at the lower and upper bound then try to shift such that violation is equal + on both sides. + 3) mix in yaw and scale if it leads to limit violation. + 4) scale all outputs to range [idle_speed,1] + */ + 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 = 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 min_out = 0.0f; float max_out = 0.0f; + // clean register for saturation status flags if (status_reg != NULL) { (*status_reg) = 0; } + // thrust boost parameters + float thrust_increase_factor = 1.5f; + float thrust_decrease_factor = 0.6f; /* perform initial mix pass yielding unbounded outputs, ignore yaw */ for (unsigned i = 0; i < _rotor_count; i++) { @@ -221,14 +234,6 @@ MultirotorMixer::mix(float *outputs, unsigned space, uint16_t *status_reg) out *= _rotors[i].out_scale; - /* limit yaw if it causes outputs clipping */ - if (out >= 0.0f && out < -yaw * _rotors[i].yaw_scale) { - yaw = -out / _rotors[i].yaw_scale; - if(status_reg != NULL) { - (*status_reg) |= PX4IO_P_STATUS_MIXER_YAW_LIMIT; - } - } - /* calculate min and max output values */ if (out < min_out) { min_out = out; @@ -240,51 +245,89 @@ MultirotorMixer::mix(float *outputs, unsigned space, uint16_t *status_reg) outputs[i] = out; } - /* scale down roll/pitch controls if some outputs are negative, don't add yaw, keep total thrust */ - if (min_out < 0.0f) { - float scale_in = thrust / (thrust - min_out); - - max_out = 0.0f; - - /* mix again with adjusted controls */ - for (unsigned i = 0; i < _rotor_count; i++) { - float out = scale_in * (roll * _rotors[i].roll_scale + pitch * _rotors[i].pitch_scale) + thrust; - - /* update max output value */ - if (out > max_out) { - max_out = out; - } + float boost = 0.0f; // value added to demanded thrust (can also be negative) + float roll_pitch_scale = 1.0f; // scale for demanded roll and pitch - outputs[i] = out; + if(min_out < 0.0f && max_out < 1.0f && -min_out <= 1.0f - max_out) { + float max_thrust_diff = thrust * thrust_increase_factor - thrust; + if(max_thrust_diff >= -min_out) { + boost = -min_out; } + else { + boost = max_thrust_diff; + roll_pitch_scale = (thrust + boost)/(thrust - min_out); + } + } + else if (max_out > 1.0f && min_out > 0.0f && min_out >= max_out - 1.0f) { + float max_thrust_diff = thrust - thrust_decrease_factor*thrust; + if(max_thrust_diff >= max_out - 1.0f) { + boost = -(max_out - 1.0f); + } else { + boost = -max_thrust_diff; + roll_pitch_scale = (1 - (thrust + boost))/(max_out - thrust); + } + } + else if (min_out < 0.0f && max_out < 1.0f && -min_out > 1.0f - max_out) { + float max_thrust_diff = thrust * thrust_increase_factor - thrust; + boost = constrain(-min_out - (1.0f - max_out)/2.0f,0.0f, max_thrust_diff); + roll_pitch_scale = (thrust + boost)/(thrust - min_out); + } + else if (max_out > 1.0f && min_out > 0.0f && min_out < max_out - 1.0f ) { + float max_thrust_diff = thrust - thrust_decrease_factor*thrust; + boost = constrain(-(max_out - 1.0f - min_out)/2.0f, -max_thrust_diff, 0.0f); + roll_pitch_scale = (1 - (thrust + boost))/(max_out - thrust); + } + else if (min_out < 0.0f && max_out > 1.0f) { + boost = constrain(-(max_out - 1.0f + min_out)/2.0f, thrust_decrease_factor*thrust - thrust, thrust_increase_factor*thrust - thrust); + roll_pitch_scale = (thrust + boost)/(thrust - min_out); + } + // notify if saturation has occurred + if(min_out < 0.0f) { if(status_reg != NULL) { (*status_reg) |= PX4IO_P_STATUS_MIXER_LOWER_LIMIT; } - - } else { - /* roll/pitch mixed without lower side limiting, add yaw control */ - for (unsigned i = 0; i < _rotor_count; i++) { - outputs[i] += yaw * _rotors[i].yaw_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; - + if(max_out > 0.0f) { if(status_reg != NULL) { (*status_reg) |= PX4IO_P_STATUS_MIXER_UPPER_LIMIT; } + } - } else { - scale_out = 1.0f; + // mix again but now with thrust boost, scale roll/pitch and also add yaw + for(unsigned i = 0; i < _rotor_count; i++) { + float out = (roll * _rotors[i].roll_scale + + pitch * _rotors[i].pitch_scale) * roll_pitch_scale + + yaw * _rotors[i].yaw_scale + + thrust + boost; + + out *= _rotors[i].out_scale; + + // scale yaw if it violates limits. inform about yaw limit reached + if(out < 0.0f) { + yaw = -((roll * _rotors[i].roll_scale + pitch * _rotors[i].pitch_scale) * + roll_pitch_scale + thrust + boost)/_rotors[i].yaw_scale; + if(status_reg != NULL) { + (*status_reg) |= PX4IO_P_STATUS_MIXER_YAW_LIMIT; + } + } + else if(out > 1.0f) { + yaw = (1.0f - ((roll * _rotors[i].roll_scale + pitch * _rotors[i].pitch_scale) * + roll_pitch_scale + thrust + boost))/_rotors[i].yaw_scale; + if(status_reg != NULL) { + (*status_reg) |= PX4IO_P_STATUS_MIXER_YAW_LIMIT; + } + } } - /* scale outputs to range _idle_speed..1, and do final limiting */ + /* last mix, add yaw and scale outputs to range idle_speed...1 */ for (unsigned i = 0; i < _rotor_count; i++) { - outputs[i] = constrain(_idle_speed + (outputs[i] * (1.0f - _idle_speed) * scale_out), _idle_speed, 1.0f); + outputs[i] = (roll * _rotors[i].roll_scale + + pitch * _rotors[i].pitch_scale) * roll_pitch_scale + + yaw * _rotors[i].yaw_scale + + thrust + boost; + + outputs[i] = constrain(_idle_speed + (outputs[i] * (1.0f - _idle_speed)), _idle_speed, 1.0f); } return _rotor_count; -- cgit v1.2.3