diff --git a/src/lib/mixer/mixer.h b/src/lib/mixer/mixer.h index c9220af5ac..8a8107f25b 100644 --- a/src/lib/mixer/mixer.h +++ b/src/lib/mixer/mixer.h @@ -648,7 +648,7 @@ public: * @param control_cb Callback invoked to read inputs. * @param cb_handle Passed to control_cb. * @param rotors control allocation matrix - * @param rotor_count length of rotors + * @param rotor_count length of rotors array (= number of motors) */ MultirotorMixer(ControlCallback control_cb, uintptr_t cb_handle, @@ -730,34 +730,34 @@ public: private: /** - * Computes the gain k by which delta_outputs has to be multiplied + * Computes the gain k by which desaturation_vector has to be multiplied * in order to unsaturate the output that has the greatest saturation. * @see also minimize_saturation(). * * @return desaturation gain */ - float compute_desaturation_gain(const float *delta_outputs, const float *outputs, saturation_status &sat_status, + float compute_desaturation_gain(const float *desaturation_vector, const float *outputs, saturation_status &sat_status, float min_output, float max_output) const; /** - * Minimize the saturation of the actuators by adding or substracting a fraction of delta_outputs. - * delta_outputs is the vector that added to the output outputs, modifies the thrust or angular + * Minimize the saturation of the actuators by adding or substracting a fraction of desaturation_vector. + * desaturation_vector is the vector that added to the output outputs, modifies the thrust or angular * acceleration on a specific axis. - * For example, if delta_outputs is given to slide along the vertical thrust axis (thrust_scale), the + * For example, if desaturation_vector is given to slide along the vertical thrust axis (thrust_scale), the * saturation will be minimized by shifting the vertical thrust setpoint, without changing the * roll/pitch/yaw accelerations. * * Note that as we only slide along the given axis, in extreme cases outputs can still contain values * outside of [min_output, max_output]. * - * @param delta_outputs vector that is added to the outputs, e.g. thrust_scale + * @param desaturation_vector vector that is added to the outputs, e.g. thrust_scale * @param outputs output vector that is modified * @param sat_status saturation status output * @param min_output minimum desired value in outputs * @param max_output maximum desired value in outputs - * @param reduce_only if true, only allow to reduce (substract) a fraction of delta_outputs + * @param reduce_only if true, only allow to reduce (substract) a fraction of desaturation_vector */ - void minimize_saturation(const float *delta_outputs, float *outputs, saturation_status &sat_status, + void minimize_saturation(const float *desaturation_vector, float *outputs, saturation_status &sat_status, float min_output = 0.f, float max_output = 1.f, bool reduce_only = false) const; /** diff --git a/src/lib/mixer/mixer_multirotor.cpp b/src/lib/mixer/mixer_multirotor.cpp index eaf6a6b5ee..d7bdd3d0b5 100644 --- a/src/lib/mixer/mixer_multirotor.cpp +++ b/src/lib/mixer/mixer_multirotor.cpp @@ -188,20 +188,20 @@ MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handl s[3] / 10000.0f); } -float MultirotorMixer::compute_desaturation_gain(const float *delta_outputs, const float *outputs, +float MultirotorMixer::compute_desaturation_gain(const float *desaturation_vector, const float *outputs, saturation_status &sat_status, float min_output, float max_output) const { float k_min = 0.f; float k_max = 0.f; for (unsigned i = 0; i < _rotor_count; i++) { - // Avoid division by zero. If delta_outputs[i] is zero, there's nothing we can do to unsaturate anyway - if (fabsf(delta_outputs[i]) < FLT_EPSILON) { + // Avoid division by zero. If desaturation_vector[i] is zero, there's nothing we can do to unsaturate anyway + if (fabsf(desaturation_vector[i]) < FLT_EPSILON) { continue; } if (outputs[i] < min_output) { - float k = (min_output - outputs[i]) / delta_outputs[i]; + float k = (min_output - outputs[i]) / desaturation_vector[i]; if (k < k_min) { k_min = k; } @@ -211,7 +211,7 @@ float MultirotorMixer::compute_desaturation_gain(const float *delta_outputs, con } if (outputs[i] > max_output) { - float k = (max_output - outputs[i]) / delta_outputs[i]; + float k = (max_output - outputs[i]) / desaturation_vector[i]; if (k < k_min) { k_min = k; } @@ -225,26 +225,27 @@ float MultirotorMixer::compute_desaturation_gain(const float *delta_outputs, con return k_min + k_max; } -void MultirotorMixer::minimize_saturation(const float *delta_outputs, float *outputs, saturation_status &sat_status, +void MultirotorMixer::minimize_saturation(const float *desaturation_vector, float *outputs, + saturation_status &sat_status, float min_output, float max_output, bool reduce_only) const { - float k1 = compute_desaturation_gain(delta_outputs, outputs, sat_status, min_output, max_output); + float k1 = compute_desaturation_gain(desaturation_vector, outputs, sat_status, min_output, max_output); if (reduce_only && k1 > 0.f) { return; } for (unsigned i = 0; i < _rotor_count; i++) { - outputs[i] += k1 * delta_outputs[i]; + outputs[i] += k1 * desaturation_vector[i]; } // Compute the desaturation gain again based on the updated outputs. // In most cases it will be zero. It won't be if max(outputs) - min(outputs) > max_output - min_output. // In that case adding 0.5 of the gain will equilibrate saturations. - float k2 = 0.5f * compute_desaturation_gain(delta_outputs, outputs, sat_status, min_output, max_output); + float k2 = 0.5f * compute_desaturation_gain(desaturation_vector, outputs, sat_status, min_output, max_output); for (unsigned i = 0; i < _rotor_count; i++) { - outputs[i] += k2 * delta_outputs[i]; + outputs[i] += k2 * desaturation_vector[i]; } } diff --git a/src/lib/mixer/mixer_multirotor.py b/src/lib/mixer/mixer_multirotor.py index bfa0af0271..5420c82b3d 100755 --- a/src/lib/mixer/mixer_multirotor.py +++ b/src/lib/mixer/mixer_multirotor.py @@ -22,23 +22,23 @@ import subprocess # mixing algorithms # -------------------------------------------------- -def compute_desaturation_gain(u, u_min, u_max, delta_u): +def compute_desaturation_gain(u, u_min, u_max, desaturation_vector): """ - Computes the gain k by which delta_u has to be multiplied + Computes the gain k by which desaturation_vector has to be multiplied in order to unsaturate the output that has the greatest saturation """ d_u_sat_plus = u_max - u d_u_sat_minus = u_min - u k = np.zeros(u.size*2) for i in range(u.size): - if abs(delta_u[i]) < 0.000001: + if abs(desaturation_vector[i]) < 0.000001: # avoid division by zero continue if d_u_sat_minus[i] > 0.0: - k[2*i] = d_u_sat_minus[i] / delta_u[i] + k[2*i] = d_u_sat_minus[i] / desaturation_vector[i] if d_u_sat_plus[i] < 0.0: - k[2*i+1] = d_u_sat_plus[i] / delta_u[i] + k[2*i+1] = d_u_sat_plus[i] / desaturation_vector[i] k_min = min(k) k_max = max(k) @@ -48,31 +48,31 @@ def compute_desaturation_gain(u, u_min, u_max, delta_u): return k -def minimize_sat(u, u_min, u_max, delta_u): +def minimize_sat(u, u_min, u_max, desaturation_vector): """ Minimize the saturation of the actuators by - adding or substracting a fraction of delta_u. - Delta_u is the vector that added to the output u, + adding or substracting a fraction of desaturation_vector. + desaturation_vector is the vector that added to the output u, modifies the thrust or angular acceleration on a specific axis. - For example, if delta_u is given + For example, if desaturation_vector is given to slide along the vertical thrust axis, the saturation will be minimized by shifting the vertical thrust setpoint, without changing the roll/pitch/yaw accelerations. """ - k_1 = compute_desaturation_gain(u, u_min, u_max, delta_u) - u_1 = u + k_1 * delta_u # Try to unsaturate + k_1 = compute_desaturation_gain(u, u_min, u_max, desaturation_vector) + u_1 = u + k_1 * desaturation_vector # Try to unsaturate # Compute the desaturation gain again based on the updated outputs. # In most cases it will be zero. It won't be if max(outputs) - min(outputs) # > max_output - min_output. # In that case adding 0.5 of the gain will equilibrate saturations. - k_2 = compute_desaturation_gain(u_1, u_min, u_max, delta_u) + k_2 = compute_desaturation_gain(u_1, u_min, u_max, desaturation_vector) k_opt = k_1 + 0.5 * k_2 - u_prime = u + k_opt * delta_u + u_prime = u + k_opt * desaturation_vector return u_prime def mix_yaw(m_sp, u, P, u_min, u_max):