Move acroplus to mixer
This commit is contained in:
parent
fa391507b6
commit
293820f567
|
@ -134,7 +134,7 @@ static uint32_t activeFeaturesLatch = 0;
|
|||
static uint8_t currentControlRateProfileIndex = 0;
|
||||
controlRateConfig_t *currentControlRateProfile;
|
||||
|
||||
static const uint8_t EEPROM_CONF_VERSION = 123;
|
||||
static const uint8_t EEPROM_CONF_VERSION = 124;
|
||||
|
||||
static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims)
|
||||
{
|
||||
|
@ -180,8 +180,6 @@ static void resetPidProfile(pidProfile_t *pidProfile)
|
|||
pidProfile->dterm_average_count = 4;
|
||||
pidProfile->dterm_lpf_hz = 0; // filtering ON by default
|
||||
pidProfile->deltaMethod = DELTA_FROM_MEASUREMENT;
|
||||
pidProfile->acroPlusFactor = 30;
|
||||
pidProfile->acroPlusOffset = 40;
|
||||
|
||||
pidProfile->P_f[ROLL] = 1.1f;
|
||||
pidProfile->I_f[ROLL] = 0.4f;
|
||||
|
@ -448,6 +446,8 @@ static void resetConf(void)
|
|||
masterConfig.rxConfig.rcSmoothing = 0;
|
||||
masterConfig.rxConfig.fpvCamAngleDegrees = 0;
|
||||
masterConfig.rxConfig.max_aux_channel = 4;
|
||||
masterConfig.rxConfig.acroPlusFactor = 30;
|
||||
masterConfig.rxConfig.acroPlusOffset = 40;
|
||||
|
||||
resetAllRxChannelRangeConfigurations(masterConfig.rxConfig.channelRanges);
|
||||
|
||||
|
|
|
@ -749,6 +749,35 @@ STATIC_UNIT_TESTED void servoMixer(void)
|
|||
|
||||
#endif
|
||||
|
||||
void acroPlusApply(int16_t axisPID[3]) {
|
||||
int axis;
|
||||
|
||||
for (axis = 0; axis < 2; axis++) {
|
||||
int16_t factor;
|
||||
q_number_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;
|
||||
}
|
||||
qConstruct(&wowFactor,ABS(rcCommandDeflection) * rxConfig->acroPlusFactor / 100, 500, Q12_NUMBER);
|
||||
factor = qMultiply(wowFactor, (rcCommandDeflection * motorRange) / 500);
|
||||
wowFactor.num = wowFactor.den - wowFactor.num;
|
||||
} else {
|
||||
qConstruct(&wowFactor, 1, 1, Q12_NUMBER);
|
||||
factor = 0;
|
||||
}
|
||||
axisPID[axis] = factor + qMultiply(wowFactor, axisPID[axis]);
|
||||
}
|
||||
}
|
||||
|
||||
void mixTable(void)
|
||||
{
|
||||
uint32_t i;
|
||||
|
@ -756,6 +785,10 @@ 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(axisPID);
|
||||
}
|
||||
|
||||
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]));
|
||||
|
|
|
@ -122,29 +122,8 @@ q_number_t scaleItermToRcInput(int axis) {
|
|||
return qItermScaler[axis];
|
||||
}
|
||||
|
||||
void acroPlusApply(acroPlus_t *axisState, int axis, pidProfile_t *pidProfile) {
|
||||
int16_t rcCommandDeflection = constrain(rcCommand[axis], -500, 500); // Limit stick input to 500 (rcCommand 100)
|
||||
int16_t acroPlusStickOffset = pidProfile->acroPlusOffset * 5;
|
||||
|
||||
/* acro plus factor handling */
|
||||
if (pidProfile->acroPlusFactor && ABS(rcCommandDeflection) > acroPlusStickOffset + 10) {
|
||||
if (rcCommandDeflection > 0) {
|
||||
rcCommandDeflection -= acroPlusStickOffset;
|
||||
} else {
|
||||
rcCommandDeflection += acroPlusStickOffset;
|
||||
}
|
||||
qConstruct(&axisState->wowFactor,ABS(rcCommandDeflection) * pidProfile->acroPlusFactor / 100, 500, Q12_NUMBER);
|
||||
axisState->factor = qMultiply(axisState->wowFactor, rcCommandDeflection << 1); // Max factor 1000 on rcCommand of 500
|
||||
axisState->wowFactor.num = axisState->wowFactor.den - axisState->wowFactor.num;
|
||||
} else {
|
||||
qConstruct(&axisState->wowFactor, 1, 1, Q12_NUMBER);
|
||||
axisState->factor = 0;
|
||||
}
|
||||
}
|
||||
|
||||
const angle_index_t rcAliasToAngleIndexMap[] = { AI_ROLL, AI_PITCH };
|
||||
|
||||
static acroPlus_t acroPlusState[3];
|
||||
static biquad_t deltaBiQuadState[3];
|
||||
static bool deltaStateIsSet;
|
||||
|
||||
|
@ -264,11 +243,6 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
|
|||
// -----calculate total PID output
|
||||
axisPID[axis] = constrain(lrintf(PTerm + ITerm + DTerm), -1000, 1000);
|
||||
|
||||
if (IS_RC_MODE_ACTIVE(BOXACROPLUS) && axis != YAW) {
|
||||
acroPlusApply(&acroPlusState[axis], axis, pidProfile);
|
||||
axisPID[axis] = acroPlusState[axis].factor + qMultiply(acroPlusState[axis].wowFactor, axisPID[axis]);
|
||||
}
|
||||
|
||||
#ifdef GTUNE
|
||||
if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) {
|
||||
calculate_Gtune(axis);
|
||||
|
@ -380,11 +354,6 @@ static void pidMultiWii23(pidProfile_t *pidProfile, controlRateConfig_t *control
|
|||
|
||||
axisPID[axis] = PTerm + ITerm + DTerm;
|
||||
|
||||
if (IS_RC_MODE_ACTIVE(BOXACROPLUS) && axis != YAW) {
|
||||
acroPlusApply(&acroPlusState[axis], axis, pidProfile);
|
||||
axisPID[axis] = acroPlusState[axis].factor + qMultiply(acroPlusState[axis].wowFactor, axisPID[axis]);
|
||||
}
|
||||
|
||||
#ifdef GTUNE
|
||||
if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) {
|
||||
calculate_Gtune(axis);
|
||||
|
@ -554,11 +523,6 @@ static void pidMultiWiiRewrite(pidProfile_t *pidProfile, controlRateConfig_t *co
|
|||
// -----calculate total PID output
|
||||
axisPID[axis] = PTerm + ITerm + DTerm;
|
||||
|
||||
if (IS_RC_MODE_ACTIVE(BOXACROPLUS) && axis != YAW) {
|
||||
acroPlusApply(&acroPlusState[axis], axis, pidProfile);
|
||||
axisPID[axis] = acroPlusState[axis].factor + qMultiply(acroPlusState[axis].wowFactor, axisPID[axis]);
|
||||
}
|
||||
|
||||
#ifdef GTUNE
|
||||
if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) {
|
||||
calculate_Gtune(axis);
|
||||
|
|
|
@ -64,8 +64,6 @@ typedef struct pidProfile_s {
|
|||
float H_level;
|
||||
uint8_t H_sensitivity;
|
||||
|
||||
uint8_t acroPlusFactor; // Air mode acrobility factor
|
||||
uint8_t acroPlusOffset; // Air mode stick offset
|
||||
float dterm_lpf_hz; // Delta Filter in hz
|
||||
uint8_t deltaMethod; // Alternative delta Calculation
|
||||
uint16_t yaw_p_limit;
|
||||
|
@ -80,11 +78,6 @@ typedef struct pidProfile_s {
|
|||
#endif
|
||||
} pidProfile_t;
|
||||
|
||||
typedef struct acroPlus_s {
|
||||
int16_t factor;
|
||||
q_number_t wowFactor;
|
||||
} acroPlus_t;
|
||||
|
||||
extern int16_t axisPID[XYZ_AXIS_COUNT];
|
||||
extern int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3];
|
||||
bool antiWindupProtection;
|
||||
|
|
|
@ -644,6 +644,8 @@ const clivalue_t valueTable[] = {
|
|||
{ "yaw_rate", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].controlRateProfile.rates[FD_YAW], .config.minmax = { 0, CONTROL_RATE_CONFIG_YAW_RATE_MAX } },
|
||||
{ "tpa_rate", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].controlRateProfile.dynThrPID, .config.minmax = { 0, CONTROL_RATE_CONFIG_TPA_MAX} },
|
||||
{ "tpa_breakpoint", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].controlRateProfile.tpa_breakpoint, .config.minmax = { PWM_RANGE_MIN, PWM_RANGE_MAX} },
|
||||
{ "acro_plus_factor", VAR_UINT8 | PROFILE_VALUE, &masterConfig.rxConfig.acroPlusFactor, .config.minmax = {1, 100 } },
|
||||
{ "acro_plus_offset", VAR_UINT8 | PROFILE_VALUE, &masterConfig.rxConfig.acroPlusOffset, .config.minmax = {1, 90 } },
|
||||
|
||||
{ "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 } },
|
||||
|
@ -717,9 +719,6 @@ const clivalue_t valueTable[] = {
|
|||
{ "i_vel", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.I8[PIDVEL], .config.minmax = { 0, 200 } },
|
||||
{ "d_vel", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.D8[PIDVEL], .config.minmax = { 0, 200 } },
|
||||
|
||||
{ "acro_plus_factor", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.acroPlusFactor, .config.minmax = {1, 100 } },
|
||||
{ "acro_plus_offset", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.acroPlusOffset, .config.minmax = {1, 90 } },
|
||||
|
||||
#ifdef BLACKBOX
|
||||
{ "blackbox_rate_num", VAR_UINT8 | MASTER_VALUE, &masterConfig.blackbox_rate_num, .config.minmax = { 1, 32 } },
|
||||
{ "blackbox_rate_denom", VAR_UINT8 | MASTER_VALUE, &masterConfig.blackbox_rate_denom, .config.minmax = { 1, 32 } },
|
||||
|
|
|
@ -122,6 +122,8 @@ 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
|
||||
|
||||
uint16_t rx_min_usec;
|
||||
uint16_t rx_max_usec;
|
||||
|
|
Loading…
Reference in New Issue