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

Rework maxing out motors

Browse source
This commit is contained in:
borisbstyle 2015-12-16 17:27:36 +01:00
parent fdd984bc32
commit 83e932d3a3
2 changed files with 10 additions and 19 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -69,7 +69,7 @@ static rxConfig_t *rxConfig;
static mixerMode_e currentMixerMode;
static motorMixer_t currentMixer[MAX_SUPPORTED_MOTORS];
float totalErrorRatioLimit = 1.0f;
bool motorLimitReached = false;
#ifdef USE_SERVOS
static uint8_t servoRuleCount = 0;
@ -751,7 +751,7 @@ void mixTable(void)
}
if (!(IS_RC_MODE_ACTIVE(BOXAIRMODE)) && !(feature(FEATURE_3D))) {
totalErrorRatioLimit = 1.0f; // It always needs to be full ratio so it can't get stuck when flipping back and fourth
motorLimitReached = false; // It always needs to be reset so it can't get stuck when flipping back and fourth
// motors for non-servo mixes
for (i = 0; i < motorCount; i++) {
motor[i] =
@ -761,6 +761,7 @@ void mixTable(void)
-mixerConfig->yaw_motor_direction * axisPID[YAW] * currentMixer[i].yaw;
}
} else {
// Initial mixer concept by bdoiron74 reused and optimized for Air Mode
int16_t rollPitchYawMix[MAX_SUPPORTED_MOTORS];
int16_t rollPitchYawMixMax = 0; // assumption: symetrical about zero.
int16_t rollPitchYawMixMin = 0;
@ -782,25 +783,23 @@ void mixTable(void)
int16_t throttleMin, throttleMax;
if (rollPitchYawMixRange > throttleRange) {
motorLimitReached = true;
for (i = 0; i < motorCount; i++) {
rollPitchYawMix[i] = (rollPitchYawMix[i] * throttleRange) / rollPitchYawMixRange;
}
throttleMin = throttleMax = escAndServoConfig->minthrottle + (throttleRange / 2);
throttleMin = escAndServoConfig->minthrottle;
throttleMax = escAndServoConfig->maxthrottle;
} else {
motorLimitReached = false;
throttleMin = escAndServoConfig->minthrottle + (rollPitchYawMixRange / 2);
throttleMax = escAndServoConfig->maxthrottle - (rollPitchYawMixRange / 2);
}
// Now add in the desired throttle, but keep in a range that doesn't clip adjusted
// roll/pitch/yaw. This could move throttle down, but also up for those low throttle flips.
//
// TODO: handle the case when motors don't all get the same throttle factor...
for (i = 0; i < motorCount; i++) {
motor[i] = rollPitchYawMix[i] + constrainf(rcCommand[THROTTLE] * currentMixer[i].throttle, throttleMin, throttleMax);
}
// adjust feedback to scale PID error inputs to our limitations.
totalErrorRatioLimit = constrainf(((float)throttleRange / rollPitchYawMixRange), 0.4f, 1.0f);
}
if (ARMING_FLAG(ARMED)) {

14
src/main/flight/pid.c
View file

@ -48,7 +48,7 @@
#include "config/runtime_config.h"
extern float dT;
extern float totalErrorRatioLimit;
extern bool motorLimitReached;
extern bool allowITermShrinkOnly;
int16_t axisPID[3];
@ -161,10 +161,6 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
// multiplication of rcCommand corresponds to changing the sticks scaling here
RateError = AngleRate - gyroRate;
if (IS_RC_MODE_ACTIVE(BOXAIRMODE)) {
RateError = RateError * totalErrorRatioLimit;
}
// -----calculate P component
PTerm = RateError * pidProfile->P_f[axis] * PIDweight[axis] / 100;
@ -175,7 +171,7 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
// -----calculate I component.
errorGyroIf[axis] = constrainf(errorGyroIf[axis] + RateError * dT * pidProfile->I_f[axis] * 10, -250.0f, 250.0f);
if (allowITermShrinkOnly || totalErrorRatioLimit < 0.98f) {
if (allowITermShrinkOnly || motorLimitReached) {
if (ABS(errorGyroIf[axis]) < ABS(previousErrorGyroIf[axis])) {
previousErrorGyroIf[axis] = errorGyroIf[axis];
} else {
@ -302,10 +298,6 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
// multiplication of rcCommand corresponds to changing the sticks scaling here
RateError = AngleRateTmp - (gyroADC[axis] / 4);
if (IS_RC_MODE_ACTIVE(BOXAIRMODE)) {
RateError = RateError * totalErrorRatioLimit;
}
// -----calculate P component
PTerm = (RateError * pidProfile->P8[axis] * PIDweight[axis] / 100) >> 7;
@ -326,7 +318,7 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
ITerm = errorGyroI[axis] >> 13;
if (allowITermShrinkOnly || totalErrorRatioLimit < 0.98f) {
if (allowITermShrinkOnly || motorLimitReached) {
if (ABS(errorGyroI[axis]) < ABS(previousErrorGyroI[axis])) {
previousErrorGyroI[axis] = errorGyroI[axis];
} else {