Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-20 06:45:16 +03:00
Code Issues Wiki Activity
betaflight/src/main/flight/pid_init.c
KarateBrot 48f14686d5 Add biquad crossfading 
+ RPM filter fix (oscillations on throttle chops)
2021-06-27 23:46:45 +02:00

446 lines
18 KiB
C
Raw Blame History

/*
* This file is part of Cleanflight and Betaflight.
*
* Cleanflight and Betaflight are free software. You can redistribute
* this software and/or modify this software under the terms of the
* GNU General Public License as published by the Free Software
* Foundation, either version 3 of the License, or (at your option)
* any later version.
*
* Cleanflight and Betaflight are distributed in the hope that they
* will be useful, but WITHOUT ANY WARRANTY; without even the implied
* warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
* See the GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this software.
*
* If not, see <http://www.gnu.org/licenses/>.
*/
#include <stdbool.h>
#include <stdint.h>
#include <string.h>
#include <math.h>
#include "platform.h"
#include "build/build_config.h"
#include "build/debug.h"
#include "common/axis.h"
#include "common/filter.h"
#include "drivers/dshot_command.h"
#include "fc/rc_controls.h"
#include "fc/runtime_config.h"
#include "flight/feedforward.h"
#include "flight/pid.h"
#include "flight/rpm_filter.h"
#include "sensors/gyro.h"
#include "sensors/sensors.h"
#include "pid_init.h"
#if defined(USE_D_MIN)
#define D_MIN_RANGE_HZ 80 // Biquad lowpass input cutoff to peak D around propwash frequencies
#define D_MIN_LOWPASS_HZ 10 // PT1 lowpass cutoff to smooth the boost effect
#define D_MIN_GAIN_FACTOR 0.00005f
#define D_MIN_SETPOINT_GAIN_FACTOR 0.00005f
#endif
#define ANTI_GRAVITY_THROTTLE_FILTER_CUTOFF 15 // The anti gravity throttle highpass filter cutoff
#define ANTI_GRAVITY_SMOOTH_FILTER_CUTOFF 3 // The anti gravity P smoothing filter cutoff
static void pidSetTargetLooptime(uint32_t pidLooptime)
{
targetPidLooptime = pidLooptime;
pidRuntime.dT = targetPidLooptime * 1e-6f;
pidRuntime.pidFrequency = 1.0f / pidRuntime.dT;
#ifdef USE_DSHOT
dshotSetPidLoopTime(targetPidLooptime);
#endif
}
void pidInitFilters(const pidProfile_t *pidProfile)
{
STATIC_ASSERT(FD_YAW == 2, FD_YAW_incorrect); // ensure yaw axis is 2
if (targetPidLooptime == 0) {
// no looptime set, so set all the filters to null
pidRuntime.dtermNotchApplyFn = nullFilterApply;
pidRuntime.dtermLowpassApplyFn = nullFilterApply;
pidRuntime.dtermLowpass2ApplyFn = nullFilterApply;
pidRuntime.ptermYawLowpassApplyFn = nullFilterApply;
return;
}
const uint32_t pidFrequencyNyquist = pidRuntime.pidFrequency / 2; // No rounding needed
uint16_t dTermNotchHz;
if (pidProfile->dterm_notch_hz <= pidFrequencyNyquist) {
dTermNotchHz = pidProfile->dterm_notch_hz;
} else {
if (pidProfile->dterm_notch_cutoff < pidFrequencyNyquist) {
dTermNotchHz = pidFrequencyNyquist;
} else {
dTermNotchHz = 0;
}
}
if (dTermNotchHz != 0 && pidProfile->dterm_notch_cutoff != 0) {
pidRuntime.dtermNotchApplyFn = (filterApplyFnPtr)biquadFilterApply;
const float notchQ = filterGetNotchQ(dTermNotchHz, pidProfile->dterm_notch_cutoff);
for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
biquadFilterInit(&pidRuntime.dtermNotch[axis], dTermNotchHz, targetPidLooptime, notchQ, FILTER_NOTCH, 1.0f);
}
} else {
pidRuntime.dtermNotchApplyFn = nullFilterApply;
}
//1st Dterm Lowpass Filter
uint16_t dterm_lowpass_hz = pidProfile->dterm_lowpass_hz;
#ifdef USE_DYN_LPF
if (pidProfile->dyn_lpf_dterm_min_hz) {
dterm_lowpass_hz = pidProfile->dyn_lpf_dterm_min_hz;
}
#endif
if (dterm_lowpass_hz > 0) {
switch (pidProfile->dterm_filter_type) {
case FILTER_PT1:
pidRuntime.dtermLowpassApplyFn = (filterApplyFnPtr)pt1FilterApply;
for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
pt1FilterInit(&pidRuntime.dtermLowpass[axis].pt1Filter, pt1FilterGain(dterm_lowpass_hz, pidRuntime.dT));
}
break;
case FILTER_BIQUAD:
if (pidProfile->dterm_lowpass_hz < pidFrequencyNyquist) {
#ifdef USE_DYN_LPF
pidRuntime.dtermLowpassApplyFn = (filterApplyFnPtr)biquadFilterApplyDF1;
#else
pidRuntime.dtermLowpassApplyFn = (filterApplyFnPtr)biquadFilterApply;
#endif
for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
biquadFilterInitLPF(&pidRuntime.dtermLowpass[axis].biquadFilter, dterm_lowpass_hz, targetPidLooptime);
}
} else {
pidRuntime.dtermLowpassApplyFn = nullFilterApply;
}
break;
case FILTER_PT2:
pidRuntime.dtermLowpassApplyFn = (filterApplyFnPtr)pt2FilterApply;
for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
pt2FilterInit(&pidRuntime.dtermLowpass[axis].pt2Filter, pt2FilterGain(dterm_lowpass_hz, pidRuntime.dT));
}
break;
case FILTER_PT3:
pidRuntime.dtermLowpassApplyFn = (filterApplyFnPtr)pt3FilterApply;
for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
pt3FilterInit(&pidRuntime.dtermLowpass[axis].pt3Filter, pt3FilterGain(dterm_lowpass_hz, pidRuntime.dT));
}
break;
default:
pidRuntime.dtermLowpassApplyFn = nullFilterApply;
break;
}
} else {
pidRuntime.dtermLowpassApplyFn = nullFilterApply;
}
//2nd Dterm Lowpass Filter
if (pidProfile->dterm_lowpass2_hz > 0) {
switch (pidProfile->dterm_filter2_type) {
case FILTER_PT1:
pidRuntime.dtermLowpass2ApplyFn = (filterApplyFnPtr)pt1FilterApply;
for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
pt1FilterInit(&pidRuntime.dtermLowpass2[axis].pt1Filter, pt1FilterGain(pidProfile->dterm_lowpass2_hz, pidRuntime.dT));
}
break;
case FILTER_BIQUAD:
if (pidProfile->dterm_lowpass2_hz < pidFrequencyNyquist) {
pidRuntime.dtermLowpass2ApplyFn = (filterApplyFnPtr)biquadFilterApply;
for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
biquadFilterInitLPF(&pidRuntime.dtermLowpass2[axis].biquadFilter, pidProfile->dterm_lowpass2_hz, targetPidLooptime);
}
} else {
pidRuntime.dtermLowpassApplyFn = nullFilterApply;
}
break;
case FILTER_PT2:
pidRuntime.dtermLowpass2ApplyFn = (filterApplyFnPtr)pt2FilterApply;
for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
pt2FilterInit(&pidRuntime.dtermLowpass2[axis].pt2Filter, pt2FilterGain(pidProfile->dterm_lowpass2_hz, pidRuntime.dT));
}
break;
case FILTER_PT3:
pidRuntime.dtermLowpass2ApplyFn = (filterApplyFnPtr)pt3FilterApply;
for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
pt3FilterInit(&pidRuntime.dtermLowpass2[axis].pt3Filter, pt3FilterGain(pidProfile->dterm_lowpass2_hz, pidRuntime.dT));
}
break;
default:
pidRuntime.dtermLowpass2ApplyFn = nullFilterApply;
break;
}
} else {
pidRuntime.dtermLowpass2ApplyFn = nullFilterApply;
}
if (pidProfile->yaw_lowpass_hz == 0) {
pidRuntime.ptermYawLowpassApplyFn = nullFilterApply;
} else {
pidRuntime.ptermYawLowpassApplyFn = (filterApplyFnPtr)pt1FilterApply;
pt1FilterInit(&pidRuntime.ptermYawLowpass, pt1FilterGain(pidProfile->yaw_lowpass_hz, pidRuntime.dT));
}
#if defined(USE_THROTTLE_BOOST)
pt1FilterInit(&throttleLpf, pt1FilterGain(pidProfile->throttle_boost_cutoff, pidRuntime.dT));
#endif
#if defined(USE_ITERM_RELAX)
if (pidRuntime.itermRelax) {
for (int i = 0; i < XYZ_AXIS_COUNT; i++) {
pt1FilterInit(&pidRuntime.windupLpf[i], pt1FilterGain(pidRuntime.itermRelaxCutoff, pidRuntime.dT));
}
}
#endif
#if defined(USE_ABSOLUTE_CONTROL)
if (pidRuntime.itermRelax) {
for (int i = 0; i < XYZ_AXIS_COUNT; i++) {
pt1FilterInit(&pidRuntime.acLpf[i], pt1FilterGain(pidRuntime.acCutoff, pidRuntime.dT));
}
}
#endif
#if defined(USE_D_MIN)
// Initialize the filters for all axis even if the d_min[axis] value is 0
// Otherwise if the pidProfile->d_min_xxx parameters are ever added to
// in-flight adjustments and transition from 0 to > 0 in flight the feature
// won't work because the filter wasn't initialized.
for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
biquadFilterInitLPF(&pidRuntime.dMinRange[axis], D_MIN_RANGE_HZ, targetPidLooptime);
pt1FilterInit(&pidRuntime.dMinLowpass[axis], pt1FilterGain(D_MIN_LOWPASS_HZ, pidRuntime.dT));
}
#endif
#if defined(USE_AIRMODE_LPF)
if (pidProfile->transient_throttle_limit) {
pt1FilterInit(&pidRuntime.airmodeThrottleLpf1, pt1FilterGain(7.0f, pidRuntime.dT));
pt1FilterInit(&pidRuntime.airmodeThrottleLpf2, pt1FilterGain(20.0f, pidRuntime.dT));
}
#endif
pt1FilterInit(&pidRuntime.antiGravityThrottleLpf, pt1FilterGain(ANTI_GRAVITY_THROTTLE_FILTER_CUTOFF, pidRuntime.dT));
pt1FilterInit(&pidRuntime.antiGravitySmoothLpf, pt1FilterGain(ANTI_GRAVITY_SMOOTH_FILTER_CUTOFF, pidRuntime.dT));
pidRuntime.ffBoostFactor = (float)pidProfile->feedforward_boost / 10.0f;
}
void pidInit(const pidProfile_t *pidProfile)
{
pidSetTargetLooptime(gyro.targetLooptime); // Initialize pid looptime
pidInitFilters(pidProfile);
pidInitConfig(pidProfile);
#ifdef USE_RPM_FILTER
rpmFilterInit(rpmFilterConfig());
#endif
}
#ifdef USE_RC_SMOOTHING_FILTER
void pidInitFeedforwardLpf(uint16_t filterCutoff, uint8_t debugAxis)
{
pidRuntime.rcSmoothingDebugAxis = debugAxis;
if (filterCutoff > 0) {
pidRuntime.feedforwardLpfInitialized = true;
for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
pt3FilterInit(&pidRuntime.feedforwardPt3[axis], pt3FilterGain(filterCutoff, pidRuntime.dT));
}
}
}
void pidUpdateFeedforwardLpf(uint16_t filterCutoff)
{
if (filterCutoff > 0) {
for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
pt3FilterUpdateCutoff(&pidRuntime.feedforwardPt3[axis], pt3FilterGain(filterCutoff, pidRuntime.dT));
}
}
}
#endif // USE_RC_SMOOTHING_FILTER
void pidInitConfig(const pidProfile_t *pidProfile)
{
if (pidProfile->feedforwardTransition == 0) {
pidRuntime.feedforwardTransition = 0;
} else {
pidRuntime.feedforwardTransition = 100.0f / pidProfile->feedforwardTransition;
}
for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
pidRuntime.pidCoefficient[axis].Kp = PTERM_SCALE * pidProfile->pid[axis].P;
pidRuntime.pidCoefficient[axis].Ki = ITERM_SCALE * pidProfile->pid[axis].I;
pidRuntime.pidCoefficient[axis].Kd = DTERM_SCALE * pidProfile->pid[axis].D;
pidRuntime.pidCoefficient[axis].Kf = FEEDFORWARD_SCALE * (pidProfile->pid[axis].F / 100.0f);
}
#ifdef USE_INTEGRATED_YAW_CONTROL
if (!pidProfile->use_integrated_yaw)
#endif
{
pidRuntime.pidCoefficient[FD_YAW].Ki *= 2.5f;
}
pidRuntime.levelGain = pidProfile->pid[PID_LEVEL].P / 10.0f;
pidRuntime.horizonGain = pidProfile->pid[PID_LEVEL].I / 10.0f;
pidRuntime.horizonTransition = (float)pidProfile->pid[PID_LEVEL].D;
pidRuntime.horizonTiltExpertMode = pidProfile->horizon_tilt_expert_mode;
pidRuntime.horizonCutoffDegrees = (175 - pidProfile->horizon_tilt_effect) * 1.8f;
pidRuntime.horizonFactorRatio = (100 - pidProfile->horizon_tilt_effect) * 0.01f;
pidRuntime.maxVelocity[FD_ROLL] = pidRuntime.maxVelocity[FD_PITCH] = pidProfile->rateAccelLimit * 100 * pidRuntime.dT;
pidRuntime.maxVelocity[FD_YAW] = pidProfile->yawRateAccelLimit * 100 * pidRuntime.dT;
pidRuntime.itermWindupPointInv = 1.0f;
if (pidProfile->itermWindupPointPercent < 100) {
const float itermWindupPoint = pidProfile->itermWindupPointPercent / 100.0f;
pidRuntime.itermWindupPointInv = 1.0f / (1.0f - itermWindupPoint);
}
pidRuntime.itermAcceleratorGain = pidProfile->itermAcceleratorGain;
pidRuntime.crashTimeLimitUs = pidProfile->crash_time * 1000;
pidRuntime.crashTimeDelayUs = pidProfile->crash_delay * 1000;
pidRuntime.crashRecoveryAngleDeciDegrees = pidProfile->crash_recovery_angle * 10;
pidRuntime.crashRecoveryRate = pidProfile->crash_recovery_rate;
pidRuntime.crashGyroThreshold = pidProfile->crash_gthreshold;
pidRuntime.crashDtermThreshold = pidProfile->crash_dthreshold;
pidRuntime.crashSetpointThreshold = pidProfile->crash_setpoint_threshold;
pidRuntime.crashLimitYaw = pidProfile->crash_limit_yaw;
pidRuntime.itermLimit = pidProfile->itermLimit;
#if defined(USE_THROTTLE_BOOST)
throttleBoost = pidProfile->throttle_boost * 0.1f;
#endif
pidRuntime.itermRotation = pidProfile->iterm_rotation;
pidRuntime.antiGravityMode = pidProfile->antiGravityMode;
// Calculate the anti-gravity value that will trigger the OSD display.
// For classic AG it's either 1.0 for off and > 1.0 for on.
// For the new AG it's a continuous floating value so we want to trigger the OSD
// display when it exceeds 25% of its possible range. This gives a useful indication
// of AG activity without excessive display.
pidRuntime.antiGravityOsdCutoff = 0.0f;
if (pidRuntime.antiGravityMode == ANTI_GRAVITY_SMOOTH) {
pidRuntime.antiGravityOsdCutoff += (pidRuntime.itermAcceleratorGain / 1000.0f) * 0.25f;
}
#if defined(USE_ITERM_RELAX)
pidRuntime.itermRelax = pidProfile->iterm_relax;
pidRuntime.itermRelaxType = pidProfile->iterm_relax_type;
pidRuntime.itermRelaxCutoff = pidProfile->iterm_relax_cutoff;
#endif
#ifdef USE_ACRO_TRAINER
pidRuntime.acroTrainerAngleLimit = pidProfile->acro_trainer_angle_limit;
pidRuntime.acroTrainerLookaheadTime = (float)pidProfile->acro_trainer_lookahead_ms / 1000.0f;
pidRuntime.acroTrainerDebugAxis = pidProfile->acro_trainer_debug_axis;
pidRuntime.acroTrainerGain = (float)pidProfile->acro_trainer_gain / 10.0f;
#endif // USE_ACRO_TRAINER
#if defined(USE_ABSOLUTE_CONTROL)
pidRuntime.acGain = (float)pidProfile->abs_control_gain;
pidRuntime.acLimit = (float)pidProfile->abs_control_limit;
pidRuntime.acErrorLimit = (float)pidProfile->abs_control_error_limit;
pidRuntime.acCutoff = (float)pidProfile->abs_control_cutoff;
for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
float iCorrection = -pidRuntime.acGain * PTERM_SCALE / ITERM_SCALE * pidRuntime.pidCoefficient[axis].Kp;
pidRuntime.pidCoefficient[axis].Ki = MAX(0.0f, pidRuntime.pidCoefficient[axis].Ki + iCorrection);
}
#endif
#ifdef USE_DYN_LPF
if (pidProfile->dyn_lpf_dterm_min_hz > 0) {
switch (pidProfile->dterm_filter_type) {
case FILTER_PT1:
pidRuntime.dynLpfFilter = DYN_LPF_PT1;
break;
case FILTER_BIQUAD:
pidRuntime.dynLpfFilter = DYN_LPF_BIQUAD;
break;
case FILTER_PT2:
pidRuntime.dynLpfFilter = DYN_LPF_PT2;
break;
case FILTER_PT3:
pidRuntime.dynLpfFilter = DYN_LPF_PT3;
break;
default:
pidRuntime.dynLpfFilter = DYN_LPF_NONE;
break;
}
} else {
pidRuntime.dynLpfFilter = DYN_LPF_NONE;
}
pidRuntime.dynLpfMin = pidProfile->dyn_lpf_dterm_min_hz;
pidRuntime.dynLpfMax = pidProfile->dyn_lpf_dterm_max_hz;
pidRuntime.dynLpfCurveExpo = pidProfile->dyn_lpf_curve_expo;
#endif
#ifdef USE_LAUNCH_CONTROL
pidRuntime.launchControlMode = pidProfile->launchControlMode;
if (sensors(SENSOR_ACC)) {
pidRuntime.launchControlAngleLimit = pidProfile->launchControlAngleLimit;
} else {
pidRuntime.launchControlAngleLimit = 0;
}
pidRuntime.launchControlKi = ITERM_SCALE * pidProfile->launchControlGain;
#endif
#ifdef USE_INTEGRATED_YAW_CONTROL
pidRuntime.useIntegratedYaw = pidProfile->use_integrated_yaw;
pidRuntime.integratedYawRelax = pidProfile->integrated_yaw_relax;
#endif
#ifdef USE_THRUST_LINEARIZATION
pidRuntime.thrustLinearization = pidProfile->thrustLinearization / 100.0f;
pidRuntime.throttleCompensateAmount = pidRuntime.thrustLinearization - 0.5f * powf(pidRuntime.thrustLinearization, 2);
#endif
#if defined(USE_D_MIN)
for (int axis = FD_ROLL; axis <= FD_YAW; ++axis) {
const uint8_t dMin = pidProfile->d_min[axis];
if ((dMin > 0) && (dMin < pidProfile->pid[axis].D)) {
pidRuntime.dMinPercent[axis] = dMin / (float)(pidProfile->pid[axis].D);
} else {
pidRuntime.dMinPercent[axis] = 0;
}
}
pidRuntime.dMinGyroGain = pidProfile->d_min_gain * D_MIN_GAIN_FACTOR / D_MIN_LOWPASS_HZ;
pidRuntime.dMinSetpointGain = pidProfile->d_min_gain * D_MIN_SETPOINT_GAIN_FACTOR * pidProfile->d_min_advance * pidRuntime.pidFrequency / (100 * D_MIN_LOWPASS_HZ);
// lowpass included inversely in gain since stronger lowpass decreases peak effect
#endif
#if defined(USE_AIRMODE_LPF)
pidRuntime.airmodeThrottleOffsetLimit = pidProfile->transient_throttle_limit / 100.0f;
#endif
#ifdef USE_FEEDFORWARD
pidRuntime.feedforwardAveraging = pidProfile->feedforward_averaging;
if (pidProfile->feedforward_smooth_factor) {
pidRuntime.ffSmoothFactor = 1.0f - ((float)pidProfile->feedforward_smooth_factor) / 100.0f;
} else {
// set automatically according to boost amount, limit to 0.5 for auto
pidRuntime.ffSmoothFactor = MAX(0.5f, 1.0f - ((float)pidProfile->feedforward_boost) * 2.0f / 100.0f);
}
pidRuntime.ffJitterFactor = pidProfile->feedforward_jitter_factor;
feedforwardInit(pidProfile);
#endif
pidRuntime.levelRaceMode = pidProfile->level_race_mode;
}
void pidCopyProfile(uint8_t dstPidProfileIndex, uint8_t srcPidProfileIndex)
{
if (dstPidProfileIndex < PID_PROFILE_COUNT && srcPidProfileIndex < PID_PROFILE_COUNT
&& dstPidProfileIndex != srcPidProfileIndex) {
memcpy(pidProfilesMutable(dstPidProfileIndex), pidProfilesMutable(srcPidProfileIndex), sizeof(pidProfile_t));
}
}
Reference in a new issue View git blame Copy permalink