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:
parent
07e3ad10af
commit
b11a4f7405
@ -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
|
||||||
|
@ -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
|
||||||
|
|
||||||
|
Loading…
x
Reference in New Issue
Block a user