Flight/Actuator: Made it use the detected dT for the feedforward computation

and removed the unused field from actuator settings (old setting time and all
the aircraft specific stuff).

git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@1820 ebee16cc-31ac-478f-84a7-5cbb03baadba

flight/OpenPilot/Modules/Actuator/actuator.c

@@ -47,6 +47,7 @@
 #define STACK_SIZE configMINIMAL_STACK_SIZE
 #define TASK_PRIORITY (tskIDLE_PRIORITY+4)
 #define FAILSAFE_TIMEOUT_MS 100
+#define MAX_MIX_ACTUATORS ACTUATORCOMMAND_CHANNEL_NUMELEM
 
 // Private types
 
@@ -57,7 +58,6 @@ static xTaskHandle taskHandle;
 
 // Private functions
 static void actuatorTask(void* parameters);
-static int32_t RunMixers(ActuatorCommandData * command, ActuatorSettingsData* settings);
 static int16_t scaleChannel(float value, int16_t max, int16_t min, int16_t neutral);
 static void setFailsafe();
 static float MixerCurve(const float throttle, const float* curve);
@@ -66,6 +66,13 @@ float ProcessMixer(const int index, const float curve1, const float curve2,
 		   const float period);
 
 
+//this structure is equivalent to the UAVObjects for one mixer.
+typedef struct {
+	uint8_t type;
+	float matrix[5];
+} __attribute__((packed)) Mixer_t;
+
+
 /**
  * @brief Module initialization
  * @return 0
@@ -84,20 +91,42 @@ int32_t ActuatorInitialize()
 	return 0;
 }
 
+
+
 /**
- * @brief Main module task
+ * @brieft Main Actuator module task
+ *
+ * Universal matrix based mixer for VTOL, helis and fixed wing.
+ * Converts desired roll,pitch,yaw and throttle to servo/ESC outputs.
+ *
+ * Because of how the Throttle ranges from 0 to 1, the motors should too!
+ *
+ * Note this code depends on the UAVObjects for the mixers being all being the same
+ * and in sequence. If you change the object definition, make sure you check the code!
+ *
+ * @return -1 if error, 0 if success
  */
 static void actuatorTask(void* parameters)
 {
 	UAVObjEvent ev;
 	portTickType lastSysTime;
+	portTickType thisSysTime;
+	float dT;
 	ActuatorCommandData command;
 	ActuatorSettingsData settings;
 	
-	// Set servo update frequency (done only on start-up)
+	SystemSettingsData sysSettings;
+	MixerSettingsData mixerSettings;
+	ActuatorDesiredData desired;
+	MixerStatusData mixerStatus;
+	ManualControlCommandData manualControl;
+	
 	ActuatorSettingsGet(&settings);
+	// Set servo update frequency (done only on start-up)
 	PIOS_Servo_SetHz(settings.ChannelUpdateFreq[0], settings.ChannelUpdateFreq[1]);
 		
+	float * status = (float *)&mixerStatus; //access status objects as an array of floats		
+	
 	// Go to the neutral (failsafe) values until an ActuatorDesired update is received
 	setFailsafe();
 
@@ -112,18 +141,62 @@ static void actuatorTask(void* parameters)
                         continue;
                 }
 		
+		// Check how long since last update
+		thisSysTime = xTaskGetTickCount();
+		if(thisSysTime > lastSysTime) // reuse dt in case of wraparound
+			dT = (thisSysTime - lastSysTime) / portTICK_RATE_MS / 1000.0f;		
+		lastSysTime = thisSysTime;
 		
 		
+		ManualControlCommandGet(&manualControl);
+		SystemSettingsGet(&sysSettings);
+		MixerStatusGet(&mixerStatus);
+		MixerSettingsGet (&mixerSettings);
+		ActuatorDesiredGet(&desired);
 		ActuatorCommandGet(&command);
 		ActuatorSettingsGet(&settings);
-		if ( RunMixers(&command, &settings) == -1 )
+		
+		int nMixers = 0;
+		Mixer_t * mixers = (Mixer_t *)&mixerSettings.Mixer0Type;
+		for(int ct=0; ct < MAX_MIX_ACTUATORS; ct++)
 		{
-			AlarmsSet(SYSTEMALARMS_ALARM_ACTUATOR, SYSTEMALARMS_ALARM_CRITICAL);
+			if(mixers[ct].type != MIXERSETTINGS_MIXER0TYPE_DISABLED)
-		}
-		else
 			{
-			AlarmsClear(SYSTEMALARMS_ALARM_ACTUATOR);
+				nMixers ++;
 			}
+		}
+		if(nMixers < 2) //Nothing can fly with less than two mixers.
+		{
+			AlarmsSet(SYSTEMALARMS_ALARM_ACTUATOR, SYSTEMALARMS_ALARM_WARNING);
+			continue;			
+		}
+		
+		
+		bool armed = manualControl.Armed == MANUALCONTROLCOMMAND_ARMED_TRUE;
+		armed &= desired.Throttle > 0.05; //zero throttle stops the motors
+				
+		float curve1 = MixerCurve(desired.Throttle,mixerSettings.ThrottleCurve1);
+		float curve2 = MixerCurve(desired.Throttle,mixerSettings.ThrottleCurve2);
+		for(int ct=0; ct < MAX_MIX_ACTUATORS; ct++)
+		{
+			if(mixers[ct].type != MIXERSETTINGS_MIXER0TYPE_DISABLED)
+			{
+				status[ct] = ProcessMixer(ct, curve1, curve2, &mixerSettings, &desired, dT);
+
+				if(!armed &&
+				   mixers[ct].type == MIXERSETTINGS_MIXER0TYPE_MOTOR)
+				{
+					command.Channel[ct] = settings.ChannelMin[ct]; //force zero throttle
+				}else
+				{
+					command.Channel[ct] = scaleChannel(status[ct],
+									   settings.ChannelMax[ct],
+									   settings.ChannelMin[ct],
+									   settings.ChannelNeutral[ct]);
+				}
+			}
+		}
+		MixerStatusSet(&mixerStatus);		
 		
 		// Update output object
 		ActuatorCommandSet(&command);
@@ -138,86 +211,7 @@ static void actuatorTask(void* parameters)
 	}
 }
 
-/**
- * Universal matrix based mixer for VTOL, helis and fixed wing.
- * Converts desired roll,pitch,yaw and throttle to servo/ESC outputs.
- *
- * Because of how the Throttle ranges from 0 to 1, the motors should too!
- *
- * Note this code depends on the UAVObjects for the mixers being all being the same
- * and in sequence. If you change the object definition, make sure you check the code!
- *
- * @return -1 if error, 0 if success
- */
 
-//this structure is equivalent to the UAVObjects for one mixer.
-typedef struct {
-	uint8_t type;
-	float matrix[5];
-} __attribute__((packed)) Mixer_t;
-
-
-#define MAX_MIX_ACTUATORS ACTUATORCOMMAND_CHANNEL_NUMELEM
-
-static int32_t RunMixers(ActuatorCommandData* command, ActuatorSettingsData* settings)
-{
-	SystemSettingsData sysSettings;
-	MixerSettingsData mixerSettings;
-	ActuatorDesiredData desired;
-	MixerStatusData mixerStatus;
-	
-	SystemSettingsGet(&sysSettings);
-	MixerStatusGet(&mixerStatus);
-	MixerSettingsGet (&mixerSettings);
-	ActuatorDesiredGet(&desired);
-	
-	float * status = (float *)&mixerStatus; //access status objects as an array of floats
-	
-	
-	int nMixers = 0;
-	Mixer_t * mixers = (Mixer_t *)&mixerSettings.Mixer0Type;
-	for(int ct=0; ct < MAX_MIX_ACTUATORS; ct++)
-	{
-		if(mixers[ct].type != MIXERSETTINGS_MIXER0TYPE_DISABLED)
-		{
-			nMixers ++;
-		}
-	}
-	if(nMixers < 2) //Nothing can fly with less than two mixers.
-	{
-		return(-1);
-	}
-	
-	ManualControlCommandData manualControl;
-	ManualControlCommandGet(&manualControl);
-	
-	bool armed = manualControl.Armed == MANUALCONTROLCOMMAND_ARMED_TRUE;
-	armed &= desired.Throttle > 0.05; //zero throttle stops the motors
-	
-	
-	float curve1 = MixerCurve(desired.Throttle,mixerSettings.ThrottleCurve1);
-	float curve2 = MixerCurve(desired.Throttle,mixerSettings.ThrottleCurve2);
-	for(int ct=0; ct < MAX_MIX_ACTUATORS; ct++)
-	{
-		if(mixers[ct].type != MIXERSETTINGS_MIXER0TYPE_DISABLED)
-		{
-			status[ct] = scaleChannel(ProcessMixer(ct, curve1, curve2, &mixerSettings, &desired, settings->UpdatePeriod),
-						  settings->ChannelMax[ct],
-						  settings->ChannelMin[ct],
-						  settings->ChannelNeutral[ct]);
-			if(!armed &&
-			   mixers[ct].type == MIXERSETTINGS_MIXER0TYPE_MOTOR)
-			{
-				command->Channel[ct] = settings->ChannelMin[ct]; //force zero throttle
-			}else
-			{
-				command->Channel[ct] = status[ct]; //servos always follow command
-			}
-		}
-	}
-	MixerStatusSet(&mixerStatus);
-	return(0);
-}
 
 /**
  *Process mixing for one actuator

flight/OpenPilot/UAVObjects/actuatorsettings.c

@@ -118,7 +118,6 @@ static void setDefaults(UAVObjHandle obj, uint16_t instId)
 	data.CCPMServoZ = 8;
 	data.CCPMThrottle = 8;
 	data.CCPMTailRotor = 8;
-	data.UpdatePeriod = 5;
 	data.ChannelUpdateFreq[0] = 50;
 	data.ChannelUpdateFreq[1] = 50;
 	data.ChannelMax[0] = 2000;

flight/OpenPilot/UAVObjects/inc/actuatorsettings.h

@@ -41,7 +41,7 @@
 #define ACTUATORSETTINGS_H
 
 // Object constants
-#define ACTUATORSETTINGS_OBJID 562991684U
+#define ACTUATORSETTINGS_OBJID 3352303420U
 #define ACTUATORSETTINGS_NAME "ActuatorSettings"
 #define ACTUATORSETTINGS_METANAME "ActuatorSettingsMeta"
 #define ACTUATORSETTINGS_ISSINGLEINST 1
@@ -101,7 +101,6 @@ typedef struct {
     uint8_t CCPMServoZ;
     uint8_t CCPMThrottle;
     uint8_t CCPMTailRotor;
-    uint16_t UpdatePeriod;
     int16_t ChannelUpdateFreq[2];
     int16_t ChannelMax[8];
     int16_t ChannelNeutral[8];
@@ -188,7 +187,6 @@ typedef enum { ACTUATORSETTINGS_CCPMTHROTTLE_CHANNEL0=0, ACTUATORSETTINGS_CCPMTH
 // Field CCPMTailRotor information
 /* Enumeration options for field CCPMTailRotor */
 typedef enum { ACTUATORSETTINGS_CCPMTAILROTOR_CHANNEL0=0, ACTUATORSETTINGS_CCPMTAILROTOR_CHANNEL1=1, ACTUATORSETTINGS_CCPMTAILROTOR_CHANNEL2=2, ACTUATORSETTINGS_CCPMTAILROTOR_CHANNEL3=3, ACTUATORSETTINGS_CCPMTAILROTOR_CHANNEL4=4, ACTUATORSETTINGS_CCPMTAILROTOR_CHANNEL5=5, ACTUATORSETTINGS_CCPMTAILROTOR_CHANNEL6=6, ACTUATORSETTINGS_CCPMTAILROTOR_CHANNEL7=7, ACTUATORSETTINGS_CCPMTAILROTOR_NONE=8 } ActuatorSettingsCCPMTailRotorOptions;
-// Field UpdatePeriod information
 // Field ChannelUpdateFreq information
 /* Number of elements for field ChannelUpdateFreq */
 #define ACTUATORSETTINGS_CHANNELUPDATEFREQ_NUMELEM 2

ground/src/plugins/uavobjects/actuatorsettings.cpp

@@ -346,9 +346,6 @@ ActuatorSettings::ActuatorSettings(): UAVDataObject(OBJID, ISSINGLEINST, ISSETTI
     CCPMTailRotorEnumOptions.append("Channel7");
     CCPMTailRotorEnumOptions.append("None");
     fields.append( new UAVObjectField(QString("CCPMTailRotor"), QString("channel"), UAVObjectField::ENUM, CCPMTailRotorElemNames, CCPMTailRotorEnumOptions) );
-    QStringList UpdatePeriodElemNames;
-    UpdatePeriodElemNames.append("0");
-    fields.append( new UAVObjectField(QString("UpdatePeriod"), QString("ms"), UAVObjectField::UINT16, UpdatePeriodElemNames, QStringList()) );
     QStringList ChannelUpdateFreqElemNames;
     ChannelUpdateFreqElemNames.append("0");
     ChannelUpdateFreqElemNames.append("1");
@@ -454,7 +451,6 @@ void ActuatorSettings::setDefaultFieldValues()
     data.CCPMServoZ = 8;
     data.CCPMThrottle = 8;
     data.CCPMTailRotor = 8;
-    data.UpdatePeriod = 5;
     data.ChannelUpdateFreq[0] = 50;
     data.ChannelUpdateFreq[1] = 50;
     data.ChannelMax[0] = 2000;

ground/src/plugins/uavobjects/actuatorsettings.h

@@ -73,7 +73,6 @@ public:
         quint8 CCPMServoZ;
         quint8 CCPMThrottle;
         quint8 CCPMTailRotor;
-        quint16 UpdatePeriod;
         qint16 ChannelUpdateFreq[2];
         qint16 ChannelMax[8];
         qint16 ChannelNeutral[8];
@@ -160,7 +159,6 @@ public:
     // Field CCPMTailRotor information
     /* Enumeration options for field CCPMTailRotor */
     typedef enum { CCPMTAILROTOR_CHANNEL0=0, CCPMTAILROTOR_CHANNEL1=1, CCPMTAILROTOR_CHANNEL2=2, CCPMTAILROTOR_CHANNEL3=3, CCPMTAILROTOR_CHANNEL4=4, CCPMTAILROTOR_CHANNEL5=5, CCPMTAILROTOR_CHANNEL6=6, CCPMTAILROTOR_CHANNEL7=7, CCPMTAILROTOR_NONE=8 } CCPMTailRotorOptions;
-    // Field UpdatePeriod information
     // Field ChannelUpdateFreq information
     /* Number of elements for field ChannelUpdateFreq */
     static const quint32 CHANNELUPDATEFREQ_NUMELEM = 2;
@@ -176,7 +174,7 @@ public:
 
   
     // Constants
-    static const quint32 OBJID = 562991684U;
+    static const quint32 OBJID = 3352303420U;
     static const QString NAME;
     static const bool ISSINGLEINST = 1;
     static const bool ISSETTINGS = 1;

ground/src/plugins/uavobjects/actuatorsettings.py

@@ -529,16 +529,6 @@ _fields = [ \
 			'8' : 'None',
 		}
 	),
-	uavobject.UAVObjectField(
-		'UpdatePeriod',
-		'H',
-		1,
-		[
-			'0',
-		],
-		{
-		}
-	),
 	uavobject.UAVObjectField(
 		'ChannelUpdateFreq',
 		'h',
@@ -606,7 +596,7 @@ _fields = [ \
 
 class ActuatorSettings(uavobject.UAVObject):
     ## Object constants
-    OBJID        = 562991684
+    OBJID        = 3352303420
     NAME         = "ActuatorSettings"
     METANAME     = "ActuatorSettingsMeta"
     ISSINGLEINST = 1

ground/src/shared/uavobjectdefinition/actuatorsettings.xml

@@ -31,7 +31,6 @@
         <field name="CCPMServoZ" units="channel" type="enum" elements="1" options="Channel0,Channel1,Channel2,Channel3,Channel4,Channel5,Channel6,Channel7,None" defaultvalue="None"/>
         <field name="CCPMThrottle" units="channel" type="enum" elements="1" options="Channel0,Channel1,Channel2,Channel3,Channel4,Channel5,Channel6,Channel7,None" defaultvalue="None"/>
         <field name="CCPMTailRotor" units="channel" type="enum" elements="1" options="Channel0,Channel1,Channel2,Channel3,Channel4,Channel5,Channel6,Channel7,None" defaultvalue="None"/>
-        <field name="UpdatePeriod" units="ms" type="uint16" elements="1" defaultvalue="5"/>
         <field name="ChannelUpdateFreq" units="Hz" type="int16" elements="2" defaultvalue="50"/>
         <field name="ChannelMax" units="us" type="int16" elements="8" defaultvalue="2000"/>
         <field name="ChannelNeutral" units="us" type="int16" elements="8" defaultvalue="1500"/>