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

Merge pull request #4702 from martinbudden/bfa_3d_iterm

Browse source 
Reset ITerm on motor reversal
This commit is contained in:
Martin Budden 2017-12-01 08:16:22 +00:00 • committed by GitHub
commit dd3a40f036
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
5 changed files with 20 additions and 4 deletions
Download patch file Download diff file Expand all files Collapse all files

2
src/main/fc/fc_core.c
View file

@ -421,7 +421,7 @@ void processRx(timeUs_t currentTimeUs)
/* In airmode Iterm should be prevented to grow when Low thottle and Roll + Pitch Centered.
This is needed to prevent Iterm winding on the ground, but keep full stabilisation on 0 throttle while in air */
if (throttleStatus == THROTTLE_LOW && !airmodeIsActivated) {
pidResetErrorGyroState();
pidResetITerm();
if (currentPidProfile->pidAtMinThrottle)
pidStabilisationState(PID_STABILISATION_ON);
else

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

@ -541,6 +541,10 @@ static void calculateThrottleAndCurrentMotorEndpoints(void)
motorOutputMin = deadbandMotor3dLow;
motorOutputRange = motorOutputLow - deadbandMotor3dLow;
}
if (motorOutputMixSign != -1) {
// reset ITerm on motor reversal
pidResetITerm();
}
motorOutputMixSign = -1;
rcThrottlePrevious = rcCommand[THROTTLE];
throttle = rcCommand3dDeadBandLow - rcCommand[THROTTLE];
@ -551,6 +555,10 @@ static void calculateThrottleAndCurrentMotorEndpoints(void)
motorRangeMax = motorOutputHigh;
motorOutputMin = deadbandMotor3dHigh;
motorOutputRange = motorOutputHigh - deadbandMotor3dHigh;
if (motorOutputMixSign != 1) {
// reset ITerm on motor reversal
pidResetITerm();
}
motorOutputMixSign = 1;
rcThrottlePrevious = rcCommand[THROTTLE];
throttle = rcCommand[THROTTLE] - rcCommand3dDeadBandHigh;
@ -568,6 +576,10 @@ static void calculateThrottleAndCurrentMotorEndpoints(void)
motorOutputMin = deadbandMotor3dLow;
motorOutputRange = motorOutputLow - deadbandMotor3dLow;
}
if (motorOutputMixSign != -1) {
// reset ITerm on motor reversal
pidResetITerm();
}
motorOutputMixSign = -1;
throttle = 0;
currentThrottleInputRange = rcCommandThrottleRange3dLow;
@ -577,6 +589,10 @@ static void calculateThrottleAndCurrentMotorEndpoints(void)
motorRangeMax = motorOutputHigh;
motorOutputMin = deadbandMotor3dHigh;
motorOutputRange = motorOutputHigh - deadbandMotor3dHigh;
if (motorOutputMixSign != 1) {
// reset ITerm on motor reversal
pidResetITerm();
}
motorOutputMixSign = 1;
throttle = 0;
currentThrottleInputRange = rcCommandThrottleRange3dHigh;

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

@ -136,7 +136,7 @@ static void pidSetTargetLooptime(uint32_t pidLooptime)
dT = (float)targetPidLooptime * 0.000001f;
}
void pidResetErrorGyroState(void)
void pidResetITerm(void)
{
for (int axis = 0; axis < 3; axis++) {
axisPID_I[axis] = 0.0f;

2
src/main/flight/pid.h
View file

@ -127,7 +127,7 @@ extern uint32_t targetPidLooptime;
// PIDweight is a scale factor for PIDs which is derived from the throttle and TPA setting, and 100 = 100% scale means no PID reduction
extern uint8_t PIDweight[3];
void pidResetErrorGyroState(void);
void pidResetITerm(void);
void pidStabilisationState(pidStabilisationState_e pidControllerState);
void pidSetItermAccelerator(float newItermAccelerator);
void pidInitFilters(const pidProfile_t *pidProfile);

2
src/test/unit/arming_prevention_unittest.cc
View file

@ -642,7 +642,7 @@ extern "C" {
void failsafeStartMonitoring(void) {}
void failsafeUpdateState(void) {}
bool failsafeIsActive(void) { return false; }
void pidResetErrorGyroState(void) {}
void pidResetITerm(void) {}
void updateAdjustmentStates(void) {}
void processRcAdjustments(controlRateConfig_t *) {}
void updateGpsWaypointsAndMode(void) {}