rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-12-01 09:24:10 +01:00
Code Issues Releases Activity

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

Browse Source 
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
This commit is contained in:
peabody124 2010-10-01 12:33:33 +00:00 committed by peabody124
parent 2dc40d11a1
commit 9f4e3906e3
7 changed files with 84 additions and 110 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

168
flight/OpenPilot/Modules/Actuator/actuator.c
View File

@ -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

1
flight/OpenPilot/UAVObjects/actuatorsettings.c
View File

@ -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;

4
flight/OpenPilot/UAVObjects/inc/actuatorsettings.h
View File

@ -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

4
ground/src/plugins/uavobjects/actuatorsettings.cpp
View File

@ -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;

4
ground/src/plugins/uavobjects/actuatorsettings.h
View File

@ -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;

12
ground/src/plugins/uavobjects/actuatorsettings.py
View File

@ -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

1
ground/src/shared/uavobjectdefinition/actuatorsettings.xml
View File

@ -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"/>