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

Add Back Iterm limit for saturation scenarios

This commit is contained in:
borisbstyle 2016-06-01 20:58:00 +02:00
parent 704c09cced
commit a3c1f6e168
4 changed files with 27 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
src/main/flight/mixer.c
View File

@ -817,6 +817,7 @@ void mixTable(void)
throttleRange = throttleMax - throttleMin;
if (rollPitchYawMixRange > throttleRange) {
motorLimitReached = true;
mixReduction = qConstruct(throttleRange, rollPitchYawMixRange);
for (i = 0; i < motorCount; i++) {
@ -827,6 +828,7 @@ void mixTable(void)
if (debugMode == DEBUG_AIRMODE && i < 3) debug[1] = rollPitchYawMixRange;
} else {
motorLimitReached = false;
throttleMin = throttleMin + (rollPitchYawMixRange / 2);
throttleMax = throttleMax - (rollPitchYawMixRange / 2);
}
@ -857,11 +859,15 @@ void mixTable(void)
// Experimental Code. Anti Desync feature for ESC's
if (escAndServoConfig->escDesyncProtection) {
const int16_t maxThrottleStep = escAndServoConfig->escDesyncProtection / (1000 / targetPidLooptime);
static int16_t motorPrevious[MAX_SUPPORTED_MOTORS];
const int16_t maxThrottleStep = constrain(escAndServoConfig->escDesyncProtection / (1000 / targetPidLooptime), 5, 10000);
motor[i] = constrain(motor[i], motorPrevious[i] - maxThrottleStep, motorPrevious[i] + maxThrottleStep);
motorPrevious[i] = motor[i];
// Only makes sense when it's within the range
if (maxThrottleStep < throttleRange) {
static int16_t motorPrevious[MAX_SUPPORTED_MOTORS];
motor[i] = constrain(motor[i], motorPrevious[i] - maxThrottleStep, motorPrevious[i] + maxThrottleStep);
motorPrevious[i] = motor[i];
}
}
}

2
src/main/flight/mixer.h
View File

@ -189,7 +189,7 @@ void filterServos(void);
extern int16_t motor[MAX_SUPPORTED_MOTORS];
extern int16_t motor_disarmed[MAX_SUPPORTED_MOTORS];
bool motorLimitReached;
struct escAndServoConfig_s;
struct rxConfig_s;

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

@ -49,6 +49,7 @@
#include "config/runtime_config.h"
extern uint8_t motorCount;
extern bool motorLimitReached;
uint32_t targetPidLooptime;
int16_t axisPID[3];
@ -60,9 +61,9 @@ int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3];
// PIDweight is a scale factor for PIDs which is derived from the throttle and TPA setting, and 100 = 100% scale means no PID reduction
uint8_t PIDweight[3];
static int32_t errorGyroI[3];
static int32_t errorGyroI[3], errorGyroILimit[3];
#ifndef SKIP_PID_LUXFLOAT
static float errorGyroIf[3];
static float errorGyroIf[3], errorGyroIfLimit[3];;
#endif
static void pidMultiWiiRewrite(const pidProfile_t *pidProfile, const controlRateConfig_t *controlRateConfig,
@ -215,6 +216,12 @@ static void pidLuxFloat(const pidProfile_t *pidProfile, const controlRateConfig_
errorGyroIf[axis] = constrainf(errorGyroIf[axis] + luxITermScale * RateError * getdT() * kI, -250.0f, 250.0f);
if (motorLimitReached) {
errorGyroIf[axis] = constrainf(errorGyroIf[axis], -errorGyroIfLimit[axis], errorGyroIfLimit[axis]);
} else {
errorGyroIfLimit[axis] = ABS(errorGyroIf[axis]);
}
// limit maximum integrator value to prevent WindUp - accumulating extreme values when system is saturated.
// I coefficient (I8) moved before integration to make limiting independent from PID settings
ITerm = errorGyroIf[axis];
@ -341,6 +348,12 @@ static void pidMultiWiiRewrite(const pidProfile_t *pidProfile, const controlRate
// I coefficient (I8) moved before integration to make limiting independent from PID settings
errorGyroI[axis] = constrain(errorGyroI[axis], (int32_t) - GYRO_I_MAX << 13, (int32_t) + GYRO_I_MAX << 13);
if (motorLimitReached) {
errorGyroI[axis] = constrain(errorGyroI[axis], -errorGyroILimit[axis], errorGyroILimit[axis]);
} else {
errorGyroILimit[axis] = ABS(errorGyroI[axis]);
}
ITerm = errorGyroI[axis] >> 13;
//-----calculate D-term

2
src/main/io/serial_cli.c
View File

@ -574,7 +574,7 @@ const clivalue_t valueTable[] = {
{ "min_throttle", VAR_UINT16 | MASTER_VALUE, &masterConfig.escAndServoConfig.minthrottle, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } },
{ "max_throttle", VAR_UINT16 | MASTER_VALUE, &masterConfig.escAndServoConfig.maxthrottle, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } },
{ "min_command", VAR_UINT16 | MASTER_VALUE, &masterConfig.escAndServoConfig.mincommand, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } },
{ "anti_desync_power_step", VAR_UINT16 | MASTER_VALUE, &masterConfig.escAndServoConfig.escDesyncProtection, .config.minmax = { 0, 2000 } },
{ "anti_desync_power_step", VAR_UINT16 | MASTER_VALUE, &masterConfig.escAndServoConfig.escDesyncProtection, .config.minmax = { 0, 10000 } },
{ "servo_center_pulse", VAR_UINT16 | MASTER_VALUE, &masterConfig.escAndServoConfig.servoCenterPulse, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } },
{ "3d_deadband_low", VAR_UINT16 | MASTER_VALUE, &masterConfig.flight3DConfig.deadband3d_low, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } }, // FIXME upper limit should match code in the mixer, 1500 currently