1
0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-04-10 02:02:21 +02:00

Clean up the actuator module for floating point usage. Also there was a bug I

don't understand where the channels should be scaled AFTER all of them are
computed.  It feels like a memory overwriting bug and needs followup.
This commit is contained in:
James Cotton 2011-11-26 16:12:32 -06:00
parent 07e3ad10af
commit b11a4f7405
2 changed files with 32 additions and 27 deletions

View File

@ -216,7 +216,7 @@ static void actuatorTask(void* parameters)
AlarmsClear(SYSTEMALARMS_ALARM_ACTUATOR); AlarmsClear(SYSTEMALARMS_ALARM_ACTUATOR);
bool armed = flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED; bool armed = flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED;
bool positiveThrottle = desired.Throttle >= 0.00; bool positiveThrottle = desired.Throttle >= 0.00f;
bool spinWhileArmed = MotorsSpinWhileArmed == ACTUATORSETTINGS_MOTORSSPINWHILEARMED_TRUE; bool spinWhileArmed = MotorsSpinWhileArmed == ACTUATORSETTINGS_MOTORSSPINWHILEARMED_TRUE;
float curve1 = MixerCurve(desired.Throttle,mixerSettings.ThrottleCurve1,MIXERSETTINGS_THROTTLECURVE1_NUMELEM); float curve1 = MixerCurve(desired.Throttle,mixerSettings.ThrottleCurve1,MIXERSETTINGS_THROTTLECURVE1_NUMELEM);
@ -325,26 +325,29 @@ static void actuatorTask(void* parameters)
else else
status[ct] = -1; status[ct] = -1;
} }
command.Channel[ct] = scaleChannel(status[ct],
ChannelMax[ct],
ChannelMin[ct],
ChannelNeutral[ct]);
} }
#if defined(DIAGNOSTICS)
MixerStatusSet(&mixerStatus); for(int i = 0; i < MAX_MIX_ACTUATORS; i++)
#endif command.Channel[i] = scaleChannel(status[i],
ChannelMax[i],
ChannelMin[i],
ChannelNeutral[i]);
// Store update time // Store update time
command.UpdateTime = 1000*dT; command.UpdateTime = 1000.0f*dT;
if(1000*dT > command.MaxUpdateTime) if(1000.0f*dT > command.MaxUpdateTime)
command.MaxUpdateTime = 1000*dT; command.MaxUpdateTime = 1000.0f*dT;
// Update output object // Update output object
ActuatorCommandSet(&command); ActuatorCommandSet(&command);
// Update in case read only (eg. during servo configuration) // Update in case read only (eg. during servo configuration)
ActuatorCommandGet(&command); ActuatorCommandGet(&command);
#if defined(DIAGNOSTICS)
MixerStatusSet(&mixerStatus);
#endif
// Update servo outputs // Update servo outputs
bool success = true; bool success = true;
@ -367,22 +370,21 @@ static void actuatorTask(void* parameters)
/** /**
*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)
{ {
Mixer_t * mixers = (Mixer_t *)&mixerSettings->Mixer1Type; //pointer to array of mixers in UAVObjects Mixer_t * mixers = (Mixer_t *)&mixerSettings->Mixer1Type; //pointer to array of mixers in UAVObjects
Mixer_t * mixer = &mixers[index]; Mixer_t * mixer = &mixers[index];
float result = ((mixer->matrix[MIXERSETTINGS_MIXER1VECTOR_THROTTLECURVE1] / 128.0f) * curve1) + float result = (((float)mixer->matrix[MIXERSETTINGS_MIXER1VECTOR_THROTTLECURVE1] / 128.0f) * curve1) +
((mixer->matrix[MIXERSETTINGS_MIXER1VECTOR_THROTTLECURVE2] / 128.0f) * curve2) + (((float)mixer->matrix[MIXERSETTINGS_MIXER1VECTOR_THROTTLECURVE2] / 128.0f) * curve2) +
((mixer->matrix[MIXERSETTINGS_MIXER1VECTOR_ROLL] / 128.0f) * desired->Roll) + (((float)mixer->matrix[MIXERSETTINGS_MIXER1VECTOR_ROLL] / 128.0f) * desired->Roll) +
((mixer->matrix[MIXERSETTINGS_MIXER1VECTOR_PITCH] / 128.0f) * desired->Pitch) + (((float)mixer->matrix[MIXERSETTINGS_MIXER1VECTOR_PITCH] / 128.0f) * desired->Pitch) +
((mixer->matrix[MIXERSETTINGS_MIXER1VECTOR_YAW] / 128.0f) * desired->Yaw); (((float)mixer->matrix[MIXERSETTINGS_MIXER1VECTOR_YAW] / 128.0f) * desired->Yaw);
if(mixer->type == MIXERSETTINGS_MIXER1TYPE_MOTOR) if(mixer->type == MIXERSETTINGS_MIXER1TYPE_MOTOR)
{ {
if(result < 0) //idle throttle if(result < 0.0f) //idle throttle
{ {
result = 0; result = 0.0f;
} }
//feed forward //feed forward
@ -392,7 +394,7 @@ float ProcessMixer(const int index, const float curve1, const float curve2,
result += accumulator; result += accumulator;
if(period !=0) if(period !=0)
{ {
if(accumulator > 0) if(accumulator > 0.0f)
{ {
float filter = mixerSettings->AccelTime / period; float filter = mixerSettings->AccelTime / period;
if(filter <1) if(filter <1)
@ -422,6 +424,7 @@ float ProcessMixer(const int index, const float curve1, const float curve2,
} }
lastFilteredResult[index] = result; lastFilteredResult[index] = result;
} }
return(result); return(result);
} }
@ -432,7 +435,7 @@ float ProcessMixer(const int index, const float curve1, const float curve2,
*/ */
static float MixerCurve(const float throttle, const float* curve, uint8_t elements) static float MixerCurve(const float throttle, const float* curve, uint8_t elements)
{ {
float scale = throttle * (elements - 1); float scale = throttle * (float) (elements - 1);
int idx1 = scale; int idx1 = scale;
scale -= (float)idx1; //remainder scale -= (float)idx1; //remainder
if(curve[0] < -1) if(curve[0] < -1)
@ -453,7 +456,7 @@ static float MixerCurve(const float throttle, const float* curve, uint8_t elemen
idx1 = elements -1; idx1 = elements -1;
} }
} }
return((curve[idx1] * (1 - scale)) + (curve[idx2] * scale)); return curve[idx1] * (1.0f - scale) + curve[idx2] * scale;
} }
@ -464,7 +467,7 @@ static int16_t scaleChannel(float value, int16_t max, int16_t min, int16_t neutr
{ {
int16_t valueScaled; int16_t valueScaled;
// Scale // Scale
if ( value >= 0.0) if ( value >= 0.0f)
{ {
valueScaled = (int16_t)(value*((float)(max-neutral))) + neutral; valueScaled = (int16_t)(value*((float)(max-neutral))) + neutral;
} }
@ -520,6 +523,8 @@ static void setFailsafe()
{ {
Channel[n] = 0; Channel[n] = 0;
} }
} }
// Set alarm // Set alarm

View File

@ -49,7 +49,7 @@ endif
FLASH_TOOL = OPENOCD FLASH_TOOL = OPENOCD
# List of modules to include # List of modules to include
MODULES = Actuator Telemetry ManualControl Stabilization Attitude/revolution MODULES = Telemetry Attitude/revolution ManualControl Stabilization Actuator
PYMODULES = PYMODULES =
#FlightPlan #FlightPlan