rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-11-29 07:24:13 +01:00
Code Issues Releases Activity

OP-1474 Thrust PID scaling

Browse Source
This commit is contained in:
Stefan Karlsson 2014-09-06 20:02:22 +02:00
parent 849bbfd481
commit c0152b7e19
11 changed files with 179 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -140,3 +140,57 @@ void pid_configure(struct pid *pid, float p, float i, float d, float iLim)
pid->d = d;
pid->iLim = iLim;
}
float pid_scale_factor_from_line(float x, struct point *p0, struct point *p1)
{
// Setup line y = m * x + b.
const float dY1 = p1->y - p0->y;
const float dX1 = p1->x - p0->x;
const float m = dY1 / dX1; // == dY0 / dX0 == (p0.y - b) / (p0.x - 0.0f) ==>
const float b = p0->y - m * p0->x;
// Scale according to given x.
float y = m * x + b;
return 1.0f + y;
}
float pid_scale_factor(pid_scaler *scaler)
{
const int length = sizeof(scaler->points) / sizeof(typeof(scaler->points[0]));
// Find the two points where is within scaler->x. Use the outer points if
// scaler->x is smaller than the first x value or larger than the last x value.
int end_point = length - 1;
for (int i = 1; i < length; i++) {
if (scaler->x < scaler->points[i].x) {
end_point = i;
break;
}
}
return pid_scale_factor_from_line(scaler->x, &scaler->points[end_point - 1], &scaler->points[end_point]);
}
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;
}

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

@ -42,11 +42,22 @@ struct pid {
float lastDer;
};
typedef struct pid_scaler {
float x;
struct point {
float x;
float y;
} points[5];
} 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);
float pid_scale_factor(pid_scaler *scaler);
#endif /* PID_H */

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

@ -42,6 +42,10 @@
#include <flightstatus.h>
#include <manualcontrolcommand.h>
#include <stabilizationbank.h>
#include <stabilizationdesired.h>
#include <actuatordesired.h>
#include <tpsdebug.h>
#include <pid.h> // FIXME: Temporary debugging
#include <stabilization.h>
#include <relay_tuning.h>
@ -80,6 +84,9 @@ void stabilizationInnerloopInit()
StabilizationStatusInitialize();
FlightStatusInitialize();
ManualControlCommandInitialize();
StabilizationDesiredInitialize();
ActuatorDesiredInitialize();
TPSDebugInitialize();
#ifdef REVOLUTION
AirspeedStateInitialize();
AirspeedStateConnectCallback(AirSpeedUpdatedCb);
@ -93,6 +100,79 @@ void stabilizationInnerloopInit()
PIOS_CALLBACKSCHEDULER_Schedule(callbackHandle, FAILSAFE_TIMEOUT_MS, CALLBACK_UPDATEMODE_LATER);
}
static float get_pid_scale_source_value()
{
float value;
switch (stabSettings.settings.ThrustPIDScaleSource) {
case STABILIZATIONSETTINGS_THRUSTPIDSCALESOURCE_MANUALCONTROLTHROTTLE:
ManualControlCommandThrottleGet(&value);
break;
case STABILIZATIONSETTINGS_THRUSTPIDSCALESOURCE_STABILIZATIONDESIREDTHRUST:
StabilizationDesiredThrustGet(&value);
break;
case STABILIZATIONSETTINGS_THRUSTPIDSCALESOURCE_ACTUATORDESIRETHRUST:
ActuatorDesiredThrustGet(&value);
break;
default:
ManualControlCommandThrottleGet(&value);
break;
}
if (value < 0) {
value = 0.0f;
}
return value;
}
static inline pid_scaler create_pid_scaler()
{
float throttle;
ManualControlCommandThrottleGet(&throttle);
struct pid_scaler scaler = {
.x = get_pid_scale_source_value(),
.points = {
{ 0.0f, stabSettings.settings.ThrustPIDScaleCurve[0] },
{ 0.25f, stabSettings.settings.ThrustPIDScaleCurve[1] },
{ 0.50f, stabSettings.settings.ThrustPIDScaleCurve[2] },
{ 0.75f, stabSettings.settings.ThrustPIDScaleCurve[3] },
{ 1.0f, stabSettings.settings.ThrustPIDScaleCurve[4] }
}
};
// FIXME: Temporary debugging
static int pidDebugCount = 0;
pidDebugCount++;
if (pidDebugCount == 100) {
struct pid *pid = &stabSettings.innerPids[0];
TPSDebugData tpsDebug;
TPSDebugGet(&tpsDebug);
tpsDebug.thrust = scaler.x;
tpsDebug.p = pid->p;
tpsDebug.d = pid->d;
tpsDebug.factor = pid_scale_factor(&scaler);
tpsDebug.p_scaled = tpsDebug.p * tpsDebug.factor;
tpsDebug.d_scaled = tpsDebug.d * tpsDebug.factor;
TPSDebugSet(&tpsDebug);
pidDebugCount = 0;
}
return scaler;
}
static int is_pid_scaled_for_axis(int axis)
{
return stabSettings.settings.EnableThrustPIDScaling
&& (axis == 0 // Roll
|| axis == 1); // Pitch
}
/**
* WARNING! This callback executes with critical flight control priority every
@ -200,7 +280,12 @@ static void stabilizationInnerloopTask()
-StabilizationBankMaximumRateToArray(stabSettings.stabBank.MaximumRate)[t],
StabilizationBankMaximumRateToArray(stabSettings.stabBank.MaximumRate)[t]
);
actuatorDesiredAxis[t] = pid_apply_setpoint(&stabSettings.innerPids[t], speedScaleFactor, rate[t], gyro_filtered[t], dT);
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);
}
break;
case STABILIZATIONSTATUS_INNERLOOP_DIRECT:
default:

1
flight/targets/boards/coptercontrol/firmware/Makefile
View File

@ -128,6 +128,7 @@ ifndef TESTAPP
SRC += $(OPUAVSYNTHDIR)/airspeedstate.c
SRC += $(OPUAVSYNTHDIR)/mpu6000settings.c
SRC += $(OPUAVSYNTHDIR)/perfcounter.c
SRC += $(OPUAVSYNTHDIR)/tpsdebug.c
else
## Test Code
SRC += $(OPTESTS)/test_common.c

1
flight/targets/boards/discoveryf4bare/firmware/UAVObjects.inc
View File

@ -115,6 +115,7 @@ UAVOBJSRCFILENAMES += mpu6000settings
UAVOBJSRCFILENAMES += txpidsettings
UAVOBJSRCFILENAMES += takeofflocation
UAVOBJSRCFILENAMES += perfcounter
UAVOBJSRCFILENAMES += tpsdebug
UAVOBJSRC = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),$(OPUAVSYNTHDIR)/$(UAVOBJSRCFILE).c )
UAVOBJDEFINE = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),-DUAVOBJ_INIT_$(UAVOBJSRCFILE) )

1
flight/targets/boards/revolution/firmware/UAVObjects.inc
View File

@ -115,6 +115,7 @@ UAVOBJSRCFILENAMES += mpu6000settings
UAVOBJSRCFILENAMES += txpidsettings
UAVOBJSRCFILENAMES += takeofflocation
UAVOBJSRCFILENAMES += perfcounter
UAVOBJSRCFILENAMES += tpsdebug
UAVOBJSRC = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),$(OPUAVSYNTHDIR)/$(UAVOBJSRCFILE).c )
UAVOBJDEFINE = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),-DUAVOBJ_INIT_$(UAVOBJSRCFILE) )

1
flight/targets/boards/revoproto/firmware/UAVObjects.inc
View File

@ -114,6 +114,7 @@ UAVOBJSRCFILENAMES += poilearnsettings
UAVOBJSRCFILENAMES += mpu6000settings
UAVOBJSRCFILENAMES += txpidsettings
UAVOBJSRCFILENAMES += takeofflocation
UAVOBJSRCFILENAMES += tpsdebug
UAVOBJSRC = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),$(OPUAVSYNTHDIR)/$(UAVOBJSRCFILE).c )
UAVOBJDEFINE = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),-DUAVOBJ_INIT_$(UAVOBJSRCFILE) )

1
flight/targets/boards/simposix/firmware/UAVObjects.inc
View File

@ -112,6 +112,7 @@ UAVOBJSRCFILENAMES += altitudeholdstatus
UAVOBJSRCFILENAMES += ekfconfiguration
UAVOBJSRCFILENAMES += ekfstatevariance
UAVOBJSRCFILENAMES += takeofflocation
UAVOBJSRCFILENAMES += tpsdebug
UAVOBJSRC = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),$(UAVOBJSYNTHDIR)/$(UAVOBJSRCFILE).c )
UAVOBJDEFINE = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),-DUAVOBJ_INIT_$(UAVOBJSRCFILE) )

6
ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro
View File

@ -127,7 +127,8 @@ HEADERS += \
$$UAVOBJECT_SYNTHETICS/waypointactive.h \
$$UAVOBJECT_SYNTHETICS/mpu6000settings.h \
$$UAVOBJECT_SYNTHETICS/takeofflocation.h \
$$UAVOBJECT_SYNTHETICS/perfcounter.h
$$UAVOBJECT_SYNTHETICS/perfcounter.h \
$$UAVOBJECT_SYNTHETICS/tpsdebug.h
SOURCES += \
$$UAVOBJECT_SYNTHETICS/accelgyrosettings.cpp \
@ -231,5 +232,6 @@ SOURCES += \
$$UAVOBJECT_SYNTHETICS/waypointactive.cpp \
$$UAVOBJECT_SYNTHETICS/mpu6000settings.cpp \
$$UAVOBJECT_SYNTHETICS/takeofflocation.cpp \
$$UAVOBJECT_SYNTHETICS/perfcounter.cpp
$$UAVOBJECT_SYNTHETICS/perfcounter.cpp \
$$UAVOBJECT_SYNTHETICS/tpsdebug.cpp

4
shared/uavobjectdefinition/stabilizationsettings.xml
View File

@ -47,6 +47,10 @@
<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"/>
<field name="EnableThrustPIDScaling" units="" type="enum" elements="1" options="FALSE,TRUE" defaultvalue="FALSE"/>
<field name="ThrustPIDScaleCurve" units="percent" type="float" elementnames="0,25,50,75,100" defaultvalue="0.3,0.15,0,-0.15,-0.3"/>
<field name="ThrustPIDScaleSource" units="" type="enum" elements="1" options="ManualControlThrottle,StabilizationDesiredThrust,ActuatorDesireThrust" defaultvalue="ManualControlThrottle" />
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="true" updatemode="onchange" period="0"/>
<telemetryflight acked="true" updatemode="onchange" period="0"/>

15
shared/uavobjectdefinition/tpsdebug.xml Normal file
View File

@ -0,0 +1,15 @@
<xml>
<object name="TPSDebug" singleinstance="true" settings="false" category="State">
<description>Debugging of the @ref Thrust PID Scale module</description>
<field name="thrust" units="ratio" type="float" elements="1" defaultvalue="0" />
<field name="factor" units="ratio" type="float" elements="1" defaultvalue="0" />
<field name="p" units="ratio" type="float" elements="1" defaultvalue="0" />
<field name="p_scaled" units="ratio" type="float" elements="1" defaultvalue="0" />
<field name="d" units="ratio" type="float" elements="1" defaultvalue="0" />
<field name="d_scaled" units="ratio" type="float" elements="1" defaultvalue="0" />
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="true" updatemode="onchange" period="0"/>
<telemetryflight acked="true" updatemode="onchange" period="0"/>
<logging updatemode="manual" period="0"/>
</object>
</xml>