Merge remote-tracking branch 'mlyle/mpl-actmodel' into glowtape-delaycomp

Author: glowtape

flight/Modules/Actuator/actuator.c

@@ -96,6 +96,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;
@@ -372,6 +373,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,
@@ -738,20 +745,31 @@ static void actuator_task(void* parameters)
 			actuator_settings_update();
 		}
 
+		PIOS_WDG_UpdateFlag(PIOS_WDG_ACTUATOR);
+
+		UAVObjEvent ev;
+
 		if (mixer_settings_updated) {
 			mixer_settings_updated = false;
 			SystemSettingsAirframeTypeGet(&airframe_type);
 
 			compute_mixer();
+<<<<<<< HEAD
+=======
+
+			/* If we can't calculate a proper inverse mixer,
+			 * set failsafe.
+			 */
+			if (compute_inverse_mixer()) {
+				set_failsafe();
+				continue;
+			}
+>>>>>>> mlyle/mpl-actmodel
 
 			MixerSettingsThrottleCurve2Get(curve2);
 			MixerSettingsCurve2SourceGet(&curve2_src);
 		}
 
-		PIOS_WDG_UpdateFlag(PIOS_WDG_ACTUATOR);
-
-		UAVObjEvent ev;
-
 		// Wait until the ActuatorDesired object is updated
 		if (!PIOS_Queue_Receive(queue, &ev, FAILSAFE_TIMEOUT_MS)) {
 			// If we hit a timeout, set the actuator failsafe and

flight/targets/sprf3e/fw/Makefile

@@ -50,11 +50,11 @@ OPTMODULES += VtolPathFollower
 OPTMODULES += FixedWingPathFollower
 #OPTMODULES += UAVOMavlinkBridge
 OPTMODULES += UAVOMSPBridge
-OPTMODULES += UAVOLighttelemetryBridge
+#OPTMODULES += UAVOLighttelemetryBridge
 OPTMODULES += Battery
 OPTMODULES += ComUsbBridge
 OPTMODULES += Airspeed
-OPTMODULES += UAVOHoTTBridge
+#OPTMODULES += UAVOHoTTBridge
 OPTMODULES += UAVOFrSKYSensorHubBridge
 OPTMODULES += UAVOFrSKYSPortBridge
 OPTMODULES += Logging