refactor mixer: rename delta_outputs to desaturation_vector

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;
 
 	/**

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];
 	}
 }
 

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):