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

Merge branch 'corvuscorax/OP-1036_fixed-wing-improvements' into next

Browse Source
This commit is contained in:
Corvus Corax 2013-08-11 15:13:23 +02:00
commit 0f31368c64
4 changed files with 41 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -69,6 +69,7 @@ float pid_apply(struct pid *pid, const float err, float dT)
/**
* Update the PID computation with setpoint weighting on the derivative
* @param[in] pid The PID struture which stores temporary information
* @param[in] factor A dynamic factor to scale pid's by, to compensate nonlinearities
* @param[in] setpoint The setpoint to use
* @param[in] measured The measured value of output
* @param[in] dT The time step
@ -77,12 +78,12 @@ 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 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 err = setpoint - measured;
// Scale up accumulator by 1000 while computing to avoid losing precision
pid->iAccumulator += err * (pid->i * dT * 1000.0f);
pid->iAccumulator += err * (factor * pid->i * dT * 1000.0f);
pid->iAccumulator = bound(pid->iAccumulator, pid->iLim * 1000.0f);
// Calculate DT1 term,
@ -90,11 +91,11 @@ float pid_apply_setpoint(struct pid *pid, const float setpoint, const float meas
float diff = ((deriv_gamma * setpoint - measured) - pid->lastErr);
pid->lastErr = (deriv_gamma * setpoint - measured);
if (pid->d > 0.0f && dT > 0.0f) {
dterm = pid->lastDer + dT / (dT + deriv_tau) * ((diff * pid->d / dT) - pid->lastDer);
dterm = pid->lastDer + dT / (dT + deriv_tau) * ((factor * diff * pid->d / dT) - pid->lastDer);
pid->lastDer = dterm; // ^ set constant to 1/(2*pi*f_cutoff)
} // 7.9577e-3 means 20 Hz f_cutoff
return (err * pid->p) + pid->iAccumulator / 1000.0f + dterm;
return (err * factor * pid->p) + pid->iAccumulator / 1000.0f + dterm;
}
/**

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

@ -44,7 +44,7 @@ struct pid {
// ! 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 setpoint, const float measured, float dT);
float pid_apply_setpoint(struct pid *pid, const float factor, const float setpoint, const float measured, float dT);
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);

36
flight/modules/Stabilization/stabilization.c
View File

@ -41,6 +41,7 @@
#include "relaytuningsettings.h"
#include "stabilizationdesired.h"
#include "attitudestate.h"
#include "airspeedstate.h"
#include "gyrostate.h"
#include "flightstatus.h"
#include "manualcontrol.h" // Just to get a macro
@ -129,6 +130,9 @@ int32_t StabilizationInitialize()
ActuatorDesiredInitialize();
#ifdef DIAG_RATEDESIRED
RateDesiredInitialize();
#endif
#ifdef REVOLUTION
AirspeedStateInitialize();
#endif
// Code required for relay tuning
sin_lookup_initalize();
@ -156,6 +160,10 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
GyroStateData gyroStateData;
FlightStatusData flightStatus;
#ifdef REVOLUTION
AirspeedStateData airspeedState;
#endif
SettingsUpdatedCb((UAVObjEvent *)NULL);
// Main task loop
@ -183,6 +191,26 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
#ifdef DIAG_RATEDESIRED
RateDesiredGet(&rateDesired);
#endif
#ifdef REVOLUTION
float speedScaleFactor;
// Scale PID coefficients based on current airspeed estimation - needed for fixed wing planes
AirspeedStateGet(&airspeedState);
if (settings.ScaleToAirspeed < 0.1f || airspeedState.CalibratedAirspeed < 0.1f) {
// feature has been turned off
speedScaleFactor = 1.0f;
} else {
// scale the factor to be 1.0 at the specified airspeed (for example 10m/s) but scaled by 1/speed^2
speedScaleFactor = (settings.ScaleToAirspeed * settings.ScaleToAirspeed) / (airspeedState.CalibratedAirspeed * airspeedState.CalibratedAirspeed);
if (speedScaleFactor < settings.ScaleToAirspeedLimits[STABILIZATIONSETTINGS_SCALETOAIRSPEEDLIMITS_MIN]) {
speedScaleFactor = settings.ScaleToAirspeedLimits[STABILIZATIONSETTINGS_SCALETOAIRSPEEDLIMITS_MIN];
}
if (speedScaleFactor > settings.ScaleToAirspeedLimits[STABILIZATIONSETTINGS_SCALETOAIRSPEEDLIMITS_MAX]) {
speedScaleFactor = settings.ScaleToAirspeedLimits[STABILIZATIONSETTINGS_SCALETOAIRSPEEDLIMITS_MAX];
}
}
#else
const float speedScaleFactor = 1.0f;
#endif
#if defined(PIOS_QUATERNION_STABILIZATION)
// Quaternion calculation of error in each axis. Uses more memory.
@ -262,7 +290,7 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
rateDesiredAxis[i] = bound(attitudeDesiredAxis[i], settings.ManualRate[i]);
// Compute the inner loop
actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], rateDesiredAxis[i], gyro_filtered[i], dT);
actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], speedScaleFactor, rateDesiredAxis[i], gyro_filtered[i], dT);
actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i], 1.0f);
break;
@ -278,7 +306,7 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
rateDesiredAxis[i] = bound(rateDesiredAxis[i], settings.MaximumRate[i]);
// Compute the inner loop
actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], rateDesiredAxis[i], gyro_filtered[i], dT);
actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], speedScaleFactor, rateDesiredAxis[i], gyro_filtered[i], dT);
actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i], 1.0f);
break;
@ -304,7 +332,7 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
// Compute desired rate as input biased towards leveling
rateDesiredAxis[i] = attitudeDesiredAxis[i] + weak_leveling;
actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], rateDesiredAxis[i], gyro_filtered[i], dT);
actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], speedScaleFactor, rateDesiredAxis[i], gyro_filtered[i], dT);
actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i], 1.0f);
break;
@ -328,7 +356,7 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
rateDesiredAxis[i] = bound(rateDesiredAxis[i], settings.ManualRate[i]);
actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], rateDesiredAxis[i], gyro_filtered[i], dT);
actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], speedScaleFactor, rateDesiredAxis[i], gyro_filtered[i], dT);
actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i], 1.0f);
break;

3
shared/uavobjectdefinition/stabilizationsettings.xml
View File

@ -37,6 +37,9 @@
<field name="LowThrottleZeroAxis" units="" type="enum" elementnames="Roll,Pitch,Yaw" options="FALSE,TRUE" defaultvalue="FALSE,FALSE,FALSE"/>
<field name="ScaleToAirspeed" units="m/s" type="float" elements="1" defaultvalue="0"/>
<field name="ScaleToAirspeedLimits" units="" type="float" elementnames="Min,Max" defaultvalue="0.05,3"/>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="true" updatemode="onchange" period="0"/>
<telemetryflight acked="true" updatemode="onchange" period="0"/>