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

[LP-276] some perf improvements (mainly for f1 targets)

Browse Source 
(authored by Lalanne Laurent <f5soh@free.fr>)
This commit is contained in:
Alessio Morale 2016-04-08 22:18:01 +02:00
parent a513b2c675
commit f83c9e2c73
1 changed files with 6 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
flight/modules/Stabilization/innerloop.c
View File

@ -68,7 +68,7 @@ static uint8_t previous_mode[AXES] = { 255, 255, 255, 255 };
static PiOSDeltatimeConfig timeval; static PiOSDeltatimeConfig timeval;
static float speedScaleFactor = 1.0f; static float speedScaleFactor = 1.0f;
static bool frame_is_multirotor; static bool frame_is_multirotor;
static bool measuredDterm_enabled;
// Private functions // Private functions
static void stabilizationInnerloopTask(); static void stabilizationInnerloopTask();
static void GyroStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev); static void GyroStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev);
@ -99,6 +99,7 @@ void stabilizationInnerloopInit()
PIOS_CALLBACKSCHEDULER_Schedule(callbackHandle, FAILSAFE_TIMEOUT_MS, CALLBACK_UPDATEMODE_LATER); PIOS_CALLBACKSCHEDULER_Schedule(callbackHandle, FAILSAFE_TIMEOUT_MS, CALLBACK_UPDATEMODE_LATER);
frame_is_multirotor = (GetCurrentFrameType() == FRAME_TYPE_MULTIROTOR); frame_is_multirotor = (GetCurrentFrameType() == FRAME_TYPE_MULTIROTOR);
measuredDterm_enabled = (stabSettings.settings.MeasureBasedDTerm == STABILIZATIONSETTINGS_MEASUREBASEDDTERM_TRUE);
} }
static float get_pid_scale_source_value() static float get_pid_scale_source_value()
@ -250,6 +251,7 @@ static void stabilizationInnerloopTask()
StabilizationStatusOuterLoopGet(&outerLoop); StabilizationStatusOuterLoopGet(&outerLoop);
bool allowPiroComp = true; bool allowPiroComp = true;
for (t = 0; t < AXES; t++) { for (t = 0; t < AXES; t++) {
bool reinit = (StabilizationStatusInnerLoopToArray(enabled)[t] != previous_mode[t]); bool reinit = (StabilizationStatusInnerLoopToArray(enabled)[t] != previous_mode[t]);
previous_mode[t] = StabilizationStatusInnerLoopToArray(enabled)[t]; previous_mode[t] = StabilizationStatusInnerLoopToArray(enabled)[t];
@ -290,7 +292,7 @@ static void stabilizationInnerloopTask()
StabilizationBankMaximumRateToArray(stabSettings.stabBank.MaximumRate)[t] StabilizationBankMaximumRateToArray(stabSettings.stabBank.MaximumRate)[t]
); );
pid_scaler scaler = create_pid_scaler(t); pid_scaler scaler = create_pid_scaler(t);
actuatorDesiredAxis[t] = pid_apply_setpoint(&stabSettings.innerPids[t], &scaler, rate[t], gyro_filtered[t], dT, stabSettings.settings.MeasureBasedDTerm == STABILIZATIONSETTINGS_MEASUREBASEDDTERM_TRUE); actuatorDesiredAxis[t] = pid_apply_setpoint(&stabSettings.innerPids[t], &scaler, rate[t], gyro_filtered[t], dT, measuredDterm_enabled);
break; break;
case STABILIZATIONSTATUS_INNERLOOP_ACRO: case STABILIZATIONSTATUS_INNERLOOP_ACRO:
{ {
@ -305,7 +307,7 @@ static void stabilizationInnerloopTask()
pid_scaler ascaler = create_pid_scaler(t); pid_scaler ascaler = create_pid_scaler(t);
ascaler.i *= boundf(1.0f - (1.5f * fabsf(stickinput[t])), 0.0f, 1.0f); // this prevents Integral from getting too high while controlled manually ascaler.i *= boundf(1.0f - (1.5f * fabsf(stickinput[t])), 0.0f, 1.0f); // this prevents Integral from getting too high while controlled manually
float arate = pid_apply_setpoint(&stabSettings.innerPids[t], &ascaler, rate[t], gyro_filtered[t], dT, stabSettings.settings.MeasureBasedDTerm == STABILIZATIONSETTINGS_MEASUREBASEDDTERM_TRUE); float arate = pid_apply_setpoint(&stabSettings.innerPids[t], &ascaler, rate[t], gyro_filtered[t], dT, measuredDterm_enabled);
float factor = fabsf(stickinput[t]) * stabSettings.acroInsanityFactors[t]; float factor = fabsf(stickinput[t]) * stabSettings.acroInsanityFactors[t];
actuatorDesiredAxis[t] = factor * stickinput[t] + (1.0f - factor) * arate; actuatorDesiredAxis[t] = factor * stickinput[t] + (1.0f - factor) * arate;
} }