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); 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 // ! Methods to use the pid structures
float pid_apply(struct pid *pid, const float err, float dT); 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(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_zero(struct pid *pid);
void pid_configure(struct pid *pid, float p, float i, float d, float iLim); void pid_configure(struct pid *pid, float p, float i, float d, float iLim);
void pid_configure_derivative(float cutoff, float gamma); 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; return value;
} }
static inline pid_scaler create_pid_scaler() static float get_pid_scale_factor()
{ {
float throttle;
ManualControlCommandThrottleGet(&throttle);
struct pid_scaler scaler = { struct pid_scaler scaler = {
.x = get_pid_scale_source_value(), .x = get_pid_scale_source_value(),
.points = { .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) 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],
StabilizationBankMaximumRateToArray(stabSettings.stabBank.MaximumRate)[t] StabilizationBankMaximumRateToArray(stabSettings.stabBank.MaximumRate)[t]
); );
float scaleFactor = speedScaleFactor;
if (is_pid_scaled_for_axis(t)) { if (is_pid_scaled_for_axis(t)) {
pid_scaler scaler = create_pid_scaler(); scaleFactor *= get_pid_scale_factor();
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);
} }
actuatorDesiredAxis[t] = pid_apply_setpoint(&stabSettings.innerPids[t], scaleFactor, rate[t], gyro_filtered[t], dT);
break; break;
case STABILIZATIONSTATUS_INNERLOOP_DIRECT: case STABILIZATIONSTATUS_INNERLOOP_DIRECT:
default: default: