diff --git a/flight/OpenPilot/Modules/Actuator/actuator.c b/flight/OpenPilot/Modules/Actuator/actuator.c
index f036db29c..23162b2a1 100644
--- a/flight/OpenPilot/Modules/Actuator/actuator.c
+++ b/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,23 +91,45 @@ 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();
-
+
// Main task loop
lastSysTime = xTaskGetTickCount();
while (1)
@@ -112,19 +141,63 @@ 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)
+ {
+ nMixers ++;
+ }
}
- else
+ if(nMixers < 2) //Nothing can fly with less than two mixers.
{
- AlarmsClear(SYSTEMALARMS_ALARM_ACTUATOR);
+ 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);
// Update in case read only (eg. during servo configuration)
@@ -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
diff --git a/flight/OpenPilot/UAVObjects/actuatorsettings.c b/flight/OpenPilot/UAVObjects/actuatorsettings.c
index 2a1665388..8e772d2fe 100644
--- a/flight/OpenPilot/UAVObjects/actuatorsettings.c
+++ b/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;
diff --git a/flight/OpenPilot/UAVObjects/inc/actuatorsettings.h b/flight/OpenPilot/UAVObjects/inc/actuatorsettings.h
index 48989afad..f643b4f7c 100644
--- a/flight/OpenPilot/UAVObjects/inc/actuatorsettings.h
+++ b/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
diff --git a/ground/src/plugins/uavobjects/actuatorsettings.cpp b/ground/src/plugins/uavobjects/actuatorsettings.cpp
index 2413471b0..362f87435 100644
--- a/ground/src/plugins/uavobjects/actuatorsettings.cpp
+++ b/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;
diff --git a/ground/src/plugins/uavobjects/actuatorsettings.h b/ground/src/plugins/uavobjects/actuatorsettings.h
index bda59765d..83d38298a 100644
--- a/ground/src/plugins/uavobjects/actuatorsettings.h
+++ b/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;
diff --git a/ground/src/plugins/uavobjects/actuatorsettings.py b/ground/src/plugins/uavobjects/actuatorsettings.py
index f97d17ef6..b51b162f2 100644
--- a/ground/src/plugins/uavobjects/actuatorsettings.py
+++ b/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
diff --git a/ground/src/shared/uavobjectdefinition/actuatorsettings.xml b/ground/src/shared/uavobjectdefinition/actuatorsettings.xml
index 1f01bf3ac..e09571586 100644
--- a/ground/src/shared/uavobjectdefinition/actuatorsettings.xml
+++ b/ground/src/shared/uavobjectdefinition/actuatorsettings.xml
@@ -31,7 +31,6 @@
-