Flight: Made actuator a periodic task. I don't think the queue is low enough latency - should figure out how to make this happen as callback is the right way to go.

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

flight/OpenPilot/Modules/Actuator/actuator.c

@@ -86,11 +86,12 @@ int32_t ActuatorInitialize()
  */
 static void actuatorTask(void* parameters)
 {
-	UAVObjEvent ev;
+//	UAVObjEvent ev;
 	ActuatorSettingsData settings;
 	SystemSettingsData sysSettings;
 	ActuatorDesiredData desired;
 	ActuatorCommandData cmd;
+	portTickType lastSysTime;
 
 	// Set servo update frequency (done only on start-up)
 	ActuatorSettingsGet(&settings);
@@ -100,14 +101,15 @@ static void actuatorTask(void* parameters)
 	setFailsafe();
 
 	// Main task loop
+	lastSysTime = xTaskGetTickCount();
 	while (1)
 	{
 		// Wait until the ActuatorDesired object is updated, if a timeout then go to failsafe
-		if ( xQueueReceive(queue, &ev, FAILSAFE_TIMEOUT_MS / portTICK_RATE_MS) != pdTRUE )
+		/*if ( xQueueReceive(queue, &ev, FAILSAFE_TIMEOUT_MS / portTICK_RATE_MS) != pdTRUE )
 		{
 			setFailsafe();
 			continue;
-		}
+		}*/
 
 		// Read settings
 		ActuatorSettingsGet(&settings);
@@ -178,6 +180,10 @@ static void actuatorTask(void* parameters)
 		{
 			PIOS_Servo_Set( n, cmd.Channel[n] );
 		}
+		
+		// Wait until next update
+		vTaskDelayUntil(&lastSysTime, settings.UpdatePeriod / portTICK_RATE_MS );
+
 	}
 }
 

flight/OpenPilot/UAVObjects/actuatorsettings.c

@@ -118,6 +118,7 @@ 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 3352303420U
+#define ACTUATORSETTINGS_OBJID 562991684U
 #define ACTUATORSETTINGS_NAME "ActuatorSettings"
 #define ACTUATORSETTINGS_METANAME "ActuatorSettingsMeta"
 #define ACTUATORSETTINGS_ISSINGLEINST 1
@@ -101,6 +101,7 @@ 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];
@@ -187,6 +188,7 @@ 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,6 +346,9 @@ 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");
@@ -451,6 +454,7 @@ 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,6 +73,7 @@ public:
         quint8 CCPMServoZ;
         quint8 CCPMThrottle;
         quint8 CCPMTailRotor;
+        quint16 UpdatePeriod;
         qint16 ChannelUpdateFreq[2];
         qint16 ChannelMax[8];
         qint16 ChannelNeutral[8];
@@ -159,6 +160,7 @@ 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;
@@ -174,7 +176,7 @@ public:
 
   
     // Constants
-    static const quint32 OBJID = 3352303420U;
+    static const quint32 OBJID = 562991684U;
     static const QString NAME;
     static const bool ISSINGLEINST = 1;
     static const bool ISSETTINGS = 1;

ground/src/plugins/uavobjects/actuatorsettings.py

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

ground/src/shared/uavobjectdefinition/actuatorsettings.xml

@@ -31,6 +31,7 @@
         <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"/>