rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-22 12:54:14 +01:00
Code Issues Releases Activity

Flight/Les/Actuator: Space->Tab, somehow slipped passed previous patch

Browse Source 
git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@1784 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
peabody124 2010-09-27 17:30:42 +00:00 committed by peabody124
parent 21278e7a0e
commit 9b1d258a94
1 changed files with 151 additions and 151 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

250
flight/OpenPilot/Modules/Actuator/matrixmixer/Actuator/actuator.c
View File

@ -62,8 +62,8 @@ static int16_t scaleChannel(float value, int16_t max, int16_t min, int16_t neutr
static void setFailsafe(); static void setFailsafe();
static float MixerCurve(const float throttle, const float* curve); static float MixerCurve(const float throttle, const float* curve);
float ProcessMixer(const int index, const float curve1, const float curve2, float ProcessMixer(const int index, const float curve1, const float curve2,
MixerSettingsData* mixerSettings, ActuatorDesiredData* desired, MixerSettingsData* mixerSettings, ActuatorDesiredData* desired,
const float period); const float period);
/** /**
@ -89,9 +89,9 @@ int32_t ActuatorInitialize()
*/ */
static void actuatorTask(void* parameters) static void actuatorTask(void* parameters)
{ {
// UAVObjEvent ev; // UAVObjEvent ev;
portTickType lastSysTime; portTickType lastSysTime;
ActuatorCommandData command; ActuatorCommandData command;
ActuatorSettingsData settings; ActuatorSettingsData settings;
// Set servo update frequency (done only on start-up) // Set servo update frequency (done only on start-up)
@ -107,15 +107,15 @@ static void actuatorTask(void* parameters)
{ {
ActuatorCommandGet(&command); ActuatorCommandGet(&command);
ActuatorSettingsGet(&settings); ActuatorSettingsGet(&settings);
if ( RunMixers(&command, &settings) == -1 ) if ( RunMixers(&command, &settings) == -1 )
{ {
AlarmsSet(SYSTEMALARMS_ALARM_ACTUATOR, SYSTEMALARMS_ALARM_CRITICAL); AlarmsSet(SYSTEMALARMS_ALARM_ACTUATOR, SYSTEMALARMS_ALARM_CRITICAL);
} }
else else
{ {
AlarmsClear(SYSTEMALARMS_ALARM_ACTUATOR); AlarmsClear(SYSTEMALARMS_ALARM_ACTUATOR);
} }
// Update output object // Update output object
ActuatorCommandSet(&command); ActuatorCommandSet(&command);
@ -147,8 +147,8 @@ static void actuatorTask(void* parameters)
//this structure is equivalent to the UAVObjects for one mixer. //this structure is equivalent to the UAVObjects for one mixer.
typedef struct { typedef struct {
uint8_t type; uint8_t type;
float matrix[5]; float matrix[5];
} __attribute__((packed)) Mixer_t; } __attribute__((packed)) Mixer_t;
@ -156,31 +156,31 @@ typedef struct {
static int32_t RunMixers(ActuatorCommandData* command, ActuatorSettingsData* settings) static int32_t RunMixers(ActuatorCommandData* command, ActuatorSettingsData* settings)
{ {
SystemSettingsData sysSettings; SystemSettingsData sysSettings;
MixerSettingsData mixerSettings; MixerSettingsData mixerSettings;
ActuatorDesiredData desired; ActuatorDesiredData desired;
MixerStatusData mixerStatus; MixerStatusData mixerStatus;
SystemSettingsGet(&sysSettings); SystemSettingsGet(&sysSettings);
MixerStatusGet(&mixerStatus); MixerStatusGet(&mixerStatus);
MixerSettingsGet (&mixerSettings); MixerSettingsGet (&mixerSettings);
ActuatorDesiredGet(&desired); ActuatorDesiredGet(&desired);
float * status = (float *)&mixerStatus; //access status objects as an array of floats float * status = (float *)&mixerStatus; //access status objects as an array of floats
int nMixers = 0; int nMixers = 0;
Mixer_t * mixers = (Mixer_t *)&mixerSettings.Mixer0Type; Mixer_t * mixers = (Mixer_t *)&mixerSettings.Mixer0Type;
for(int ct=0; ct < MAX_MIX_ACTUATORS; ct++) for(int ct=0; ct < MAX_MIX_ACTUATORS; ct++)
{ {
if(mixers[ct].type != MIXERSETTINGS_MIXER0TYPE_DISABLED) if(mixers[ct].type != MIXERSETTINGS_MIXER0TYPE_DISABLED)
{ {
nMixers ++; nMixers ++;
} }
} }
if(nMixers < 2) //Nothing can fly with less than two mixers. if(nMixers < 2) //Nothing can fly with less than two mixers.
{ {
return(-1); return(-1);
} }
ManualControlCommandData manualControl; ManualControlCommandData manualControl;
@ -190,25 +190,25 @@ static int32_t RunMixers(ActuatorCommandData* command, ActuatorSettingsData* set
armed &= desired.Throttle > 0.05; //zero throttle stops the motors armed &= desired.Throttle > 0.05; //zero throttle stops the motors
float curve1 = MixerCurve(desired.Throttle,mixerSettings.ThrottleCurve1); float curve1 = MixerCurve(desired.Throttle,mixerSettings.ThrottleCurve1);
float curve2 = MixerCurve(desired.Throttle,mixerSettings.ThrottleCurve2); float curve2 = MixerCurve(desired.Throttle,mixerSettings.ThrottleCurve2);
for(int ct=0; ct < MAX_MIX_ACTUATORS; ct++) for(int ct=0; ct < MAX_MIX_ACTUATORS; ct++)
{ {
if(mixers[ct].type != MIXERSETTINGS_MIXER0TYPE_DISABLED) if(mixers[ct].type != MIXERSETTINGS_MIXER0TYPE_DISABLED)
{ {
status[ct] = scaleChannel(ProcessMixer(ct, curve1, curve2, &mixerSettings, &desired, settings->UpdatePeriod), status[ct] = scaleChannel(ProcessMixer(ct, curve1, curve2, &mixerSettings, &desired, settings->UpdatePeriod),
settings->ChannelMax[ct], settings->ChannelMax[ct],
settings->ChannelMin[ct], settings->ChannelMin[ct],
settings->ChannelNeutral[ct]); settings->ChannelNeutral[ct]);
if(!armed && if(!armed &&
mixers[ct].type == MIXERSETTINGS_MIXER0TYPE_MOTOR) mixers[ct].type == MIXERSETTINGS_MIXER0TYPE_MOTOR)
{ {
command->Channel[ct] = settings->ChannelMin[ct]; //force zero throttle command->Channel[ct] = settings->ChannelMin[ct]; //force zero throttle
}else }else
{ {
command->Channel[ct] = status[ct]; //servos always follow command command->Channel[ct] = status[ct]; //servos always follow command
} }
} }
} }
MixerStatusSet(&mixerStatus); MixerStatusSet(&mixerStatus);
return(0); return(0);
@ -216,97 +216,97 @@ static int32_t RunMixers(ActuatorCommandData* command, ActuatorSettingsData* set
/** /**
*Process mixing for one actuator *Process mixing for one actuator
*/ */
float ProcessMixer(const int index, const float curve1, const float curve2, float ProcessMixer(const int index, const float curve1, const float curve2,
MixerSettingsData* mixerSettings, ActuatorDesiredData* desired, const float period) MixerSettingsData* mixerSettings, ActuatorDesiredData* desired, const float period)
{ {
static float lastResult[MAX_MIX_ACTUATORS]={0,0,0,0,0,0,0,0}; static float lastResult[MAX_MIX_ACTUATORS]={0,0,0,0,0,0,0,0};
static float lastFilteredResult[MAX_MIX_ACTUATORS]={0,0,0,0,0,0,0,0}; static float lastFilteredResult[MAX_MIX_ACTUATORS]={0,0,0,0,0,0,0,0};
static float filterAccumulator[MAX_MIX_ACTUATORS]={0,0,0,0,0,0,0,0}; static float filterAccumulator[MAX_MIX_ACTUATORS]={0,0,0,0,0,0,0,0};
Mixer_t * mixers = (Mixer_t *)&mixerSettings->Mixer0Type; //pointer to array of mixers in UAVObjects Mixer_t * mixers = (Mixer_t *)&mixerSettings->Mixer0Type; //pointer to array of mixers in UAVObjects
Mixer_t * mixer = &mixers[index]; Mixer_t * mixer = &mixers[index];
float result = (mixer->matrix[MIXERSETTINGS_MIXER0VECTOR_THROTTLECURVE1] * curve1) + float result = (mixer->matrix[MIXERSETTINGS_MIXER0VECTOR_THROTTLECURVE1] * curve1) +
(mixer->matrix[MIXERSETTINGS_MIXER0VECTOR_THROTTLECURVE2] * curve2) + (mixer->matrix[MIXERSETTINGS_MIXER0VECTOR_THROTTLECURVE2] * curve2) +
(mixer->matrix[MIXERSETTINGS_MIXER0VECTOR_ROLL] * desired->Roll) + (mixer->matrix[MIXERSETTINGS_MIXER0VECTOR_ROLL] * desired->Roll) +
(mixer->matrix[MIXERSETTINGS_MIXER0VECTOR_PITCH] * desired->Pitch) + (mixer->matrix[MIXERSETTINGS_MIXER0VECTOR_PITCH] * desired->Pitch) +
(mixer->matrix[MIXERSETTINGS_MIXER0VECTOR_YAW] * desired->Yaw); (mixer->matrix[MIXERSETTINGS_MIXER0VECTOR_YAW] * desired->Yaw);
if(mixer->type == MIXERSETTINGS_MIXER0TYPE_MOTOR) if(mixer->type == MIXERSETTINGS_MIXER0TYPE_MOTOR)
{ {
if(result < 0) //idle throttle if(result < 0) //idle throttle
{ {
result = 0; result = 0;
} }
//feed forward //feed forward
float accumulator = filterAccumulator[index]; float accumulator = filterAccumulator[index];
accumulator += (result - lastResult[index]) * mixerSettings->FeedForward; accumulator += (result - lastResult[index]) * mixerSettings->FeedForward;
lastResult[index] = result; lastResult[index] = result;
result += accumulator; result += accumulator;
if(accumulator > 0) if(accumulator > 0)
{ {
float filter = mixerSettings->AccelTime / period; float filter = mixerSettings->AccelTime / period;
if(filter <1) if(filter <1)
{ {
filter = 1; filter = 1;
} }
accumulator -= accumulator / filter; accumulator -= accumulator / filter;
}else }else
{ {
float filter = mixerSettings->DecelTime / period; float filter = mixerSettings->DecelTime / period;
if(filter <1) if(filter <1)
{ {
filter = 1; filter = 1;
} }
accumulator -= accumulator / filter; accumulator -= accumulator / filter;
} }
filterAccumulator[index] = accumulator; filterAccumulator[index] = accumulator;
result += accumulator; result += accumulator;
//acceleration limit //acceleration limit
float dt = result - lastFilteredResult[index]; float dt = result - lastFilteredResult[index];
float maxDt = mixerSettings->MaxAccel * (period / 1000); float maxDt = mixerSettings->MaxAccel * (period / 1000);
if(dt > maxDt) //we are accelerating too hard if(dt > maxDt) //we are accelerating too hard
{ {
result = lastFilteredResult[index] + maxDt; result = lastFilteredResult[index] + maxDt;
} }
lastFilteredResult[index] = result; lastFilteredResult[index] = result;
} }
return(result); return(result);
} }
/** /**
*Interpolate a throttle curve. Throttle input should be in the range 0 to 1. *Interpolate a throttle curve. Throttle input should be in the range 0 to 1.
*Output is in the range 0 to 1. *Output is in the range 0 to 1.
*/ */
#define MIXER_CURVE_ENTRIES 5 #define MIXER_CURVE_ENTRIES 5
static float MixerCurve(const float throttle, const float* curve) static float MixerCurve(const float throttle, const float* curve)
{ {
float scale = throttle * MIXER_CURVE_ENTRIES; float scale = throttle * MIXER_CURVE_ENTRIES;
int idx1 = scale; int idx1 = scale;
scale -= (float)idx1; //remainder scale -= (float)idx1; //remainder
if(curve[0] < -1) if(curve[0] < -1)
{ {
return(throttle); return(throttle);
} }
if (idx1 < 0) if (idx1 < 0)
{ {
idx1 = 0; //clamp to lowest entry in table idx1 = 0; //clamp to lowest entry in table
scale = 0; scale = 0;
} }
int idx2 = idx1 + 1; int idx2 = idx1 + 1;
if(idx2 >= MIXER_CURVE_ENTRIES) if(idx2 >= MIXER_CURVE_ENTRIES)
{ {
idx2 = MIXER_CURVE_ENTRIES -1; //clamp to highest entry in table idx2 = MIXER_CURVE_ENTRIES -1; //clamp to highest entry in table
if(idx1 >= MIXER_CURVE_ENTRIES) if(idx1 >= MIXER_CURVE_ENTRIES)
{ {
idx1 = MIXER_CURVE_ENTRIES -1; idx1 = MIXER_CURVE_ENTRIES -1;
} }
} }
return((curve[idx1] * (1 - scale)) + (curve[idx2] * scale)); return((curve[idx1] * (1 - scale)) + (curve[idx2] * scale));
} }
@ -345,10 +345,10 @@ static int16_t scaleChannel(float value, int16_t max, int16_t min, int16_t neutr
*/ */
static void setFailsafe() static void setFailsafe()
{ {
ActuatorCommandData command; ActuatorCommandData command;
ActuatorSettingsData settings; ActuatorSettingsData settings;
ActuatorCommandGet(&command); ActuatorCommandGet(&command);
ActuatorSettingsGet(&settings); ActuatorSettingsGet(&settings);
// Reset ActuatorCommand to neutral values // Reset ActuatorCommand to neutral values
for (int n = 0; n < ACTUATORCOMMAND_CHANNEL_NUMELEM; ++n) for (int n = 0; n < ACTUATORCOMMAND_CHANNEL_NUMELEM; ++n)
@ -372,5 +372,5 @@ static void setFailsafe()
/** /**
* @} * @}
* @} * @}
*/ */