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

Merge branch 'skarlsso/OP-1474-PID_Scaling_one_factor' of ssh://git.openpilot.org/OpenPilot into thread/OP-1474_PID_Scaling_Banks

Browse Source
This commit is contained in:
m_thread 2014-09-19 15:15:52 +02:00
commit 226fe186c4
7 changed files with 90 additions and 41 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
flight/modules/Stabilization/inc/stabilization.h
View File

@ -59,6 +59,8 @@ typedef struct {
} monitor;
float rattitude_mode_transition_stick_position;
struct pid innerPids[3], outerPids[3];
// TPS [Roll,Pitch,Yaw][P,I,D]
bool thrust_pid_scaling_enabled[3][3];
} StabilizationData;

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

@ -123,43 +123,6 @@ static float get_pid_scale_source_value()
return value;
}
static int is_pid_thrust_scaled_for_axis(int axis)
{
return stabSettings.stabBank.EnableThrustPIDScaling
&& (axis == 0 // Roll
|| axis == 1); // Pitch
}
static bool is_p_scaling_enabled()
{
uint8_t target = stabSettings.stabBank.ThrustPIDScaleTarget;
return target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_PID ||
target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_PI ||
target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_PD ||
target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_P;
}
static bool is_i_scaling_enabled()
{
uint8_t target = stabSettings.stabBank.ThrustPIDScaleTarget;
return target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_PID ||
target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_PI ||
target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_ID ||
target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_I;
}
static bool is_d_scaling_enabled()
{
uint8_t target = stabSettings.stabBank.ThrustPIDScaleTarget;
return target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_PID ||
target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_PD ||
target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_ID ||
target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_D;
}
typedef struct pid_curve_scaler {
float x;
pointf points[5];
@ -179,7 +142,9 @@ static pid_scaler create_pid_scaler(int axis)
// Always scaled with the this.
scaler.p = scaler.i = scaler.d = speedScaleFactor;
if (is_pid_thrust_scaled_for_axis(axis)) {
if (stabSettings.thrust_pid_scaling_enabled[axis][0]
|| stabSettings.thrust_pid_scaling_enabled[axis][1]
|| stabSettings.thrust_pid_scaling_enabled[axis][2]) {
const pid_curve_scaler curve_scaler = {
.x = get_pid_scale_source_value(),
.points = {
@ -193,13 +158,13 @@ static pid_scaler create_pid_scaler(int axis)
float curve_value = pid_curve_value(&curve_scaler);
if (is_p_scaling_enabled()) {
if (stabSettings.thrust_pid_scaling_enabled[axis][0]) {
scaler.p *= curve_value;
}
if (is_i_scaling_enabled()) {
if (stabSettings.thrust_pid_scaling_enabled[axis][1]) {
scaler.i *= curve_value;
}
if (is_d_scaling_enabled()) {
if (stabSettings.thrust_pid_scaling_enabled[axis][2]) {
scaler.d *= curve_value;
}
}

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

@ -229,6 +229,66 @@ static void SettingsBankUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
StabilizationBankSet(&stabSettings.stabBank);
}
static bool use_tps_for_roll()
{
uint8_t axes = stabSettings.stabBank.ThrustPIDScaleAxes;
return axes == STABILIZATIONBANK_THRUSTPIDSCALEAXES_ROLLPITCHYAW ||
axes == STABILIZATIONBANK_THRUSTPIDSCALEAXES_ROLLPITCH ||
axes == STABILIZATIONBANK_THRUSTPIDSCALEAXES_ROLLYAW ||
axes == STABILIZATIONBANK_THRUSTPIDSCALEAXES_ROLL;
}
static bool use_tps_for_pitch()
{
uint8_t axes = stabSettings.stabBank.ThrustPIDScaleAxes;
return axes == STABILIZATIONBANK_THRUSTPIDSCALEAXES_ROLLPITCHYAW ||
axes == STABILIZATIONBANK_THRUSTPIDSCALEAXES_ROLLPITCH ||
axes == STABILIZATIONBANK_THRUSTPIDSCALEAXES_PITCHYAW ||
axes == STABILIZATIONBANK_THRUSTPIDSCALEAXES_PITCH;
}
static bool use_tps_for_yaw()
{
uint8_t axes = stabSettings.stabBank.ThrustPIDScaleAxes;
return axes == STABILIZATIONBANK_THRUSTPIDSCALEAXES_ROLLPITCHYAW ||
axes == STABILIZATIONBANK_THRUSTPIDSCALEAXES_ROLLYAW ||
axes == STABILIZATIONBANK_THRUSTPIDSCALEAXES_PITCHYAW ||
axes == STABILIZATIONBANK_THRUSTPIDSCALEAXES_YAW;
}
static bool use_tps_for_p()
{
uint8_t target = stabSettings.stabBank.ThrustPIDScaleTarget;
return target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_PID ||
target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_PI ||
target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_PD ||
target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_P;
}
static bool use_tps_for_i()
{
uint8_t target = stabSettings.stabBank.ThrustPIDScaleTarget;
return target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_PID ||
target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_PI ||
target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_ID ||
target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_I;
}
static bool use_tps_for_d()
{
uint8_t target = stabSettings.stabBank.ThrustPIDScaleTarget;
return target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_PID ||
target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_PD ||
target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_ID ||
target == STABILIZATIONBANK_THRUSTPIDSCALETARGET_D;
}
static void BankUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
StabilizationBankGet(&stabSettings.stabBank);
@ -268,6 +328,24 @@ static void BankUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
stabSettings.stabBank.YawPI.Ki,
0,
stabSettings.stabBank.YawPI.ILimit);
bool tps_for_axis[3] = {
use_tps_for_roll(),
use_tps_for_pitch(),
use_tps_for_yaw()
};
bool tps_for_pid[3] = {
use_tps_for_p(),
use_tps_for_i(),
use_tps_for_d()
};
for (int axis = 0; axis < 3; axis++) {
for (int pid = 0; pid < 3; pid++) {
stabSettings.thrust_pid_scaling_enabled[axis][pid] = stabSettings.stabBank.EnableThrustPIDScaling
&& tps_for_axis[axis]
&& tps_for_pid[pid];
}
}
}

1
shared/uavobjectdefinition/stabilizationbank.xml
View File

@ -19,6 +19,7 @@
<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,ActuatorDesiredThrust" defaultvalue="ActuatorDesiredThrust" />
<field name="ThrustPIDScaleTarget" units="" type="enum" elements="1" options="PID,PI,PD,ID,P,I,D" defaultvalue="PID" />
<field name="ThrustPIDScaleAxes" units="" type="enum" elements="1" options="Roll Pitch Yaw,Roll Pitch,Roll Yaw,Roll,Pitch Yaw,Pitch,Yaw" defaultvalue="Roll Pitch" />
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="false" updatemode="manual" period="0"/>

1
shared/uavobjectdefinition/stabilizationsettingsbank1.xml
View File

@ -19,6 +19,7 @@
<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,ActuatorDesiredThrust" defaultvalue="ActuatorDesiredThrust" />
<field name="ThrustPIDScaleTarget" units="" type="enum" elements="1" options="PID,PI,PD,ID,P,I,D" defaultvalue="PID" />
<field name="ThrustPIDScaleAxes" units="" type="enum" elements="1" options="Roll Pitch Yaw,Roll Pitch,Roll Yaw,Roll,Pitch Yaw,Pitch,Yaw" defaultvalue="Roll Pitch" />
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="true" updatemode="onchange" period="0"/>

1
shared/uavobjectdefinition/stabilizationsettingsbank2.xml
View File

@ -19,6 +19,7 @@
<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,ActuatorDesiredThrust" defaultvalue="ActuatorDesiredThrust" />
<field name="ThrustPIDScaleTarget" units="" type="enum" elements="1" options="PID,PI,PD,ID,P,I,D" defaultvalue="PID" />
<field name="ThrustPIDScaleAxes" units="" type="enum" elements="1" options="Roll Pitch Yaw,Roll Pitch,Roll Yaw,Roll,Pitch Yaw,Pitch,Yaw" defaultvalue="Roll Pitch" />
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="true" updatemode="onchange" period="0"/>

1
shared/uavobjectdefinition/stabilizationsettingsbank3.xml
View File

@ -19,6 +19,7 @@
<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,ActuatorDesiredThrust" defaultvalue="ActuatorDesiredThrust" />
<field name="ThrustPIDScaleTarget" units="" type="enum" elements="1" options="PID,PI,PD,ID,P,I,D" defaultvalue="PID" />
<field name="ThrustPIDScaleAxes" units="" type="enum" elements="1" options="Roll Pitch Yaw,Roll Pitch,Roll Yaw,Roll,Pitch Yaw,Pitch,Yaw" defaultvalue="Roll Pitch" />
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="true" updatemode="onchange" period="0"/>