rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Optional SUPER EXPO for yaw // Optional always Iterm reset // Rework Iterm reset

This commit is contained in:
borisbstyle 2016-04-27 22:09:17 +02:00
parent afd5f8b542
commit 86c2e12c07
6 changed files with 46 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
src/main/blackbox/blackbox.c
View File

@ -1280,8 +1280,8 @@ static bool blackboxWriteSysinfo()
masterConfig.profile[masterConfig.current_profile_index].pidProfile.dterm_average_count);
break;
case 36:
blackboxPrintfHeaderLine("dynamic_dterm_threshold:%d",
masterConfig.profile[masterConfig.current_profile_index].pidProfile.dynamic_dterm_threshold);
blackboxPrintfHeaderLine("dterm_differentiator:%d",
masterConfig.profile[masterConfig.current_profile_index].pidProfile.dterm_differentiator);
break;
case 37:
blackboxPrintfHeaderLine("rollPitchItermResetRate:%d",

6
src/main/config/config.c
View File

@ -134,7 +134,7 @@ static uint32_t activeFeaturesLatch = 0;
static uint8_t currentControlRateProfileIndex = 0;
controlRateConfig_t *currentControlRateProfile;
static const uint8_t EEPROM_CONF_VERSION = 132;
static const uint8_t EEPROM_CONF_VERSION = 133;
static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims)
{
@ -178,9 +178,9 @@ static void resetPidProfile(pidProfile_t *pidProfile)
pidProfile->yaw_p_limit = YAW_P_LIMIT_MAX;
pidProfile->yaw_lpf_hz = 70.0f;
pidProfile->dterm_average_count = 0;
pidProfile->dterm_differentiator = 1;
pidProfile->rollPitchItermResetRate = 200;
pidProfile->rollPitchItermResetAlways = 0;
pidProfile->yawItermResetRate = 50;
pidProfile->dterm_lpf_hz = 70.0f; // filtering ON by default
pidProfile->deltaMethod = DELTA_FROM_MEASUREMENT;
@ -454,6 +454,8 @@ static void resetConf(void)
masterConfig.rxConfig.fpvCamAngleDegrees = 0;
masterConfig.rxConfig.max_aux_channel = 6;
masterConfig.rxConfig.superExpoFactor = 30;
masterConfig.rxConfig.superExpoFactorYaw = 30;
masterConfig.rxConfig.superExpoYawMode = 0;
resetAllRxChannelRangeConfigurations(masterConfig.rxConfig.channelRanges);

28
src/main/flight/pid.c
View File

@ -80,11 +80,17 @@ void setTargetPidLooptime(uint8_t pidProcessDenom) {
}
float calculateExpoPlus(int axis, rxConfig_t *rxConfig) {
float propFactor;
float propFactor;
float superExpoFactor;
propFactor = 1.0f - ((rxConfig->superExpoFactor / 100.0f) * (ABS(rcCommand[axis]) / 500.0f));
if (axis == YAW && !rxConfig->superExpoYawMode) {
propFactor = 1.0f;
} else {
superExpoFactor = (axis == YAW) ? rxConfig->superExpoFactorYaw : rxConfig->superExpoFactor;
propFactor = 1.0f - ((superExpoFactor / 100.0f) * (ABS(rcCommand[axis]) / 500.0f));
}
return propFactor;
return propFactor;
}
void pidResetErrorAngle(void)
@ -195,7 +201,7 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
RateError = AngleRate - gyroRate;
// -----calculate P component
if (IS_RC_MODE_ACTIVE(BOXSUPEREXPO) && axis != YAW) {
if ((IS_RC_MODE_ACTIVE(BOXSUPEREXPO) && axis != YAW) || (axis == YAW && rxConfig->superExpoYawMode == SUPEREXPO_YAW_ALWAYS)) {
PTerm = (luxPTermScale * pidProfile->P8[axis] * tpaFactor) * (AngleRate - gyroRate * calculateExpoPlus(axis, rxConfig));
} else {
PTerm = luxPTermScale * RateError * pidProfile->P8[axis] * tpaFactor;
@ -209,12 +215,12 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
// -----calculate I component.
errorGyroIf[axis] = constrainf(errorGyroIf[axis] + luxITermScale * RateError * getdT() * pidProfile->I8[axis], -250.0f, 250.0f);
if (IS_RC_MODE_ACTIVE(BOXSUPEREXPO) && axis != YAW) {
if (ABS(gyroRate / 4.1f) >= pidProfile->rollPitchItermResetRate) errorGyroIf[axis] = 0;
if ((pidProfile->rollPitchItermResetAlways || IS_RC_MODE_ACTIVE(BOXSUPEREXPO)) && axis != YAW) {
if (ABS(gyroRate / 4.1f) >= pidProfile->rollPitchItermResetRate) errorGyroIf[axis] = constrainf(errorGyroIf[axis], -ITERM_RESET_THRESHOLD, ITERM_RESET_THRESHOLD);
}
if (axis == YAW) {
if (ABS(gyroRate / 4.1f) >= pidProfile->yawItermResetRate) errorGyroIf[axis] = 0;
if (ABS(gyroRate / 4.1f) >= pidProfile->yawItermResetRate) errorGyroIf[axis] = constrainf(errorGyroIf[axis], -ITERM_RESET_THRESHOLD_YAW, ITERM_RESET_THRESHOLD_YAW);
}
if (antiWindupProtection || motorLimitReached) {
@ -337,7 +343,7 @@ static void pidMultiWiiRewrite(pidProfile_t *pidProfile, controlRateConfig_t *co
RateError = AngleRateTmp - gyroRate;
// -----calculate P component
if (IS_RC_MODE_ACTIVE(BOXSUPEREXPO) && axis != YAW) {
if ((IS_RC_MODE_ACTIVE(BOXSUPEREXPO) && axis != YAW) || (axis == YAW && rxConfig->superExpoYawMode == SUPEREXPO_YAW_ALWAYS)) {
PTerm = (pidProfile->P8[axis] * PIDweight[axis] / 100) * (AngleRateTmp - (int32_t)(gyroRate * calculateExpoPlus(axis, rxConfig))) >> 7;
} else {
PTerm = (RateError * pidProfile->P8[axis] * PIDweight[axis] / 100) >> 7;
@ -359,12 +365,12 @@ static void pidMultiWiiRewrite(pidProfile_t *pidProfile, controlRateConfig_t *co
// I coefficient (I8) moved before integration to make limiting independent from PID settings
errorGyroI[axis] = constrain(errorGyroI[axis], (int32_t) - GYRO_I_MAX << 13, (int32_t) + GYRO_I_MAX << 13);
if (IS_RC_MODE_ACTIVE(BOXSUPEREXPO) && axis != YAW) {
if (ABS(gyroRate *10 / 41) >= pidProfile->rollPitchItermResetRate) errorGyroI[axis] = 0;
if ((pidProfile->rollPitchItermResetAlways || IS_RC_MODE_ACTIVE(BOXSUPEREXPO)) && axis != YAW) {
if (ABS(gyroRate *10 / 41) >= pidProfile->rollPitchItermResetRate) errorGyroI[axis] = constrain(errorGyroI[axis], -ITERM_RESET_THRESHOLD, ITERM_RESET_THRESHOLD);
}
if (axis == YAW) {
if (ABS(gyroRate * 10 / 41) >= pidProfile->yawItermResetRate) errorGyroI[axis] = 0;
if (ABS(gyroRate * 10 / 41) >= pidProfile->yawItermResetRate) errorGyroI[axis] = constrain(errorGyroI[axis], -ITERM_RESET_THRESHOLD_YAW, ITERM_RESET_THRESHOLD_YAW);
}
if (antiWindupProtection || motorLimitReached) {

13
src/main/flight/pid.h
View File

@ -25,6 +25,8 @@
#define PID_LAST_RATE_COUNT 7
#define PID_DTERM_FIR_MAX_LENGTH 7
#define PID_MAX_DIFFERENTIATOR (PID_DTERM_FIR_MAX_LENGTH-2)
#define ITERM_RESET_THRESHOLD 20
#define ITERM_RESET_THRESHOLD_YAW 10
typedef enum {
PIDROLL,
@ -57,6 +59,12 @@ typedef enum {
RESET_ITERM_AND_REDUCE_PID
} pidErrorResetOption_e;
typedef enum {
SUPEREXPO_YAW_OFF = 0,
SUPEREXPO_YAW_ON,
SUPEREXPO_YAW_ALWAYS
} pidSuperExpoYaw_e;
#define IS_PID_CONTROLLER_FP_BASED(pidController) (pidController == 2)
typedef struct pidProfile_s {
@ -70,8 +78,9 @@ typedef struct pidProfile_s {
float dterm_lpf_hz; // Delta Filter in hz
float yaw_lpf_hz; // Additional yaw filter when yaw axis too noisy
uint16_t rollPitchItermResetRate; // Experimental threshold for resetting iterm for pitch and roll on certain rates
uint16_t yawItermResetRate; // Experimental threshold for resetting iterm for yaw on certain rates
uint16_t rollPitchItermResetRate; // Experimental threshold for resetting iterm for pitch and roll on certain rates
uint8_t rollPitchItermResetAlways; // Reset Iterm also without SUPER EXPO
uint16_t yawItermResetRate; // Experimental threshold for resetting iterm for yaw on certain rates
uint8_t deltaMethod; // Alternative delta Calculation
uint16_t yaw_p_limit;
uint8_t dterm_average_count; // Configurable delta count for dterm

11
src/main/io/serial_cli.c
View File

@ -440,6 +440,10 @@ static const char * const lookupTableDebug[] = {
"AIRMODE"
};
static const char * const lookupTableSuperExpoYaw[] = {
"OFF", "ON", "ALWAYS"
};
typedef struct lookupTableEntry_s {
const char * const *values;
const uint8_t valueCount;
@ -466,6 +470,7 @@ typedef enum {
TABLE_MAG_HARDWARE,
TABLE_DELTA_METHOD,
TABLE_DEBUG,
TABLE_SUPEREXPO_YAW,
} lookupTableIndex_e;
static const lookupTableEntry_t lookupTables[] = {
@ -488,7 +493,8 @@ static const lookupTableEntry_t lookupTables[] = {
{ lookupTableBaroHardware, sizeof(lookupTableBaroHardware) / sizeof(char *) },
{ lookupTableMagHardware, sizeof(lookupTableMagHardware) / sizeof(char *) },
{ lookupDeltaMethod, sizeof(lookupDeltaMethod) / sizeof(char *) },
{ lookupTableDebug, sizeof(lookupTableDebug) / sizeof(char *) }
{ lookupTableDebug, sizeof(lookupTableDebug) / sizeof(char *) },
{ lookupTableSuperExpoYaw, sizeof(lookupTableSuperExpoYaw) / sizeof(char *) }
};
#define VALUE_TYPE_OFFSET 0
@ -688,6 +694,8 @@ const clivalue_t valueTable[] = {
{ "tpa_rate", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].dynThrPID, .config.minmax = { 0, CONTROL_RATE_CONFIG_TPA_MAX} },
{ "tpa_breakpoint", VAR_UINT16 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].tpa_breakpoint, .config.minmax = { PWM_RANGE_MIN, PWM_RANGE_MAX} },
{ "super_expo_factor", VAR_UINT8 | MASTER_VALUE, &masterConfig.rxConfig.superExpoFactor, .config.minmax = {1, 100 } },
{ "super_expo_factor_yaw", VAR_UINT8 | MASTER_VALUE, &masterConfig.rxConfig.superExpoFactorYaw, .config.minmax = {1, 100 } },
{ "super_expo_yaw", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.rxConfig.superExpoYawMode, .config.lookup = { TABLE_SUPEREXPO_YAW } },
{ "failsafe_delay", VAR_UINT8 | MASTER_VALUE, &masterConfig.failsafeConfig.failsafe_delay, .config.minmax = { 0, 200 } },
{ "failsafe_off_delay", VAR_UINT8 | MASTER_VALUE, &masterConfig.failsafeConfig.failsafe_off_delay, .config.minmax = { 0, 200 } },
@ -722,6 +730,7 @@ const clivalue_t valueTable[] = {
{ "pid_delta_method", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.deltaMethod, .config.lookup = { TABLE_DELTA_METHOD } },
{ "dterm_lowpass_hz", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_lpf_hz, .config.minmax = {0, 500 } },
{ "dterm_differentiator", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.dterm_differentiator, .config.lookup = { TABLE_OFF_ON } },
{ "iterm_always_reset", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.rollPitchItermResetAlways, .config.lookup = { TABLE_OFF_ON } },
{ "iterm_reset_degrees", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.rollPitchItermResetRate, .config.minmax = {50, 1000 } },
{ "yaw_iterm_reset_degrees", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yawItermResetRate, .config.minmax = {25, 1000 } },
{ "yaw_lowpass_hz", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yaw_lpf_hz, .config.minmax = {0, 500 } },

2
src/main/rx/rx.h
View File

@ -125,6 +125,8 @@ typedef struct rxConfig_s {
uint8_t fpvCamAngleDegrees; // Camera angle to be scaled into rc commands
uint8_t max_aux_channel;
uint8_t superExpoFactor; // Super Expo Factor
uint8_t superExpoFactorYaw; // Super Expo Factor Yaw
uint8_t superExpoYawMode; // Seperate Super expo for yaw
uint16_t rx_min_usec;
uint16_t rx_max_usec;