Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-16 12:55:19 +03:00
Code Issues Wiki Activity

attempts to improve relax

Browse source 
attempts to improve relax fix

attempts to improve relax fix

bug fixes

spaces

changed settings and added isAirmodeActive()

fix rebase error
This commit is contained in:
Thorsten Laux 2018-06-05 13:33:20 +02:00 • committed by Michael Keller
parent 7936fb07d5
commit fefedbd686
6 changed files with 101 additions and 67 deletions
Download patch file Download diff file Expand all files Collapse all files

9
src/main/fc/fc_core.c
View file

@ -537,6 +537,14 @@ uint8_t calculateThrottlePercent(void)
return ret;
}
static bool airmodeIsActivated;
bool isAirmodeActivated()
{
return airmodeIsActivated;
}
/*
* processRx called from taskUpdateRxMain
@ -544,7 +552,6 @@ uint8_t calculateThrottlePercent(void)
bool processRx(timeUs_t currentTimeUs)
{
static bool armedBeeperOn = false;
static bool airmodeIsActivated;
static bool sharedPortTelemetryEnabled = false;
if (!calculateRxChannelsAndUpdateFailsafe(currentTimeUs)) {

1
src/main/fc/fc_core.h
View file

@ -52,3 +52,4 @@ void taskMainPidLoop(timeUs_t currentTimeUs);
bool isFlipOverAfterCrashMode(void);
void runawayTakeoffTemporaryDisable(uint8_t disableFlag);
bool isAirmodeActivated();

139
src/main/flight/pid.c
View file

@ -147,8 +147,8 @@ void resetPidProfile(pidProfile_t *pidProfile)
.iterm_rotation = false,
.smart_feedforward = false,
.iterm_relax = ITERM_RELAX_OFF,
.iterm_relax_cutoff_low = 3,
.iterm_relax_cutoff_high = 30,
.iterm_relax_cutoff = 11,
.iterm_relax_type = ITERM_RELAX_GYRO,
.acro_trainer_angle_limit = 20,
.acro_trainer_lookahead_ms = 50,
.acro_trainer_debug_axis = FD_ROLL,
@ -206,10 +206,10 @@ static FAST_RAM_ZERO_INIT pt1Filter_t dtermLowpass2[2];
static FAST_RAM_ZERO_INIT filterApplyFnPtr ptermYawLowpassApplyFn;
static FAST_RAM_ZERO_INIT pt1Filter_t ptermYawLowpass;
#if defined(USE_ITERM_RELAX)
static FAST_RAM_ZERO_INIT pt1Filter_t windupLpf[XYZ_AXIS_COUNT][2];
static FAST_RAM_ZERO_INIT pt1Filter_t windupLpf[XYZ_AXIS_COUNT];
static FAST_RAM_ZERO_INIT uint8_t itermRelax;
static FAST_RAM_ZERO_INIT uint8_t itermRelaxCutoffLow;
static FAST_RAM_ZERO_INIT uint8_t itermRelaxCutoffHigh;
static FAST_RAM_ZERO_INIT uint8_t itermRelaxType;
static FAST_RAM_ZERO_INIT uint8_t itermRelaxCutoff;
#endif
#ifdef USE_RC_SMOOTHING_FILTER
@ -300,8 +300,7 @@ void pidInitFilters(const pidProfile_t *pidProfile)
#if defined(USE_ITERM_RELAX)
if (itermRelax) {
for (int i = 0; i < XYZ_AXIS_COUNT; i++) {
pt1FilterInit(&windupLpf[i][0], pt1FilterGain(itermRelaxCutoffLow, dT));
pt1FilterInit(&windupLpf[i][1], pt1FilterGain(itermRelaxCutoffHigh, dT));
pt1FilterInit(&windupLpf[i], pt1FilterGain(itermRelaxCutoff, dT));
}
}
#endif
@ -427,8 +426,8 @@ void pidInitConfig(const pidProfile_t *pidProfile)
#endif
#if defined(USE_ITERM_RELAX)
itermRelax = pidProfile->iterm_relax;
itermRelaxCutoffLow = pidProfile->iterm_relax_cutoff_low;
itermRelaxCutoffHigh = pidProfile->iterm_relax_cutoff_high;
itermRelaxType = pidProfile->iterm_relax_type;
itermRelaxCutoff = pidProfile->iterm_relax_cutoff;
#endif
#ifdef USE_ACRO_TRAINER
@ -803,19 +802,77 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT
}
#endif // USE_YAW_SPIN_RECOVERY
#if defined(USE_ABSOLUTE_CONTROL)
// mix in a correction for accrued attitude error
float acCorrection = 0;
if (acGain > 0) {
acCorrection = constrainf(axisError[axis] * acGain, -acLimit, acLimit);
currentPidSetpoint += acCorrection;
}
#endif
// -----calculate error rate
const float gyroRate = gyro.gyroADCf[axis]; // Process variable from gyro output in deg/sec
float errorRate = currentPidSetpoint - gyroRate; // r - y
#ifdef USE_ABSOLUTE_CONTROL
float acCorrection = 0;
float acErrorRate;
#endif
float itermErrorRate = 0.0f;
#if defined(USE_ITERM_RELAX)
float stickSetpoint = currentPidSetpoint;
if (itermRelax && (axis < FD_YAW || itermRelax == ITERM_RELAX_RPY )) {
const float gyroTarget = pt1FilterApply(&windupLpf[axis], stickSetpoint);
const float gyroHpf = fabsf(stickSetpoint - gyroTarget);
if (itermRelaxType == ITERM_RELAX_SETPOINT) {
itermErrorRate = (1 - MIN(1, fabsf(gyroHpf) / 60.0f)) * (stickSetpoint - gyroRate);
}
const float tolerance = gyroHpf * 1.0f;
if (axis == FD_ROLL) {
DEBUG_SET(DEBUG_ITERM_RELAX, 0, gyroTarget);
DEBUG_SET(DEBUG_ITERM_RELAX, 1, gyroTarget + tolerance);
DEBUG_SET(DEBUG_ITERM_RELAX, 2, gyroTarget - tolerance);
DEBUG_SET(DEBUG_ITERM_RELAX, 3, axisError[axis] * 10);
}
if (itermRelaxType == ITERM_RELAX_GYRO ) {
const float gmax = gyroTarget + tolerance;
const float gmin = gyroTarget - tolerance;
if (gyroRate >= gmin && gyroRate <= gmax) {
itermErrorRate = 0;
} else {
itermErrorRate = (gyroRate > gmax ? gmax : gmin ) - gyroRate;
}
}
#if defined(USE_ABSOLUTE_CONTROL)
const float gmaxac = gyroTarget + 2 * tolerance;
const float gminac = gyroTarget - 2 * tolerance;
if (gyroRate >= gminac && gyroRate <= gmaxac) {
float acErrorRate1 = gmaxac - gyroRate;
float acErrorRate2 = gminac - gyroRate;
if (acErrorRate1 * axisError[axis] < 0) {
acErrorRate = acErrorRate1;
} else {
acErrorRate = acErrorRate2;
}
if (fabsf(acErrorRate*dT) > fabsf(axisError[axis]) ) {
acErrorRate = -axisError[axis]/dT;
}
} else {
acErrorRate = (gyroRate > gmaxac ? gmaxac : gminac ) - gyroRate;
}
#endif // USE_ABSOLUTE_CONTROL
} else
#endif // USE_ITERM_RELAX
{
itermErrorRate = currentPidSetpoint - gyroRate;
#if defined(USE_ABSOLUTE_CONTROL)
acErrorRate = itermErrorRate;
#endif // USE_ABSOLUTE_CONTROL
}
#if defined(USE_ABSOLUTE_CONTROL)
if (acGain > 0 && isAirmodeActivated()) {
axisError[axis] = constrainf(axisError[axis] + acErrorRate * dT, -acErrorLimit, acErrorLimit);
acCorrection = constrainf(axisError[axis] * acGain, -acLimit, acLimit);
currentPidSetpoint += acCorrection;
itermErrorRate += acCorrection;
}
#endif
float errorRate = currentPidSetpoint - gyroRate; // r - y
handleCrashRecovery(
pidProfile->crash_recovery, angleTrim, axis, currentTimeUs, gyroRate,
&currentPidSetpoint, &errorRate);
@ -831,49 +888,7 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT
}
// -----calculate I component
float itermErrorRate = 0.0f;
#if defined(USE_ABSOLUTE_CONTROL)
float acErrorRate;
#endif
#if defined(USE_ITERM_RELAX)
if (itermRelax && (axis < FD_YAW || itermRelax == ITERM_RELAX_RPY )) {
float gyroTargetLow = pt1FilterApply(&windupLpf[axis][0], currentPidSetpoint);
float gyroTargetHigh = pt1FilterApply(&windupLpf[axis][1], currentPidSetpoint);
#if defined(USE_ABSOLUTE_CONTROL)
gyroTargetLow += acCorrection;
gyroTargetHigh += acCorrection;
#endif
if (axis < FD_YAW) {
int itemOffset = (axis << 1);
DEBUG_SET(DEBUG_ITERM_RELAX, itemOffset++, gyroTargetHigh);
DEBUG_SET(DEBUG_ITERM_RELAX, itemOffset, gyroTargetLow);
}
const float gmax = MAX(gyroTargetHigh, gyroTargetLow);
const float gmin = MIN(gyroTargetHigh, gyroTargetLow);
if (gyroRate < gmin || gyroRate > gmax) {
itermErrorRate = (gyroRate > gmax ? gmax : gmin ) - gyroRate;
}
#if defined(USE_ABSOLUTE_CONTROL)
else {
itermErrorRate = acCorrection;
}
acErrorRate = itermErrorRate - acCorrection;
#endif
} else
#endif // USE_ITERM_RELAX
{
itermErrorRate = errorRate;
#if defined(USE_ABSOLUTE_CONTROL)
acErrorRate = errorRate;
#endif
}
#if defined(USE_ABSOLUTE_CONTROL)
if (acGain > 0) {
axisError[axis] = constrainf(axisError[axis] + acErrorRate * dT, -acErrorLimit, acErrorLimit);
}
#endif
const float ITerm = pidData[axis].I;
const float ITermNew = constrainf(ITerm + pidCoefficient[axis].Ki * itermErrorRate * dynCi, -itermLimit, itermLimit);
const bool outputSaturated = mixerIsOutputSaturated(axis, errorRate);

10
src/main/flight/pid.h
View file

@ -83,6 +83,12 @@ typedef enum
ITERM_RELAX_RPY
} itermRelax_e;
typedef enum
{
ITERM_RELAX_GYRO,
ITERM_RELAX_SETPOINT
} itermRelaxType_e;
typedef struct pidProfile_s {
uint16_t yaw_lowpass_hz; // Additional yaw filter when yaw axis too noisy
uint16_t dterm_lowpass_hz; // Delta Filter in hz
@ -124,8 +130,8 @@ typedef struct pidProfile_s {
uint8_t throttle_boost_cutoff; // Which cutoff frequency to use for throttle boost. higher cutoffs keep the boost on for shorter. Specified in hz.
uint8_t iterm_rotation; // rotates iterm to translate world errors to local coordinate system
uint8_t smart_feedforward; // takes only the larger of P and the D weight feed forward term if they have the same sign.
uint8_t iterm_relax_cutoff_low; // Slowest setpoint response to prevent iterm accumulation
uint8_t iterm_relax_cutoff_high; // Fastest setpoint response to prevent iterm accumulation
uint8_t iterm_relax_type; // Specifies type of relax algorithm
uint8_t iterm_relax_cutoff; // This cutoff frequency specifies a low pass filter which predicts average response of the quad to setpoint
uint8_t iterm_relax; // Enable iterm suppression during stick input
uint8_t acro_trainer_angle_limit; // Acro trainer roll/pitch angle limit in degrees
uint8_t acro_trainer_debug_axis; // The axis for which record debugging values are captured 0=roll, 1=pitch

8
src/main/interface/settings.c
View file

@ -341,6 +341,9 @@ static const char * const lookupTableVideoSystem[] = {
static const char * const lookupTableItermRelax[] = {
"OFF", "RP", "RPY"
};
static const char * const lookupTableItermRelaxType[] = {
"GYRO", "SETPOINT"
};
#endif
#ifdef USE_ACRO_TRAINER
@ -441,6 +444,7 @@ const lookupTableEntry_t lookupTables[] = {
#endif // USE_MAX7456
#if defined(USE_ITERM_RELAX)
LOOKUP_TABLE_ENTRY(lookupTableItermRelax),
LOOKUP_TABLE_ENTRY(lookupTableItermRelaxType),
#endif
#ifdef USE_ACRO_TRAINER
LOOKUP_TABLE_ENTRY(lookupTableAcroTrainerDebug),
@ -801,9 +805,9 @@ const clivalue_t valueTable[] = {
#endif
#if defined(USE_ITERM_RELAX)
{ "iterm_relax", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_ITERM_RELAX }, PG_PID_PROFILE, offsetof(pidProfile_t, iterm_relax) },
{ "iterm_relax_type", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_ITERM_RELAX_TYPE }, PG_PID_PROFILE, offsetof(pidProfile_t, iterm_relax_type) },
{ "iterm_relax_cutoff", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 1, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, iterm_relax_cutoff) },
#endif
{ "iterm_relax_cutoff_low", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 1, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, iterm_relax_cutoff_low) },
{ "iterm_relax_cutoff_high", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 1, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, iterm_relax_cutoff_high) },
{ "iterm_windup", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 30, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, itermWindupPointPercent) },
{ "iterm_limit", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 500 }, PG_PID_PROFILE, offsetof(pidProfile_t, itermLimit) },
{ "pidsum_limit", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { PIDSUM_LIMIT_MIN, PIDSUM_LIMIT_MAX }, PG_PID_PROFILE, offsetof(pidProfile_t, pidSumLimit) },

1
src/main/interface/settings.h
View file

@ -100,6 +100,7 @@ typedef enum {
#endif // USE_MAX7456
#if defined(USE_ITERM_RELAX)
TABLE_ITERM_RELAX,
TABLE_ITERM_RELAX_TYPE,
#endif
#ifdef USE_ACRO_TRAINER
TABLE_ACRO_TRAINER_DEBUG,