rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Reset ITerm on motor reversal

This commit is contained in:
Martin Budden 2017-11-30 14:54:37 +00:00
parent ce345a8446
commit 9060ecb302
5 changed files with 20 additions and 4 deletions
Show Stats 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

@ -545,6 +545,10 @@ 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];
@ -555,6 +559,10 @@ 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;
@ -572,6 +580,10 @@ void calculateThrottleAndCurrentMotorEndpoints(void)
motorOutputMin = deadbandMotor3dLow;
motorOutputRange = motorOutputLow - deadbandMotor3dLow;
}
if (motorOutputMixSign != -1) {
// reset ITerm on motor reversal
pidResetITerm();
}
motorOutputMixSign = -1;
throttle = 0;
currentThrottleInputRange = rcCommandThrottleRange3dLow;
@ -581,6 +593,10 @@ 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) {}