mirror of
https://github.com/iNavFlight/inav.git
synced 2025-07-25 17:25:18 +03:00
>Drop yaw_p_limit and replace with pidsum_limit_yaq
This commit is contained in:
Pawel Spychalski (DzikuVx)
parent
80dc0e1d5a
commit
0f4a6cfb39
8 changed files with 30 additions and 35 deletions
|
|
@ -423,8 +423,8 @@ A shorter form is also supported to enable and disable functions using `serial <
|
|||
| dyn_notch_q | 120 | Q factor for dynamic notches |
|
||||
| dyn_notch_min_hz | 150 | Minimum frequency for dynamic notches. Default value of `150` works best with 5" multirors. Should be lowered with increased size of propellers. Values around `100` work fine on 7" drones. 10" can go down to `60` - `70` |
|
||||
| gyro_stage2_lowpass_hz | 0 | Software based second stage lowpass filter for gyro. Value is cutoff frequency (Hz). Currently experimental |
|
||||
| pidsum_limit | 500 | A limitation to overall amount of correction Flight PID can request on each axis (Roll/Pitch/Yaw). If when doing a hard maneuver on one axis machine looses orientation on other axis - reducing this parameter may help |
|
||||
| yaw_p_limit | 300 | |
|
||||
| pidsum_limit | 500 | A limitation to overall amount of correction Flight PID can request on each axis (Roll/Pitch). If when doing a hard maneuver on one axis machine looses orientation on other axis - reducing this parameter may help |
|
||||
| pidsum_limit_yaw | 400 | A limitation to overall amount of correction Flight PID can request on each axis (Yaw). If when doing a hard maneuver on one axis machine looses orientation on other axis - reducing this parameter may help |
|
||||
| iterm_windup | 50 | Used to prevent Iterm accumulation on during maneuvers. Iterm will be dampened when motors are reaching it's limit (when requested motor correction range is above percentage specified by this parameter) |
|
||||
| rate_accel_limit_roll_pitch | 0 | Limits acceleration of ROLL/PITCH rotation speed that can be requested by stick input. In degrees-per-second-squared. Small and powerful UAV flies great with high acceleration limit ( > 5000 dps^2 and even > 10000 dps^2). Big and heavy multirotors will benefit from low acceleration limit (~ 360 dps^2). When set correctly, it greatly improves stopping performance. Value of 0 disables limiting. |
|
||||
| rate_accel_limit_yaw | 10000 | Limits acceleration of YAW rotation speed that can be requested by stick input. In degrees-per-second-squared. Small and powerful UAV flies great with high acceleration limit ( > 10000 dps^2). Big and heavy multirotors will benefit from low acceleration limit (~ 180 dps^2). When set correctly, it greatly improves stopping performance and general stability during yaw turns. Value of 0 disables limiting. |
|
||||
|
|
|
|||
|
|
@ -1679,7 +1679,6 @@ static bool blackboxWriteSysinfo(void)
|
|||
BLACKBOX_PRINT_HEADER_LINE("velPID", "%d,%d,%d", pidBank()->pid[PID_VEL_Z].P,
|
||||
pidBank()->pid[PID_VEL_Z].I,
|
||||
pidBank()->pid[PID_VEL_Z].D);
|
||||
BLACKBOX_PRINT_HEADER_LINE("yaw_p_limit", "%d", pidProfile()->yaw_p_limit);
|
||||
BLACKBOX_PRINT_HEADER_LINE("yaw_lpf_hz", "%d", pidProfile()->yaw_lpf_hz);
|
||||
BLACKBOX_PRINT_HEADER_LINE("dterm_lpf_hz", "%d", pidProfile()->dterm_lpf_hz);
|
||||
BLACKBOX_PRINT_HEADER_LINE("dterm_notch_hz", "%d", pidProfile()->dterm_soft_notch_hz);
|
||||
|
|
@ -1709,6 +1708,7 @@ static bool blackboxWriteSysinfo(void)
|
|||
BLACKBOX_PRINT_HEADER_LINE("gyro_stage2_lowpass_hz", "%d", gyroConfig()->gyro_stage2_lowpass_hz);
|
||||
BLACKBOX_PRINT_HEADER_LINE("dterm_setpoint_weight", "%f", (double)pidProfile()->dterm_setpoint_weight);
|
||||
BLACKBOX_PRINT_HEADER_LINE("pidSumLimit", "%d", pidProfile()->pidSumLimit);
|
||||
BLACKBOX_PRINT_HEADER_LINE("pidSumLimitYaw", "%d", pidProfile()->pidSumLimitYaw);
|
||||
BLACKBOX_PRINT_HEADER_LINE("axisAccelerationLimitYaw", "%d", pidProfile()->axisAccelerationLimitYaw);
|
||||
BLACKBOX_PRINT_HEADER_LINE("axisAccelerationLimitRollPitch", "%d", pidProfile()->axisAccelerationLimitRollPitch);
|
||||
default:
|
||||
|
|
|
|||
|
|
@ -404,7 +404,7 @@ static const OSD_Entry cmsx_menuFilterPerProfileEntries[] =
|
|||
|
||||
OSD_SETTING_ENTRY("DTERM LPF", SETTING_DTERM_LPF_HZ),
|
||||
OSD_SETTING_ENTRY("GYRO SLPF", SETTING_GYRO_LPF_HZ),
|
||||
OSD_SETTING_ENTRY("YAW P LIM", SETTING_YAW_P_LIMIT),
|
||||
OSD_SETTING_ENTRY("YAW SUM LIM", SETTING_PIDSUM_LIMIT_YAW),
|
||||
OSD_SETTING_ENTRY("YAW LPF", SETTING_YAW_LPF_HZ),
|
||||
|
||||
OSD_BACK_AND_END_ENTRY,
|
||||
|
|
|
|||
|
|
@ -1143,7 +1143,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
|
|||
case MSP_PID_ADVANCED:
|
||||
sbufWriteU16(dst, 0); // pidProfile()->rollPitchItermIgnoreRate
|
||||
sbufWriteU16(dst, 0); // pidProfile()->yawItermIgnoreRate
|
||||
sbufWriteU16(dst, pidProfile()->yaw_p_limit);
|
||||
sbufWriteU16(dst, 0); //pidProfile()->yaw_p_limit
|
||||
sbufWriteU8(dst, 0); //BF: pidProfile()->deltaMethod
|
||||
sbufWriteU8(dst, 0); //BF: pidProfile()->vbatPidCompensation
|
||||
sbufWriteU8(dst, 0); //BF: pidProfile()->setpointRelaxRatio
|
||||
|
|
@ -2062,7 +2062,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
|
|||
if (dataSize >= 17) {
|
||||
sbufReadU16(src); // pidProfileMutable()->rollPitchItermIgnoreRate
|
||||
sbufReadU16(src); // pidProfileMutable()->yawItermIgnoreRate
|
||||
pidProfileMutable()->yaw_p_limit = sbufReadU16(src);
|
||||
sbufReadU16(src); //pidProfile()->yaw_p_limit
|
||||
|
||||
sbufReadU8(src); //BF: pidProfileMutable()->deltaMethod
|
||||
sbufReadU8(src); //BF: pidProfileMutable()->vbatPidCompensation
|
||||
|
|
|
|||
|
|
@ -1081,9 +1081,10 @@ groups:
|
|||
field: pidSumLimit
|
||||
min: PID_SUM_LIMIT_MIN
|
||||
max: PID_SUM_LIMIT_MAX
|
||||
- name: yaw_p_limit
|
||||
min: YAW_P_LIMIT_MIN
|
||||
max: YAW_P_LIMIT_MAX
|
||||
- name: pidsum_limit_yaw
|
||||
field: pidSumLimitYaw
|
||||
min: PID_SUM_LIMIT_MIN
|
||||
max: PID_SUM_LIMIT_MAX
|
||||
- name: iterm_windup
|
||||
field: itermWindupPointPercent
|
||||
min: 0
|
||||
|
|
|
|||
|
|
@ -94,6 +94,7 @@ typedef struct {
|
|||
pt1Filter_t dBoostLpf;
|
||||
biquadFilter_t dBoostGyroLpf;
|
||||
#endif
|
||||
uint16_t pidSumLimit;
|
||||
} pidState_t;
|
||||
|
||||
#ifdef USE_DTERM_NOTCH
|
||||
|
|
@ -136,10 +137,9 @@ static EXTENDED_FASTRAM float dBoostFactor;
|
|||
static EXTENDED_FASTRAM float dBoostMaxAtAlleceleration;
|
||||
#endif
|
||||
|
||||
static EXTENDED_FASTRAM uint16_t yawPLimit;
|
||||
static EXTENDED_FASTRAM uint8_t yawLpfHz;
|
||||
|
||||
PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 10);
|
||||
PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 11);
|
||||
|
||||
PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
|
||||
.bank_mc = {
|
||||
|
|
@ -220,13 +220,12 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
|
|||
.axisAccelerationLimitYaw = 10000, // dps/s
|
||||
.axisAccelerationLimitRollPitch = 0, // dps/s
|
||||
|
||||
.yaw_p_limit = YAW_P_LIMIT_DEFAULT,
|
||||
|
||||
.heading_hold_rate_limit = HEADING_HOLD_RATE_LIMIT_DEFAULT,
|
||||
|
||||
.max_angle_inclination[FD_ROLL] = 300, // 30 degrees
|
||||
.max_angle_inclination[FD_PITCH] = 300, // 30 degrees
|
||||
.pidSumLimit = PID_SUM_LIMIT_DEFAULT,
|
||||
.pidSumLimitYaw = PID_SUM_LIMIT_YAW_DEFAULT,
|
||||
|
||||
.fixedWingItermThrowLimit = FW_ITERM_THROW_LIMIT_DEFAULT,
|
||||
.fixedWingReferenceAirspeed = 1000,
|
||||
|
|
@ -260,11 +259,6 @@ void pidInit(void)
|
|||
itermRelaxType = pidProfile()->iterm_relax_type;
|
||||
itermRelaxSetpointThreshold = MC_ITERM_RELAX_SETPOINT_THRESHOLD * MC_ITERM_RELAX_CUTOFF_DEFAULT / pidProfile()->iterm_relax_cutoff;
|
||||
|
||||
if (mixerConfig()->platformType == PLATFORM_MULTIROTOR) {
|
||||
yawPLimit = pidProfile()->yaw_p_limit;
|
||||
} else {
|
||||
yawPLimit = 0;
|
||||
}
|
||||
yawLpfHz = pidProfile()->yaw_lpf_hz;
|
||||
|
||||
#ifdef USE_D_BOOST
|
||||
|
|
@ -276,6 +270,14 @@ void pidInit(void)
|
|||
antigravityGain = pidProfile()->antigravityGain;
|
||||
antigravityAccelerator = pidProfile()->antigravityAccelerator;
|
||||
#endif
|
||||
|
||||
for (uint8_t axis = FD_ROLL; axis <= FD_YAW; axis++) {
|
||||
if (axis == FD_YAW) {
|
||||
pidState[axis].pidSumLimit = pidProfile()->pidSumLimitYaw;
|
||||
} else {
|
||||
pidState[axis].pidSumLimit = pidProfile()->pidSumLimit;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bool pidInitFilters(void)
|
||||
|
|
@ -593,11 +595,6 @@ static FAST_CODE NOINLINE float pTermProcess(pidState_t *pidState, flight_dynami
|
|||
float newPTerm = rateError * pidState->kP;
|
||||
|
||||
if (axis == FD_YAW) {
|
||||
// Constrain YAW by yaw_p_limit value if not servo driven (in that case servo limits apply)
|
||||
if (yawPLimit) {
|
||||
newPTerm = constrain(newPTerm, -yawPLimit, yawPLimit);
|
||||
}
|
||||
|
||||
if (yawLpfHz) {
|
||||
newPTerm = pt1FilterApply4(&pidState->ptermLpfState, newPTerm, yawLpfHz, dT);
|
||||
}
|
||||
|
|
@ -631,7 +628,7 @@ static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, fligh
|
|||
}
|
||||
#endif
|
||||
|
||||
axisPID[axis] = constrainf(newPTerm + newFFTerm + pidState->errorGyroIf, -pidProfile()->pidSumLimit, +pidProfile()->pidSumLimit);
|
||||
axisPID[axis] = constrainf(newPTerm + newFFTerm + pidState->errorGyroIf, -pidState->pidSumLimit, +pidState->pidSumLimit);
|
||||
|
||||
#ifdef USE_BLACKBOX
|
||||
axisPID_P[axis] = newPTerm;
|
||||
|
|
@ -741,7 +738,7 @@ static void FAST_CODE pidApplyMulticopterRateController(pidState_t *pidState, fl
|
|||
|
||||
// TODO: Get feedback from mixer on available correction range for each axis
|
||||
const float newOutput = newPTerm + newDTerm + pidState->errorGyroIf;
|
||||
const float newOutputLimited = constrainf(newOutput, -pidProfile()->pidSumLimit, +pidProfile()->pidSumLimit);
|
||||
const float newOutputLimited = constrainf(newOutput, -pidState->pidSumLimit, +pidState->pidSumLimit);
|
||||
|
||||
// Prevent strong Iterm accumulation during stick inputs
|
||||
const float motorItermWindupPoint = 1.0f - (pidProfile()->itermWindupPointPercent / 100.0f);
|
||||
|
|
|
|||
|
|
@ -24,9 +24,7 @@
|
|||
#define PID_SUM_LIMIT_MIN 100
|
||||
#define PID_SUM_LIMIT_MAX 1000
|
||||
#define PID_SUM_LIMIT_DEFAULT 500
|
||||
#define YAW_P_LIMIT_MIN 100 // Maximum value for yaw P limiter
|
||||
#define YAW_P_LIMIT_MAX 500 // Maximum value for yaw P limiter
|
||||
#define YAW_P_LIMIT_DEFAULT 300 // Default value for yaw P limiter
|
||||
#define PID_SUM_LIMIT_YAW_DEFAULT 400
|
||||
|
||||
#define HEADING_HOLD_RATE_LIMIT_MIN 10
|
||||
#define HEADING_HOLD_RATE_LIMIT_MAX 250
|
||||
|
|
@ -109,7 +107,6 @@ typedef struct pidProfile_s {
|
|||
uint8_t use_dterm_fir_filter; // Use classical INAV FIR differentiator. Very noise robust, can be quite slowish
|
||||
|
||||
uint8_t yaw_lpf_hz;
|
||||
uint16_t yaw_p_limit;
|
||||
|
||||
uint8_t heading_hold_rate_limit; // Maximum rotation rate HEADING_HOLD mode can feed to yaw rate PID controller
|
||||
|
||||
|
|
@ -122,6 +119,7 @@ typedef struct pidProfile_s {
|
|||
|
||||
float dterm_setpoint_weight;
|
||||
uint16_t pidSumLimit;
|
||||
uint16_t pidSumLimitYaw;
|
||||
|
||||
// Airplane-specific parameters
|
||||
uint16_t fixedWingItermThrowLimit;
|
||||
|
|
|
|||
|
|
@ -172,7 +172,6 @@ void targetConfiguration(void)
|
|||
pidProfileMutable()->dterm_soft_notch_hz = 0;
|
||||
pidProfileMutable()->dterm_soft_notch_cutoff = 1;
|
||||
pidProfileMutable()->pidSumLimit = 500;
|
||||
pidProfileMutable()->yaw_p_limit = 300;
|
||||
pidProfileMutable()->axisAccelerationLimitRollPitch = 0;
|
||||
pidProfileMutable()->axisAccelerationLimitYaw = 10000;
|
||||
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue