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

Move acroplus to mixer

Browse source
This commit is contained in:
borisbstyle 2016-02-19 21:00:16 +01:00
parent fa391507b6
commit 293820f567
6 changed files with 40 additions and 49 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -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]));