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

OP-1474 Include I in the scaling and let the TPS factor piggyback on the speedScaleFactor

Browse Source
This commit is contained in:
Stefan Karlsson 2014-09-10 22:02:21 +02:00
parent eacf0a1693
commit 662b29ac05
3 changed files with 5 additions and 34 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
flight/libraries/math/pid.c
View File

@ -147,25 +147,3 @@ float pid_scale_factor(pid_scaler *scaler)
return 1.0f + (IS_REAL(y) ? y : 0.0f);
}
float pid_apply_setpoint_scaled(struct pid *pid, const float factor, const float setpoint, const float measured, float dT,
pid_scaler *scaler)
{
// Save PD values.
float p = pid->p;
float d = pid->d;
// Scale PD values.
float scale = pid_scale_factor(scaler);
pid->p *= scale;
pid->d *= scale;
float value = pid_apply_setpoint(pid, factor, setpoint, measured, dT);
// Restore PD values.
pid->p = p;
pid->d = d;
return value;
}

2
flight/libraries/math/pid.h
View File

@ -52,8 +52,6 @@ typedef struct pid_scaler {
// ! Methods to use the pid structures
float pid_apply(struct pid *pid, const float err, float dT);
float pid_apply_setpoint(struct pid *pid, const float factor, const float setpoint, const float measured, float dT);
float pid_apply_setpoint_scaled(struct pid *pid, const float factor, const float setpoint, const float measured, float dT,
pid_scaler *scaler);
void pid_zero(struct pid *pid);
void pid_configure(struct pid *pid, float p, float i, float d, float iLim);
void pid_configure_derivative(float cutoff, float gamma);

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

@ -123,12 +123,8 @@ static float get_pid_scale_source_value()
return value;
}
static inline pid_scaler create_pid_scaler()
static float get_pid_scale_factor()
{
float throttle;
ManualControlCommandThrottleGet(&throttle);
struct pid_scaler scaler = {
.x = get_pid_scale_source_value(),
.points = {
@ -140,7 +136,7 @@ static inline pid_scaler create_pid_scaler()
}
};
return scaler;
return pid_scale_factor(&scaler);
}
static int is_pid_scaled_for_axis(int axis)
@ -256,12 +252,11 @@ static void stabilizationInnerloopTask()
-StabilizationBankMaximumRateToArray(stabSettings.stabBank.MaximumRate)[t],
StabilizationBankMaximumRateToArray(stabSettings.stabBank.MaximumRate)[t]
);
float scaleFactor = speedScaleFactor;
if (is_pid_scaled_for_axis(t)) {
pid_scaler scaler = create_pid_scaler();
actuatorDesiredAxis[t] = pid_apply_setpoint_scaled(&stabSettings.innerPids[t], speedScaleFactor, rate[t], gyro_filtered[t], dT, &scaler);
} else {
actuatorDesiredAxis[t] = pid_apply_setpoint(&stabSettings.innerPids[t], speedScaleFactor, rate[t], gyro_filtered[t], dT);
scaleFactor *= get_pid_scale_factor();
}
actuatorDesiredAxis[t] = pid_apply_setpoint(&stabSettings.innerPids[t], scaleFactor, rate[t], gyro_filtered[t], dT);
break;
case STABILIZATIONSTATUS_INNERLOOP_DIRECT:
default: