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

Merged in alessiomorale/librepilot/amorale/LP-276_measurement_based_d_term (pull request #218)

Browse Source 
[LP-276] Support D Term calculation on measurement only
This commit is contained in:
Lalanne Laurent 2016-04-14 11:44:39 +02:00
commit e6ada50587
4 changed files with 21 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -76,7 +76,7 @@ float pid_apply(struct pid *pid, const float err, float dT)
* This version of apply uses setpoint weighting for the derivative component so the gain
* on the gyro derivative can be different than the gain on the setpoint derivative
*/
float pid_apply_setpoint(struct pid *pid, const pid_scaler *scaler, const float setpoint, const float measured, float dT)
float pid_apply_setpoint(struct pid *pid, const pid_scaler *scaler, const float setpoint, const float measured, float dT, bool meas_based_d_term)
{
float err = setpoint - measured;
@ -85,9 +85,18 @@ float pid_apply_setpoint(struct pid *pid, const pid_scaler *scaler, const float
pid->iAccumulator = boundf(pid->iAccumulator, pid->iLim * -1000.0f, pid->iLim * 1000.0f);
// Calculate DT1 term,
float diff;
float derr = (-measured);
if (!meas_based_d_term) {
derr += deriv_gamma * setpoint;
}
diff = derr - pid->lastErr;
pid->lastErr = derr;
float dterm = 0;
float diff = ((deriv_gamma * setpoint - measured) - pid->lastErr);
pid->lastErr = (deriv_gamma * setpoint - measured);
if (pid->d > 0.0f && dT > 0.0f) {
// low pass filter derivative term. below formula is the same as
// dterm = (1-alpha)*pid->lastDer + alpha * (...)/dT
@ -95,7 +104,6 @@ float pid_apply_setpoint(struct pid *pid, const pid_scaler *scaler, const float
dterm = pid->lastDer + dT / (dT + deriv_tau) * ((scaler->d * diff * pid->d / dT) - pid->lastDer);
pid->lastDer = dterm;
}
return (err * scaler->p * pid->p) + pid->iAccumulator / 1000.0f + dterm;
}

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

@ -70,7 +70,7 @@ typedef struct pid_scaler_s {
// ! 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 pid_scaler *scaler, const float setpoint, const float measured, float dT);
float pid_apply_setpoint(struct pid *pid, const pid_scaler *scaler, const float setpoint, const float measured, float dT, bool meas_based_d_term);
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);

10
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 float speedScaleFactor = 1.0f;
static bool frame_is_multirotor;
static bool measuredDterm_enabled;
// Private functions
static void stabilizationInnerloopTask();
static void GyroStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev);
@ -98,7 +98,8 @@ void stabilizationInnerloopInit()
// schedule dead calls every FAILSAFE_TIMEOUT_MS to have the watchdog cleared
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()
@ -250,6 +251,7 @@ static void stabilizationInnerloopTask()
StabilizationStatusOuterLoopGet(&outerLoop);
bool allowPiroComp = true;
for (t = 0; t < AXES; t++) {
bool reinit = (StabilizationStatusInnerLoopToArray(enabled)[t] != previous_mode[t]);
previous_mode[t] = StabilizationStatusInnerLoopToArray(enabled)[t];
@ -290,7 +292,7 @@ static void stabilizationInnerloopTask()
StabilizationBankMaximumRateToArray(stabSettings.stabBank.MaximumRate)[t]
);
pid_scaler scaler = create_pid_scaler(t);
actuatorDesiredAxis[t] = pid_apply_setpoint(&stabSettings.innerPids[t], &scaler, rate[t], gyro_filtered[t], dT);
actuatorDesiredAxis[t] = pid_apply_setpoint(&stabSettings.innerPids[t], &scaler, rate[t], gyro_filtered[t], dT, measuredDterm_enabled);
break;
case STABILIZATIONSTATUS_INNERLOOP_ACRO:
{
@ -305,7 +307,7 @@ static void stabilizationInnerloopTask()
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
float arate = pid_apply_setpoint(&stabSettings.innerPids[t], &ascaler, rate[t], gyro_filtered[t], dT);
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];
actuatorDesiredAxis[t] = factor * stickinput[t] + (1.0f - factor) * arate;
}

2
shared/uavobjectdefinition/stabilizationsettings.xml
View File

@ -48,6 +48,8 @@
<field name="ScaleToAirspeedLimits" units="" type="float" elementnames="Min,Max" defaultvalue="0.05,3"/>
<field name="FlightModeAssistMap" units="" type="enum" options="None,GPSAssist" elements="6" defaultvalue="None,None,None,None,None,None" />
<field name="MeasureBasedDTerm" units="" type="enum" elements="1" options="False,True" defaultvalue="True"/>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="true" updatemode="onchange" period="0"/>
<telemetryflight acked="true" updatemode="onchange" period="0"/>