From 9f4e3906e37ba796d0724a93f7cb456de2e4c0c3 Mon Sep 17 00:00:00 2001 From: peabody124 Date: Fri, 1 Oct 2010 12:33:33 +0000 Subject: [PATCH] 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 | 168 +++++++++--------- .../OpenPilot/UAVObjects/actuatorsettings.c | 1 - .../UAVObjects/inc/actuatorsettings.h | 4 +- .../plugins/uavobjects/actuatorsettings.cpp | 4 - .../src/plugins/uavobjects/actuatorsettings.h | 4 +- .../plugins/uavobjects/actuatorsettings.py | 12 +- .../uavobjectdefinition/actuatorsettings.xml | 1 - 7 files changed, 84 insertions(+), 110 deletions(-) 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 @@ -