Merge remote-tracking branch 'borisbstyle/betaflight' into betaflight
This commit is contained in:
commit
2cc6262776
|
@ -16,7 +16,8 @@ targets=("PUBLISHMETA=True" \
|
|||
"TARGET=IRCFUSIONF3" \
|
||||
"TARGET=ALIENFLIGHTF1" \
|
||||
"TARGET=ALIENFLIGHTF3" \
|
||||
"TARGET=DOGE")
|
||||
"TARGET=DOGE" \
|
||||
"TARGET=SINGULARITY")
|
||||
|
||||
#fake a travis build environment
|
||||
export TRAVIS_BUILD_NUMBER=$(date +%s)
|
||||
|
|
|
@ -74,16 +74,15 @@ void setTargetPidLooptime(uint8_t pidProcessDenom) {
|
|||
targetPidLooptime = targetLooptime * pidProcessDenom;
|
||||
}
|
||||
|
||||
float calculateExpoPlus(int axis, const rxConfig_t *rxConfig) {
|
||||
float propFactor;
|
||||
float superExpoFactor;
|
||||
float calculateExpoPlus(int axis, const rxConfig_t *rxConfig, const controlRateConfig_t *controlRateConfig) {
|
||||
float propFactor, superExpoFactor, rcFactor;
|
||||
|
||||
if (axis == YAW && !rxConfig->superExpoYawMode) {
|
||||
propFactor = 1.0f;
|
||||
} else {
|
||||
float rcFactor = (ABS(rcCommand[axis]) / 500.0f);
|
||||
|
||||
superExpoFactor = (axis == YAW) ? rxConfig->superExpoFactorYaw : rxConfig->superExpoFactor;
|
||||
rcFactor = (axis == YAW) ? (ABS(rcCommand[axis]) / 500.0f) : (ABS(rcCommand[axis]) / (500.0f * (controlRateConfig->rcRate8 / 100.0f)));
|
||||
|
||||
propFactor = constrainf(1.0f - ((superExpoFactor / 100.0f) * rcFactor * rcFactor * rcFactor), 0.0f, 1.0f);
|
||||
}
|
||||
|
||||
|
@ -210,7 +209,7 @@ static void pidLuxFloat(const pidProfile_t *pidProfile, const controlRateConfig_
|
|||
|
||||
// -----calculate P component
|
||||
if ((isSuperExpoActive() && axis != YAW) || (axis == YAW && rxConfig->superExpoYawMode == SUPEREXPO_YAW_ALWAYS)) {
|
||||
PTerm = (luxPTermScale * kP * tpaFactor) * (AngleRate - gyroRate * calculateExpoPlus(axis, rxConfig));
|
||||
PTerm = (luxPTermScale * kP * tpaFactor) * (AngleRate - gyroRate * calculateExpoPlus(axis, rxConfig, controlRateConfig));
|
||||
} else {
|
||||
PTerm = luxPTermScale * RateError * kP * tpaFactor;
|
||||
}
|
||||
|
@ -335,7 +334,7 @@ static void pidMultiWiiRewrite(const pidProfile_t *pidProfile, const controlRate
|
|||
|
||||
// -----calculate P component
|
||||
if ((isSuperExpoActive() && axis != YAW) || (axis == YAW && rxConfig->superExpoYawMode == SUPEREXPO_YAW_ALWAYS)) {
|
||||
PTerm = (kP * PIDweight[axis] / 100) * (AngleRateTmp - (int32_t)(gyroRate * calculateExpoPlus(axis, rxConfig))) >> 7;
|
||||
PTerm = (kP * PIDweight[axis] / 100) * (AngleRateTmp - (int32_t)(gyroRate * calculateExpoPlus(axis, rxConfig, controlRateConfig))) >> 7;
|
||||
} else {
|
||||
PTerm = (RateError * kP * PIDweight[axis] / 100) >> 7;
|
||||
}
|
||||
|
|
|
@ -14,6 +14,7 @@ ALL_TARGETS += rmdo
|
|||
ALL_TARGETS += ircfusionf3
|
||||
ALL_TARGETS += afromini
|
||||
ALL_TARGETS += doge
|
||||
ALL_TARGETS += singularity
|
||||
|
||||
CLEAN_TARGETS := $(addprefix clean_, $(ALL_TARGETS))
|
||||
|
||||
|
@ -33,6 +34,7 @@ clean_rmdo rmdo : opts := TARGET=RMDO
|
|||
clean_ircfusionf3 ircfusionf3 : opts := TARGET=IRCFUSIONF3
|
||||
clean_afromini afromini : opts := TARGET=AFROMINI
|
||||
clean_doge doge : opts := TARGET=DOGE
|
||||
clean_singularity singularity : opts := TARGET=SINGULARITY
|
||||
|
||||
|
||||
.PHONY: all clean
|
||||
|
|
Loading…
Reference in New Issue