rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-30 15:52:12 +01:00
Code Issues Releases Activity

Actuator: Found a bug in actuator that affects how throttle and pitch curves

Browse Source 
were used.  Basically 80% was always 100%.  This should make hovering better
and fix heli swash pitch.
This commit is contained in:
James Cotton 2011-09-09 19:40:58 -05:00
parent 8f7712435f
commit 199db1362d
1 changed files with 15 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

31
flight/Modules/Actuator/actuator.c
View File

@ -74,7 +74,7 @@ static void actuatorTask(void* parameters);
static void actuator_update_rate(UAVObjEvent *); static void actuator_update_rate(UAVObjEvent *);
static int16_t scaleChannel(float value, int16_t max, int16_t min, int16_t neutral); static int16_t scaleChannel(float value, int16_t max, int16_t min, int16_t neutral);
static void setFailsafe(); static void setFailsafe();
static float MixerCurve(const float throttle, const float* curve); static float MixerCurve(const float throttle, const float* curve, uint8_t elements);
static bool set_channel(uint8_t mixer_channel, uint16_t value); static bool set_channel(uint8_t mixer_channel, uint16_t value);
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,
@ -219,22 +219,24 @@ static void actuatorTask(void* parameters)
bool positiveThrottle = desired.Throttle >= 0.00; bool positiveThrottle = desired.Throttle >= 0.00;
bool spinWhileArmed = MotorsSpinWhileArmed == ACTUATORSETTINGS_MOTORSSPINWHILEARMED_TRUE; bool spinWhileArmed = MotorsSpinWhileArmed == ACTUATORSETTINGS_MOTORSSPINWHILEARMED_TRUE;
float curve1 = MixerCurve(desired.Throttle,mixerSettings.ThrottleCurve1); float curve1 = MixerCurve(desired.Throttle,mixerSettings.ThrottleCurve1,MIXERSETTINGS_THROTTLECURVE1_NUMELEM);
//The source for the secondary curve is selectable //The source for the secondary curve is selectable
float curve2 = 0; float curve2 = 0;
AccessoryDesiredData accessory; AccessoryDesiredData accessory;
switch(mixerSettings.Curve2Source) { switch(mixerSettings.Curve2Source) {
case MIXERSETTINGS_CURVE2SOURCE_THROTTLE: case MIXERSETTINGS_CURVE2SOURCE_THROTTLE:
curve2 = MixerCurve(desired.Throttle,mixerSettings.ThrottleCurve2); curve2 = MixerCurve(desired.Throttle,mixerSettings.ThrottleCurve2,MIXERSETTINGS_THROTTLECURVE2_NUMELEM);
break; break;
case MIXERSETTINGS_CURVE2SOURCE_ROLL: case MIXERSETTINGS_CURVE2SOURCE_ROLL:
curve2 = MixerCurve(desired.Roll,mixerSettings.ThrottleCurve2); curve2 = MixerCurve(desired.Roll,mixerSettings.ThrottleCurve2,MIXERSETTINGS_THROTTLECURVE2_NUMELEM);
break; break;
case MIXERSETTINGS_CURVE2SOURCE_PITCH: case MIXERSETTINGS_CURVE2SOURCE_PITCH:
curve2 = MixerCurve(desired.Pitch,mixerSettings.ThrottleCurve2); curve2 = MixerCurve(desired.Pitch,mixerSettings.ThrottleCurve2,
MIXERSETTINGS_THROTTLECURVE2_NUMELEM);
break; break;
case MIXERSETTINGS_CURVE2SOURCE_YAW: case MIXERSETTINGS_CURVE2SOURCE_YAW:
curve2 = MixerCurve(desired.Yaw,mixerSettings.ThrottleCurve2); curve2 = MixerCurve(desired.Yaw,mixerSettings.ThrottleCurve2,MIXERSETTINGS_THROTTLECURVE2_NUMELEM);
break; break;
case MIXERSETTINGS_CURVE2SOURCE_ACCESSORY0: case MIXERSETTINGS_CURVE2SOURCE_ACCESSORY0:
case MIXERSETTINGS_CURVE2SOURCE_ACCESSORY1: case MIXERSETTINGS_CURVE2SOURCE_ACCESSORY1:
@ -243,7 +245,7 @@ static void actuatorTask(void* parameters)
case MIXERSETTINGS_CURVE2SOURCE_ACCESSORY4: case MIXERSETTINGS_CURVE2SOURCE_ACCESSORY4:
case MIXERSETTINGS_CURVE2SOURCE_ACCESSORY5: case MIXERSETTINGS_CURVE2SOURCE_ACCESSORY5:
if(AccessoryDesiredInstGet(mixerSettings.Curve2Source - MIXERSETTINGS_CURVE2SOURCE_ACCESSORY0,&accessory) == 0) if(AccessoryDesiredInstGet(mixerSettings.Curve2Source - MIXERSETTINGS_CURVE2SOURCE_ACCESSORY0,&accessory) == 0)
curve2 = MixerCurve(accessory.AccessoryVal,mixerSettings.ThrottleCurve2); curve2 = MixerCurve(accessory.AccessoryVal,mixerSettings.ThrottleCurve2,MIXERSETTINGS_THROTTLECURVE2_NUMELEM);
else else
curve2 = 0; curve2 = 0;
break; break;
@ -423,12 +425,9 @@ float ProcessMixer(const int index, const float curve1, const float curve2,
*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.
*/ */
static float MixerCurve(const float throttle, const float* curve, uint8_t elements)
#define MIXER_CURVE_ENTRIES 5
static float MixerCurve(const float throttle, const float* curve)
{ {
float scale = throttle * MIXER_CURVE_ENTRIES; float scale = throttle * (elements - 1);
int idx1 = scale; int idx1 = scale;
scale -= (float)idx1; //remainder scale -= (float)idx1; //remainder
if(curve[0] < -1) if(curve[0] < -1)
@ -441,12 +440,12 @@ static float MixerCurve(const float throttle, const float* curve)
scale = 0; scale = 0;
} }
int idx2 = idx1 + 1; int idx2 = idx1 + 1;
if(idx2 >= MIXER_CURVE_ENTRIES) if(idx2 >= elements)
{ {
idx2 = MIXER_CURVE_ENTRIES -1; //clamp to highest entry in table idx2 = elements -1; //clamp to highest entry in table
if(idx1 >= MIXER_CURVE_ENTRIES) if(idx1 >= elements)
{ {
idx1 = MIXER_CURVE_ENTRIES -1; idx1 = elements -1;
} }
} }
return((curve[idx1] * (1 - scale)) + (curve[idx2] * scale)); return((curve[idx1] * (1 - scale)) + (curve[idx2] * scale));