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

Acro Plus replaced by Super Expo feature

This commit is contained in:
borisbstyle 2016-03-30 12:38:29 +02:00
parent 7b468c09f0
commit 595d5d0867
8 changed files with 51 additions and 77 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -179,6 +179,8 @@ static void resetPidProfile(pidProfile_t *pidProfile)
pidProfile->yaw_p_limit = YAW_P_LIMIT_MAX;
pidProfile->yaw_lpf_hz = 0.0f;
pidProfile->dterm_average_count = 4;
pidProfile->rollPitchItermResetRate = 0;
pidProfile->yawItermResetRate = 0;
pidProfile->dterm_lpf_hz = 70.0f; // filtering ON by default
pidProfile->deltaMethod = DELTA_FROM_MEASUREMENT;
@ -450,8 +452,7 @@ static void resetConf(void)
masterConfig.rxConfig.rcSmoothing = 0;
masterConfig.rxConfig.fpvCamAngleDegrees = 0;
masterConfig.rxConfig.max_aux_channel = 6;
masterConfig.rxConfig.acroPlusFactor = 30;
masterConfig.rxConfig.acroPlusOffset = 40;
masterConfig.rxConfig.superExpoFactor = 20;
resetAllRxChannelRangeConfigurations(masterConfig.rxConfig.channelRanges);

33
src/main/flight/mixer.c
View File

@ -748,35 +748,6 @@ STATIC_UNIT_TESTED void servoMixer(void)
#endif
void acroPlusApply(void) {
int axis;
for (axis = 0; axis < 2; axis++) {
int16_t factor;
fix12_t wowFactor;
int16_t rcCommandDeflection = constrain(rcCommand[axis], -500, 500); // Limit stick input to 500 (rcCommand 100)
int16_t acroPlusStickOffset = rxConfig->acroPlusOffset * 5;
int16_t motorRange = escAndServoConfig->maxthrottle - escAndServoConfig->minthrottle;
if (feature(FEATURE_3D)) motorRange = (motorRange - (flight3DConfig->deadband3d_high - flight3DConfig->deadband3d_low)) / 2;
/* acro plus factor handling */
if (rxConfig->acroPlusFactor && ABS(rcCommandDeflection) > acroPlusStickOffset + 10) {
if (rcCommandDeflection > 0) {
rcCommandDeflection -= acroPlusStickOffset;
} else {
rcCommandDeflection += acroPlusStickOffset;
}
wowFactor = qConstruct(ABS(rcCommandDeflection) * rxConfig->acroPlusFactor / 100, 500);
factor = qMultiply(wowFactor, (rcCommandDeflection * motorRange) / 500);
wowFactor = Q12 - wowFactor;
} else {
wowFactor = Q12;
factor = 0;
}
axisPID[axis] = factor + qMultiply(wowFactor, axisPID[axis]);
}
}
void mixTable(void)
{
uint32_t i;
@ -785,10 +756,6 @@ void mixTable(void)
bool isFailsafeActive = failsafeIsActive(); // TODO - Find out if failsafe checks are really needed here in mixer code
if (IS_RC_MODE_ACTIVE(BOXACROPLUS)) {
acroPlusApply();
}
if (motorCount >= 4 && mixerConfig->yaw_jump_prevention_limit < YAW_JUMP_PREVENTION_LIMIT_HIGH) {
// prevent "yaw jump" during yaw correction
axisPID[YAW] = constrain(axisPID[YAW], -mixerConfig->yaw_jump_prevention_limit - ABS(rcCommand[YAW]), mixerConfig->yaw_jump_prevention_limit + ABS(rcCommand[YAW]));

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

@ -80,6 +80,14 @@ void setTargetPidLooptime(uint8_t pidProcessDenom) {
targetPidLooptime = targetLooptime * pidProcessDenom;
}
float calculateExpoPlus(int axis, rxConfig_t *rxConfig) {
float propFactor;
propFactor = 1.0f - ((rxConfig->superExpoFactor / 100.0f) * (ABS(rcCommand[axis]) / 500.0f));
return propFactor;
}
void pidResetErrorAngle(void)
{
errorAngleI[ROLL] = 0;
@ -113,31 +121,6 @@ float getdT (void) {
return dT;
}
void scaleItermToRcInput(int axis, pidProfile_t *pidProfile) {
float rcCommandReflection = (float)rcCommand[axis] / 500.0f;
static float iTermScaler[3] = {1.0f, 1.0f, 1.0f};
static float antiWindUpIncrement = 0;
if (!antiWindUpIncrement) antiWindUpIncrement = (0.001 / 500) * targetPidLooptime; // Calculate increment for 500ms period
if (ABS(rcCommandReflection) > 0.7f && (!flightModeFlags)) { /* scaling should not happen in level modes */
/* Reset Iterm on high stick inputs. No scaling necessary here */
iTermScaler[axis] = 0.0f;
errorGyroI[axis] = 0;
errorGyroIf[axis] = 0.0f;
} else {
/* Prevent rapid windup during acro recoveries. Slowly enable Iterm for period of 500ms */
if (iTermScaler[axis] < 1) {
iTermScaler[axis] = constrainf(iTermScaler[axis] + antiWindUpIncrement, 0.0f, 1.0f);
if (pidProfile->pidController != PID_CONTROLLER_LUX_FLOAT) {
errorGyroI[axis] *= iTermScaler[axis];
} else {
errorGyroIf[axis] *= iTermScaler[axis];
}
}
}
}
const angle_index_t rcAliasToAngleIndexMap[] = { AI_ROLL, AI_PITCH };
static filterStatePt1_t deltaFilterState[3];
@ -207,7 +190,11 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
RateError = AngleRate - gyroRate;
// -----calculate P component
if (IS_RC_MODE_ACTIVE(BOXSUPEREXPO) && axis != YAW) {
PTerm = (pidProfile->P8[axis] / 40.0f * tpaFactor) * (AngleRate - gyroRate * calculateExpoPlus(axis, rxConfig));
} else {
PTerm = RateError * (pidProfile->P8[axis] / 40.0f) * tpaFactor;
}
// Constrain YAW by yaw_p_limit value if not servo driven in that case servolimits apply
if((motorCount >= 4 && pidProfile->yaw_p_limit) && axis == YAW) {
@ -217,8 +204,15 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
// -----calculate I component.
errorGyroIf[axis] = constrainf(errorGyroIf[axis] + RateError * getdT() * pidProfile->I8[axis] / 10.0f, -250.0f, 250.0f);
if (IS_RC_MODE_ACTIVE(BOXAIRMODE) || IS_RC_MODE_ACTIVE(BOXACROPLUS)) {
if (IS_RC_MODE_ACTIVE(BOXACROPLUS)) scaleItermToRcInput(axis, pidProfile);
if (pidProfile->rollPitchItermResetRate && axis != YAW) {
if (ABS(gyroRate) >= pidProfile->rollPitchItermResetRate) errorGyroIf[axis] = 0;
}
if (pidProfile->yawItermResetRate && axis == YAW) {
if (ABS(gyroRate) >= pidProfile->yawItermResetRate) errorGyroIf[axis] = 0;
}
if (IS_RC_MODE_ACTIVE(BOXAIRMODE)) {
if (antiWindupProtection || motorLimitReached) {
errorGyroIf[axis] = constrainf(errorGyroIf[axis], -errorGyroIfLimit[axis], errorGyroIfLimit[axis]);
} else {
@ -302,8 +296,7 @@ static void pidMultiWii23(pidProfile_t *pidProfile, controlRateConfig_t *control
}
// Anti windup protection
if (IS_RC_MODE_ACTIVE(BOXAIRMODE) || IS_RC_MODE_ACTIVE(BOXACROPLUS)) {
if (IS_RC_MODE_ACTIVE(BOXACROPLUS)) scaleItermToRcInput(axis, pidProfile);
if (IS_RC_MODE_ACTIVE(BOXAIRMODE)) {
if (antiWindupProtection || motorLimitReached) {
errorGyroI[axis] = constrain(errorGyroI[axis], -errorGyroILimit[axis], errorGyroILimit[axis]);
} else {
@ -477,7 +470,11 @@ static void pidMultiWiiRewrite(pidProfile_t *pidProfile, controlRateConfig_t *co
RateError = AngleRateTmp - gyroRate;
// -----calculate P component
if (IS_RC_MODE_ACTIVE(BOXSUPEREXPO) && axis != YAW) {
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;
}
// Constrain YAW by yaw_p_limit value if not servo driven in that case servolimits apply
if((motorCount >= 4 && pidProfile->yaw_p_limit) && axis == YAW) {
@ -495,8 +492,15 @@ 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(BOXAIRMODE) || IS_RC_MODE_ACTIVE(BOXACROPLUS)) {
if (IS_RC_MODE_ACTIVE(BOXACROPLUS)) scaleItermToRcInput(axis, pidProfile);
if (pidProfile->rollPitchItermResetRate && axis != YAW) {
if (ABS(gyroRate / 4) >= pidProfile->rollPitchItermResetRate) errorGyroIf[axis] = 0;
}
if (pidProfile->yawItermResetRate && axis == YAW) {
if (ABS(gyroRate / 4) >= pidProfile->yawItermResetRate) errorGyroIf[axis] = 0;
}
if (IS_RC_MODE_ACTIVE(BOXAIRMODE)) {
if (antiWindupProtection || motorLimitReached) {
errorGyroI[axis] = constrain(errorGyroI[axis], -errorGyroILimit[axis], errorGyroILimit[axis]);
} else {

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

@ -67,7 +67,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
uint8_t deltaMethod; // Alternative delta Calculation
uint8_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 deltaMethod; // Alternative delta Calculation
uint16_t yaw_p_limit;
uint8_t dterm_average_count; // Configurable delta count for dterm

2
src/main/io/rc_controls.h
View File

@ -49,7 +49,7 @@ typedef enum {
BOXBLACKBOX,
BOXFAILSAFE,
BOXAIRMODE,
BOXACROPLUS,
BOXSUPEREXPO,
BOX3DDISABLESWITCH,
CHECKBOX_ITEM_COUNT
} boxId_e;

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

@ -693,8 +693,7 @@ const clivalue_t valueTable[] = {
{ "yaw_rate", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rates[FD_YAW], .config.minmax = { 0, CONTROL_RATE_CONFIG_YAW_RATE_MAX } },
{ "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} },
{ "acro_plus_factor", VAR_UINT8 | MASTER_VALUE, &masterConfig.rxConfig.acroPlusFactor, .config.minmax = {1, 100 } },
{ "acro_plus_offset", VAR_UINT8 | MASTER_VALUE, &masterConfig.rxConfig.acroPlusOffset, .config.minmax = {1, 90 } },
{ "super_expo_factor", VAR_UINT8 | MASTER_VALUE, &masterConfig.rxConfig.superExpoFactor, .config.minmax = {1, 100 } },
{ "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 } },
@ -729,6 +728,8 @@ const clivalue_t valueTable[] = {
{ "pid_delta_method", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.deltaMethod, .config.lookup = { TABLE_DELTA_METHOD } },
{ "dterm_lpf_hz", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_lpf_hz, .config.minmax = {0, 500 } },
{ "dterm_average_count", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_average_count, .config.minmax = {0, 12 } },
{ "roll_pitch_iterm_reset_deg", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.rollPitchItermResetRate, .config.minmax = {0, 1000 } },
{ "yaw_iterm_reset_deg", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yawItermResetRate, .config.minmax = {0, 1000 } },
{ "yaw_lpf_hz", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_lpf_hz, .config.minmax = {0, 500 } },
{ "pid_process_denom", VAR_UINT8 | MASTER_VALUE, &masterConfig.pid_process_denom, .config.minmax = { 1, 8 } },

6
src/main/io/serial_msp.c
View File

@ -219,7 +219,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = {
{ BOXBLACKBOX, "BLACKBOX;", 26 },
{ BOXFAILSAFE, "FAILSAFE;", 27 },
{ BOXAIRMODE, "AIR MODE;", 28 },
{ BOXACROPLUS, "ACRO PLUS;", 29 },
{ BOXSUPEREXPO, "SUPER EXPO;", 29 },
{ BOX3DDISABLESWITCH, "DISABLE 3D SWITCH;", 30},
{ CHECKBOX_ITEM_COUNT, NULL, 0xFF }
};
@ -547,7 +547,7 @@ void mspInit(serialConfig_t *serialConfig)
}
activeBoxIds[activeBoxIdCount++] = BOXAIRMODE;
activeBoxIds[activeBoxIdCount++] = BOXACROPLUS;
activeBoxIds[activeBoxIdCount++] = BOXSUPEREXPO;
activeBoxIds[activeBoxIdCount++] = BOX3DDISABLESWITCH;
if (sensors(SENSOR_BARO)) {
@ -654,7 +654,7 @@ static uint32_t packFlightModeFlags(void)
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXBLACKBOX)) << BOXBLACKBOX |
IS_ENABLED(FLIGHT_MODE(FAILSAFE_MODE)) << BOXFAILSAFE |
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXAIRMODE)) << BOXAIRMODE |
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXACROPLUS)) << BOXACROPLUS;
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXSUPEREXPO)) << BOXSUPEREXPO;
for (i = 0; i < activeBoxIdCount; i++) {
int flag = (tmp & (1 << activeBoxIds[i]));

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

@ -124,8 +124,7 @@ typedef struct rxConfig_s {
uint8_t rcSmoothing;
uint8_t fpvCamAngleDegrees; // Camera angle to be scaled into rc commands
uint8_t max_aux_channel;
uint8_t acroPlusFactor; // Air mode acrobility factor
uint8_t acroPlusOffset; // Air mode stick offset
uint8_t superExpoFactor; // Super Expo Factor
uint16_t rx_min_usec;
uint16_t rx_max_usec;