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

Overshoot logic updates, disable for GPS Rescue, constrain max setpoint 

Updates for finalize the flight performance.

Disable if GPS Rescue is active

Constrain the max setpoint calculated by the angle correction logic. To catch border cases where the user activates the feature while inverted and has a high gain which could result in excessively high setpoint rates.

Make sure internal states are reset when feature is activated via mode switch.
This commit is contained in:
Bruce Luckcuck 2018-05-27 21:09:38 -04:00
parent 2384088855
commit ad067ee09a
1 changed files with 10 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
src/main/flight/pid.c
View File

@ -89,8 +89,8 @@ PG_RESET_TEMPLATE(pidConfig_t, pidConfig,
#endif
#ifdef USE_ACRO_TRAINER
#define ACRO_TRAINER_LOOKAHEAD_MIN_TIME 0.01f
#define ACRO_TRAINER_LOOKAHEAD_RATE_LIMIT 500.0f // Max gyro rate for lookahead time scaling
#define ACRO_TRAINER_LOOKAHEAD_RATE_LIMIT 500.0f // Max gyro rate for lookahead time scaling
#define ACRO_TRAINER_SETPOINT_LIMIT 1000.0f // Limit the correcting setpoint
#endif // USE_ACRO_TRAINER
PG_REGISTER_ARRAY_WITH_RESET_FN(pidProfile_t, MAX_PROFILE_COUNT, pidProfiles, PG_PID_PROFILE, 3);
@ -599,7 +599,7 @@ static FAST_CODE_NOINLINE float applyAcroTrainer(int axis, const rollAndPitchTri
{
float ret = setPoint;
if (!FLIGHT_MODE(ANGLE_MODE) && !FLIGHT_MODE(HORIZON_MODE)) {
if (!FLIGHT_MODE(ANGLE_MODE) && !FLIGHT_MODE(HORIZON_MODE) && !FLIGHT_MODE(GPS_RESCUE_MODE)) {
bool resetIterm = false;
float projectedAngle = 0;
const int setpointSign = acroTrainerSign(setPoint);
@ -619,7 +619,7 @@ static FAST_CODE_NOINLINE float applyAcroTrainer(int axis, const rollAndPitchTri
}
if (acroTrainerAxisState[axis] != 0) {
ret = ((acroTrainerAngleLimit * angleSign) - currentAngle) * acroTrainerGain;
ret = constrainf(((acroTrainerAngleLimit * angleSign) - currentAngle) * acroTrainerGain, -ACRO_TRAINER_SETPOINT_LIMIT, ACRO_TRAINER_SETPOINT_LIMIT);
} else {
// Not currently over the limit so project the angle based on current angle and
@ -627,7 +627,6 @@ static FAST_CODE_NOINLINE float applyAcroTrainer(int axis, const rollAndPitchTri
// If the projected angle exceeds the limit then apply limiting to minimize overshoot.
// Calculate the lookahead window by scaling proportionally with gyro rate from 0-500dps
float checkInterval = constrainf(fabsf(gyro.gyroADCf[axis]) / ACRO_TRAINER_LOOKAHEAD_RATE_LIMIT, 0.0f, 1.0f) * acroTrainerLookaheadTime;
checkInterval = MAX(checkInterval, ACRO_TRAINER_LOOKAHEAD_MIN_TIME);
projectedAngle = (gyro.gyroADCf[axis] * checkInterval) + currentAngle;
const int projectedAngleSign = acroTrainerSign(projectedAngle);
if ((fabsf(projectedAngle) > acroTrainerAngleLimit) && (projectedAngleSign == setpointSign)) {
@ -840,6 +839,11 @@ bool crashRecoveryModeActive(void)
#ifdef USE_ACRO_TRAINER
void pidSetAcroTrainerState(bool newState)
{
acroTrainerActive = newState;
if (acroTrainerActive != newState) {
if (newState) {
pidAcroTrainerInit();
}
acroTrainerActive = newState;
}
}
#endif // USE_ACRO_TRAINER