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

Merge pull request #9759 from etracer65/pid_init_separate 

Split initialization from pid.c for flash savings
This commit is contained in:
Michael Keller 2020-06-22 00:35:42 +12:00 committed by GitHub
parent 3eabb814ec c06106e2d1
commit a8085bef8b
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
16 changed files with 798 additions and 710 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
make/source.mk
View File

@ -97,6 +97,7 @@ COMMON_SRC = \
flight/mixer.c \
flight/mixer_tricopter.c \
flight/pid.c \
flight/pid_init.c \
flight/rpm_filter.c \
flight/servos.c \
flight/servos_tricopter.c \
@ -346,7 +347,8 @@ SIZE_OPTIMISED_SRC := $(SIZE_OPTIMISED_SRC) \
osd/osd.c \
osd/osd_elements.c \
rx/rx_bind.c \
sensors/gyro_init.c
sensors/gyro_init.c\
flight/pid_init.c
# Gyro driver files that only contain initialization and configuration code - not runtime code
SIZE_OPTIMISED_SRC := $(SIZE_OPTIMISED_SRC) \

1
src/main/cms/cms_menu_imu.c
View File

@ -51,6 +51,7 @@
#include "flight/mixer.h"
#include "flight/pid.h"
#include "flight/pid_init.h"
#include "pg/pg.h"

1
src/main/config/config.c
View File

@ -51,6 +51,7 @@
#include "flight/imu.h"
#include "flight/mixer.h"
#include "flight/pid.h"
#include "flight/pid_init.h"
#include "flight/rpm_filter.h"
#include "flight/servos.h"

3
src/main/fc/init.c
View File

@ -38,6 +38,7 @@
#include "common/maths.h"
#include "common/printf_serial.h"
#include "config/config.h"
#include "config/config_eeprom.h"
#include "config/feature.h"
@ -83,7 +84,6 @@
#include "drivers/vtx_table.h"
#include "fc/board_info.h"
#include "config/config.h"
#include "fc/dispatch.h"
#include "fc/init.h"
#include "fc/rc_controls.h"
@ -95,6 +95,7 @@
#include "flight/imu.h"
#include "flight/mixer.h"
#include "flight/pid.h"
#include "flight/pid_init.h"
#include "flight/rpm_filter.h"
#include "flight/servos.h"

2
src/main/fc/rc.c
View File

@ -44,7 +44,7 @@
#include "flight/imu.h"
#include "flight/interpolated_setpoint.h"
#include "flight/gps_rescue.h"
#include "flight/pid.h"
#include "flight/pid_init.h"
#include "pg/rx.h"

1
src/main/fc/rc_adjustments.c
View File

@ -45,6 +45,7 @@
#include "fc/rc.h"
#include "flight/pid.h"
#include "flight/pid_init.h"
#include "io/beeper.h"
#include "io/ledstrip.h"

7
src/main/flight/interpolated_setpoint.c
View File

@ -24,9 +24,14 @@
#ifdef USE_INTERPOLATED_SP
#include "build/debug.h"
#include "common/maths.h"
#include "fc/rc.h"
#include "flight/interpolated_setpoint.h"
#include "flight/pid.h"
#include "interpolated_setpoint.h"
static float setpointDeltaImpl[XYZ_AXIS_COUNT];
static float setpointDelta[XYZ_AXIS_COUNT];

8
src/main/flight/interpolated_setpoint.h
View File

@ -25,14 +25,6 @@
#include "common/axis.h"
#include "flight/pid.h"
typedef enum ffInterpolationType_e {
FF_INTERPOLATE_OFF,
FF_INTERPOLATE_ON,
FF_INTERPOLATE_AVG2,
FF_INTERPOLATE_AVG3,
FF_INTERPOLATE_AVG4
} ffInterpolationType_t;
void interpolatedSpInit(const pidProfile_t *pidProfile);
float interpolatedSpApply(int axis, bool newRcFrame, ffInterpolationType_t type);
float applyFfLimit(int axis, float value, float Kp, float currentPidSetpoint);

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

File diff suppressed because it is too large Load Diff

156
src/main/flight/pid.h
View File

@ -24,6 +24,7 @@
#include "common/time.h"
#include "common/filter.h"
#include "common/axis.h"
#include "pg/pg.h"
#define MAX_PID_PROCESS_DENOM 16
@ -107,6 +108,14 @@ typedef enum {
ITERM_RELAX_TYPE_COUNT,
} itermRelaxType_e;
typedef enum ffInterpolationType_e {
FF_INTERPOLATE_OFF,
FF_INTERPOLATE_ON,
FF_INTERPOLATE_AVG2,
FF_INTERPOLATE_AVG3,
FF_INTERPOLATE_AVG4
} ffInterpolationType_t;
#define MAX_PROFILE_NAME_LENGTH 8u
typedef struct pidProfile_s {
@ -218,6 +227,147 @@ typedef struct pidAxisData_s {
float Sum;
} pidAxisData_t;
typedef union dtermLowpass_u {
pt1Filter_t pt1Filter;
biquadFilter_t biquadFilter;
} dtermLowpass_t;
typedef struct pidCoefficient_s {
float Kp;
float Ki;
float Kd;
float Kf;
} pidCoefficient_t;
typedef struct pidRuntime_s {
float dT;
float pidFrequency;
bool pidStabilisationEnabled;
float previousPidSetpoint[XYZ_AXIS_COUNT];
filterApplyFnPtr dtermNotchApplyFn;
biquadFilter_t dtermNotch[XYZ_AXIS_COUNT];
filterApplyFnPtr dtermLowpassApplyFn;
dtermLowpass_t dtermLowpass[XYZ_AXIS_COUNT];
filterApplyFnPtr dtermLowpass2ApplyFn;
dtermLowpass_t dtermLowpass2[XYZ_AXIS_COUNT];
filterApplyFnPtr ptermYawLowpassApplyFn;
pt1Filter_t ptermYawLowpass;
bool antiGravityEnabled;
uint8_t antiGravityMode;
pt1Filter_t antiGravityThrottleLpf;
float antiGravityOsdCutoff;
float antiGravityThrottleHpf;
float ffBoostFactor;
float ffSpikeLimitInverse;
float itermAccelerator;
uint16_t itermAcceleratorGain;
float feedForwardTransition;
pidCoefficient_t pidCoefficient[XYZ_AXIS_COUNT];
float levelGain;
float horizonGain;
float horizonTransition;
float horizonCutoffDegrees;
float horizonFactorRatio;
uint8_t horizonTiltExpertMode;
float maxVelocity[XYZ_AXIS_COUNT];
float itermWindupPointInv;
bool inCrashRecoveryMode;
timeUs_t crashDetectedAtUs;
timeDelta_t crashTimeLimitUs;
timeDelta_t crashTimeDelayUs;
int32_t crashRecoveryAngleDeciDegrees;
float crashRecoveryRate;
float crashGyroThreshold;
float crashDtermThreshold;
float crashSetpointThreshold;
float crashLimitYaw;
float itermLimit;
bool itermRotation;
bool zeroThrottleItermReset;
bool levelRaceMode;
#ifdef USE_ITERM_RELAX
pt1Filter_t windupLpf[XYZ_AXIS_COUNT];
uint8_t itermRelax;
uint8_t itermRelaxType;
uint8_t itermRelaxCutoff;
#endif
#ifdef USE_ABSOLUTE_CONTROL
float acCutoff;
float acGain;
float acLimit;
float acErrorLimit;
pt1Filter_t acLpf[XYZ_AXIS_COUNT];
float oldSetpointCorrection[XYZ_AXIS_COUNT];
#endif
#ifdef USE_D_MIN
biquadFilter_t dMinRange[XYZ_AXIS_COUNT];
pt1Filter_t dMinLowpass[XYZ_AXIS_COUNT];
float dMinPercent[XYZ_AXIS_COUNT];
float dMinGyroGain;
float dMinSetpointGain;
#endif
#ifdef USE_AIRMODE_LPF
pt1Filter_t airmodeThrottleLpf1;
pt1Filter_t airmodeThrottleLpf2;
#endif
#ifdef USE_RC_SMOOTHING_FILTER
pt1Filter_t setpointDerivativePt1[XYZ_AXIS_COUNT];
biquadFilter_t setpointDerivativeBiquad[XYZ_AXIS_COUNT];
bool setpointDerivativeLpfInitialized;
uint8_t rcSmoothingDebugAxis;
uint8_t rcSmoothingFilterType;
#endif // USE_RC_SMOOTHING_FILTER
#ifdef USE_ACRO_TRAINER
float acroTrainerAngleLimit;
float acroTrainerLookaheadTime;
uint8_t acroTrainerDebugAxis;
float acroTrainerGain;
bool acroTrainerActive;
int acroTrainerAxisState[2]; // only need roll and pitch
#endif
#ifdef USE_DYN_LPF
uint8_t dynLpfFilter;
uint16_t dynLpfMin;
uint16_t dynLpfMax;
uint8_t dynLpfCurveExpo;
#endif
#ifdef USE_LAUNCH_CONTROL
uint8_t launchControlMode;
uint8_t launchControlAngleLimit;
float launchControlKi;
#endif
#ifdef USE_INTEGRATED_YAW_CONTROL
bool useIntegratedYaw;
uint8_t integratedYawRelax;
#endif
#ifdef USE_THRUST_LINEARIZATION
float thrustLinearization;
float thrustLinearizationReciprocal;
float thrustLinearizationB;
#endif
#ifdef USE_AIRMODE_LPF
float airmodeThrottleOffsetLimit;
#endif
#ifdef USE_INTERPOLATED_SP
ffInterpolationType_t ffFromInterpolatedSetpoint;
float ffSmoothFactor;
#endif
} pidRuntime_t;
extern pidRuntime_t pidRuntime;
extern const char pidNames[];
extern pidAxisData_t pidData[3];
@ -230,15 +380,9 @@ extern pt1Filter_t throttleLpf;
void pidResetIterm(void);
void pidStabilisationState(pidStabilisationState_e pidControllerState);
void pidSetItermAccelerator(float newItermAccelerator);
void pidInitFilters(const pidProfile_t *pidProfile);
void pidInitConfig(const pidProfile_t *pidProfile);
void pidInit(const pidProfile_t *pidProfile);
void pidCopyProfile(uint8_t dstPidProfileIndex, uint8_t srcPidProfileIndex);
bool crashRecoveryModeActive(void);
void pidAcroTrainerInit(void);
void pidSetAcroTrainerState(bool newState);
void pidInitSetpointDerivativeLpf(uint16_t filterCutoff, uint8_t debugAxis, uint8_t filterType);
void pidUpdateSetpointDerivativeLpf(uint16_t filterCutoff);
void pidUpdateAntiGravityThrottleFilter(float throttle);
bool pidOsdAntiGravityActive(void);
bool pidOsdAntiGravityMode(void);

415
src/main/flight/pid_init.c Normal file
View File

@ -0,0 +1,415 @@
/*
* 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 "common/maths.h"
#include "drivers/dshot_command.h"
#include "fc/rc_controls.h"
#include "fc/runtime_config.h"
#include "flight/interpolated_setpoint.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
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);
}
} 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 && dterm_lowpass_hz < pidFrequencyNyquist) {
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:
#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);
}
break;
default:
pidRuntime.dtermLowpassApplyFn = nullFilterApply;
break;
}
} else {
pidRuntime.dtermLowpassApplyFn = nullFilterApply;
}
//2nd Dterm Lowpass Filter
if (pidProfile->dterm_lowpass2_hz == 0 || pidProfile->dterm_lowpass2_hz > pidFrequencyNyquist) {
pidRuntime.dtermLowpass2ApplyFn = nullFilterApply;
} else {
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:
pidRuntime.dtermLowpass2ApplyFn = (filterApplyFnPtr)biquadFilterApply;
for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
biquadFilterInitLPF(&pidRuntime.dtermLowpass2[axis].biquadFilter, pidProfile->dterm_lowpass2_hz, targetPidLooptime);
}
break;
default:
pidRuntime.dtermLowpass2ApplyFn = nullFilterApply;
break;
}
}
if (pidProfile->yaw_lowpass_hz == 0 || pidProfile->yaw_lowpass_hz > pidFrequencyNyquist) {
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));
pidRuntime.ffBoostFactor = (float)pidProfile->ff_boost / 10.0f;
pidRuntime.ffSpikeLimitInverse = pidProfile->ff_spike_limit ? 1.0f / ((float)pidProfile->ff_spike_limit / 10.0f) : 0.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 pidInitSetpointDerivativeLpf(uint16_t filterCutoff, uint8_t debugAxis, uint8_t filterType)
{
pidRuntime.rcSmoothingDebugAxis = debugAxis;
pidRuntime.rcSmoothingFilterType = filterType;
if ((filterCutoff > 0) && (pidRuntime.rcSmoothingFilterType != RC_SMOOTHING_DERIVATIVE_OFF)) {
pidRuntime.setpointDerivativeLpfInitialized = true;
for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
switch (pidRuntime.rcSmoothingFilterType) {
case RC_SMOOTHING_DERIVATIVE_PT1:
pt1FilterInit(&pidRuntime.setpointDerivativePt1[axis], pt1FilterGain(filterCutoff, pidRuntime.dT));
break;
case RC_SMOOTHING_DERIVATIVE_BIQUAD:
biquadFilterInitLPF(&pidRuntime.setpointDerivativeBiquad[axis], filterCutoff, targetPidLooptime);
break;
}
}
}
}
void pidUpdateSetpointDerivativeLpf(uint16_t filterCutoff)
{
if ((filterCutoff > 0) && (pidRuntime.rcSmoothingFilterType != RC_SMOOTHING_DERIVATIVE_OFF)) {
for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
switch (pidRuntime.rcSmoothingFilterType) {
case RC_SMOOTHING_DERIVATIVE_PT1:
pt1FilterUpdateCutoff(&pidRuntime.setpointDerivativePt1[axis], pt1FilterGain(filterCutoff, pidRuntime.dT));
break;
case RC_SMOOTHING_DERIVATIVE_BIQUAD:
biquadFilterUpdateLPF(&pidRuntime.setpointDerivativeBiquad[axis], filterCutoff, targetPidLooptime);
break;
}
}
}
}
#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) / 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;
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;
if (pidRuntime.thrustLinearization != 0.0f) {
pidRuntime.thrustLinearizationReciprocal = 1.0f / pidRuntime.thrustLinearization;
pidRuntime.thrustLinearizationB = (1.0f - pidRuntime.thrustLinearization) / (2.0f * pidRuntime.thrustLinearization);
}
#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_INTERPOLATED_SP
pidRuntime.ffFromInterpolatedSetpoint = pidProfile->ff_interpolate_sp;
pidRuntime.ffSmoothFactor = 1.0f - ((float)pidProfile->ff_smooth_factor) / 100.0f;
interpolatedSpInit(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));
}
}

29
src/main/flight/pid_init.h Normal file
View File

@ -0,0 +1,29 @@
/*
* 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/>.
*/
#pragma once
void pidInit(const pidProfile_t *pidProfile);
void pidInitFilters(const pidProfile_t *pidProfile);
void pidInitConfig(const pidProfile_t *pidProfile);
void pidSetItermAccelerator(float newItermAccelerator);
void pidInitSetpointDerivativeLpf(uint16_t filterCutoff, uint8_t debugAxis, uint8_t filterType);
void pidUpdateSetpointDerivativeLpf(uint16_t filterCutoff);
void pidCopyProfile(uint8_t dstPidProfileIndex, uint8_t srcPidProfileIndex);

1
src/main/msp/msp.c
View File

@ -82,6 +82,7 @@
#include "flight/imu.h"
#include "flight/mixer.h"
#include "flight/pid.h"
#include "flight/pid_init.h"
#include "flight/position.h"
#include "flight/rpm_filter.h"
#include "flight/servos.h"

23
src/main/target/COLIBRI_RACE/i2c_bst.c
View File

@ -16,6 +16,10 @@
#include "common/color.h"
#include "common/maths.h"
#include "config/config.h"
#include "config/config_eeprom.h"
#include "config/feature.h"
#include "drivers/accgyro/accgyro.h"
#include "drivers/compass/compass.h"
#include "drivers/bus_i2c.h"
@ -25,13 +29,20 @@
#include "drivers/time.h"
#include "drivers/timer.h"
#include "config/config.h"
#include "fc/controlrate_profile.h"
#include "fc/core.h"
#include "fc/rc_adjustments.h"
#include "fc/rc_controls.h"
#include "fc/runtime_config.h"
#include "flight/position.h"
#include "flight/failsafe.h"
#include "flight/imu.h"
#include "flight/mixer.h"
#include "flight/pid.h"
#include "flight/pid_init.h"
#include "flight/servos.h"
#include "io/motors.h"
#include "io/servos.h"
#include "io/gps.h"
@ -60,16 +71,6 @@
#include "telemetry/telemetry.h"
#include "flight/position.h"
#include "flight/failsafe.h"
#include "flight/imu.h"
#include "flight/mixer.h"
#include "flight/pid.h"
#include "flight/servos.h"
#include "config/config_eeprom.h"
#include "config/feature.h"
#include "bus_bst.h"
#include "i2c_bst.h"

1
src/test/Makefile
View File

@ -376,6 +376,7 @@ pid_unittest_SRC := \
$(USER_DIR)/drivers/accgyro/gyro_sync.c \
$(USER_DIR)/fc/runtime_config.c \
$(USER_DIR)/flight/pid.c \
$(USER_DIR)/flight/pid_init.c \
$(USER_DIR)/pg/pg.c
pid_unittest_DEFINES := \

3
src/test/unit/pid_unittest.cc
View File

@ -52,9 +52,10 @@ extern "C" {
#include "fc/rc_controls.h"
#include "fc/runtime_config.h"
#include "flight/pid.h"
#include "flight/imu.h"
#include "flight/mixer.h"
#include "flight/pid.h"
#include "flight/pid_init.h"
#include "io/gps.h"