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

OP-1474 Demultiplex TPS settings when pid banks are updated

Browse Source
This commit is contained in:
Stefan Karlsson 2014-09-19 15:08:07 +02:00
parent 9a21eed71e
commit b3e7a23f4f
3 changed files with 86 additions and 72 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; } monitor;
float rattitude_mode_transition_stick_position; float rattitude_mode_transition_stick_position;
struct pid innerPids[3], outerPids[3]; struct pid innerPids[3], outerPids[3];
// TPS [Roll,Pitch,Yaw][P,I,D]
bool thrust_pid_scaling_enabled[3][3];
} StabilizationData; } StabilizationData;

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

@ -123,74 +123,6 @@ static float get_pid_scale_source_value()
return value; return value;
} }
static bool is_roll_pid_thrust_scaled()
{
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 is_pitch_pid_thrust_scaled()
{
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 is_yaw_pid_thrust_scaled()
{
uint8_t axes = stabSettings.stabBank.ThrustPIDScaleAxes;
return axes == STABILIZATIONBANK_THRUSTPIDSCALEAXES_ROLLPITCHYAW ||
axes == STABILIZATIONBANK_THRUSTPIDSCALEAXES_ROLLYAW ||
axes == STABILIZATIONBANK_THRUSTPIDSCALEAXES_PITCHYAW ||
axes == STABILIZATIONBANK_THRUSTPIDSCALEAXES_YAW;
}
static int is_pid_thrust_scaled_for_axis(int axis)
{
return stabSettings.stabBank.EnableThrustPIDScaling
&& ((axis == 0 && is_roll_pid_thrust_scaled())
|| (axis == 1 && is_pitch_pid_thrust_scaled())
|| (axis == 2 && is_yaw_pid_thrust_scaled()));
}
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 { typedef struct pid_curve_scaler {
float x; float x;
pointf points[5]; pointf points[5];
@ -210,7 +142,9 @@ static pid_scaler create_pid_scaler(int axis)
// Always scaled with the this. // Always scaled with the this.
scaler.p = scaler.i = scaler.d = speedScaleFactor; 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 = { const pid_curve_scaler curve_scaler = {
.x = get_pid_scale_source_value(), .x = get_pid_scale_source_value(),
.points = { .points = {
@ -224,13 +158,13 @@ static pid_scaler create_pid_scaler(int axis)
float curve_value = pid_curve_value(&curve_scaler); 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; scaler.p *= curve_value;
} }
if (is_i_scaling_enabled()) { if (stabSettings.thrust_pid_scaling_enabled[axis][1]) {
scaler.i *= curve_value; scaler.i *= curve_value;
} }
if (is_d_scaling_enabled()) { if (stabSettings.thrust_pid_scaling_enabled[axis][2]) {
scaler.d *= curve_value; 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); 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) static void BankUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{ {
StabilizationBankGet(&stabSettings.stabBank); StabilizationBankGet(&stabSettings.stabBank);
@ -268,6 +328,24 @@ static void BankUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
stabSettings.stabBank.YawPI.Ki, stabSettings.stabBank.YawPI.Ki,
0, 0,
stabSettings.stabBank.YawPI.ILimit); 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];
}
}
} }