actuator: generate an inverse mixer matrix

flight/Modules/Actuator/actuator.c

@@ -97,6 +97,7 @@ static MixerSettingsMixer1TypeOptions types_mixer[MAX_MIX_ACTUATORS];
  */
 
 static float motor_mixer[MAX_MIX_ACTUATORS * MIXERSETTINGS_MIXER1VECTOR_NUMELEM];
+static float motor_mixer_inv[MAX_MIX_ACTUATORS * MIXERSETTINGS_MIXER1VECTOR_NUMELEM];
 
 /* These are various settings objects used throughout the actuator code */
 static ActuatorSettingsData actuatorSettings;
@@ -410,6 +411,12 @@ static void compute_mixer()
 #endif
 }
 
+static bool compute_inverse_mixer()
+{
+	return matrix_pseudoinv(motor_mixer, motor_mixer_inv,
+		MAX_MIX_ACTUATORS, MIXERSETTINGS_MIXER1VECTOR_NUMELEM);
+}
+
 static void fill_desired_vector(
 		ActuatorDesiredData *desired,
 		float val1, float val2,
@@ -782,6 +789,14 @@ static void actuator_task(void* parameters)
 
 			compute_mixer();
 
+			/* If we can't calculate a proper inverse mixer,
+			 * set failsafe.
+			 */
+			if (compute_inverse_mixer()) {
+				set_failsafe();
+				continue;
+			}
+
 			MixerSettingsThrottleCurve2Get(curve2);
 			MixerSettingsCurve2SourceGet(&curve2_src);
 			settings_updated = false;