Acro Plus replaced by Super Expo feature
This commit is contained in:
parent
7b468c09f0
commit
595d5d0867
|
@ -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);
|
||||
|
||||
|
|
|
@ -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]));
|
||||
|
|
|
@ -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
|
||||
PTerm = RateError * (pidProfile->P8[axis] / 40.0f) * tpaFactor;
|
||||
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
|
||||
PTerm = (RateError * pidProfile->P8[axis] * PIDweight[axis] / 100) >> 7;
|
||||
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 {
|
||||
|
|
|
@ -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
|
||||
|
||||
|
|
|
@ -49,7 +49,7 @@ typedef enum {
|
|||
BOXBLACKBOX,
|
||||
BOXFAILSAFE,
|
||||
BOXAIRMODE,
|
||||
BOXACROPLUS,
|
||||
BOXSUPEREXPO,
|
||||
BOX3DDISABLESWITCH,
|
||||
CHECKBOX_ITEM_COUNT
|
||||
} boxId_e;
|
||||
|
|
|
@ -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 } },
|
||||
|
||||
|
|
|
@ -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]));
|
||||
|
|
|
@ -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;
|
||||
|
|
Loading…
Reference in New Issue