mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-24 00:35:39 +03:00
96ee9e3103
Defaults will be the same regardless of whether the target has `USE_DYN_LPF` included. Previously the defaults would vary and it wouldn't be obvious why. Defaults are as follows: gyro lowpass 1: 150/BIQUAD gyro lowpass 2: OFF dterm lowpass 1: 150/BIQUAD dterm lowpass 2: 150/BIQUAD Nothing has changed int eh dynamic lowpass logic. If it's enabled those settings will be used in place of the static lowpass cutoff. Needs coordination with the Configurator to change the defaults used when re-eanbling the lowpass filters as they are currently based on previous version settings and will dfault to inappropriate values.
1555 lines
60 KiB
C
1555 lines
60 KiB
C
/*
|
|
* 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 "config/config_reset.h"
|
|
|
|
#include "drivers/sound_beeper.h"
|
|
#include "drivers/time.h"
|
|
|
|
#include "fc/controlrate_profile.h"
|
|
#include "fc/core.h"
|
|
#include "fc/rc.h"
|
|
#include "fc/rc_controls.h"
|
|
#include "fc/runtime_config.h"
|
|
|
|
#include "flight/gps_rescue.h"
|
|
#include "flight/imu.h"
|
|
#include "flight/mixer.h"
|
|
|
|
#include "io/gps.h"
|
|
|
|
#include "pg/pg.h"
|
|
#include "pg/pg_ids.h"
|
|
|
|
#include "sensors/acceleration.h"
|
|
#include "sensors/battery.h"
|
|
#include "sensors/gyro.h"
|
|
#include "sensors/rpm_filter.h"
|
|
|
|
#include "pid.h"
|
|
|
|
const char pidNames[] =
|
|
"ROLL;"
|
|
"PITCH;"
|
|
"YAW;"
|
|
"LEVEL;"
|
|
"MAG;";
|
|
|
|
FAST_RAM_ZERO_INIT uint32_t targetPidLooptime;
|
|
FAST_RAM_ZERO_INIT pidAxisData_t pidData[XYZ_AXIS_COUNT];
|
|
|
|
static FAST_RAM_ZERO_INIT bool pidStabilisationEnabled;
|
|
|
|
static FAST_RAM_ZERO_INIT bool inCrashRecoveryMode = false;
|
|
|
|
static FAST_RAM_ZERO_INIT float dT;
|
|
static FAST_RAM_ZERO_INIT float pidFrequency;
|
|
|
|
static FAST_RAM_ZERO_INIT uint8_t antiGravityMode;
|
|
static FAST_RAM_ZERO_INIT float antiGravityThrottleHpf;
|
|
static FAST_RAM_ZERO_INIT uint16_t itermAcceleratorGain;
|
|
static FAST_RAM float antiGravityOsdCutoff = 1.0f;
|
|
static FAST_RAM_ZERO_INIT bool antiGravityEnabled;
|
|
static FAST_RAM_ZERO_INIT bool zeroThrottleItermReset;
|
|
|
|
PG_REGISTER_WITH_RESET_TEMPLATE(pidConfig_t, pidConfig, PG_PID_CONFIG, 2);
|
|
|
|
#ifdef STM32F10X
|
|
#define PID_PROCESS_DENOM_DEFAULT 1
|
|
#elif defined(USE_GYRO_SPI_MPU6000) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_ICM20689)
|
|
#define PID_PROCESS_DENOM_DEFAULT 4
|
|
#else
|
|
#define PID_PROCESS_DENOM_DEFAULT 2
|
|
#endif
|
|
#if defined(USE_D_MIN)
|
|
#define D_MIN_GAIN_FACTOR 0.00005f
|
|
#define D_MIN_SETPOINT_GAIN_FACTOR 0.00005f
|
|
#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
|
|
#endif
|
|
|
|
#ifdef USE_RUNAWAY_TAKEOFF
|
|
PG_RESET_TEMPLATE(pidConfig_t, pidConfig,
|
|
.pid_process_denom = PID_PROCESS_DENOM_DEFAULT,
|
|
.runaway_takeoff_prevention = true,
|
|
.runaway_takeoff_deactivate_throttle = 20, // throttle level % needed to accumulate deactivation time
|
|
.runaway_takeoff_deactivate_delay = 500 // Accumulated time (in milliseconds) before deactivation in successful takeoff
|
|
);
|
|
#else
|
|
PG_RESET_TEMPLATE(pidConfig_t, pidConfig,
|
|
.pid_process_denom = PID_PROCESS_DENOM_DEFAULT
|
|
);
|
|
#endif
|
|
|
|
#ifdef USE_ACRO_TRAINER
|
|
#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
|
|
|
|
#ifdef USE_AIRMODE_LPF
|
|
static FAST_RAM_ZERO_INIT float airmodeThrottleOffsetLimit;
|
|
#endif
|
|
|
|
#define ANTI_GRAVITY_THROTTLE_FILTER_CUTOFF 15 // The anti gravity throttle highpass filter cutoff
|
|
|
|
#define CRASH_RECOVERY_DETECTION_DELAY_US 1000000 // 1 second delay before crash recovery detection is active after entering a self-level mode
|
|
|
|
#define LAUNCH_CONTROL_YAW_ITERM_LIMIT 50 // yaw iterm windup limit when launch mode is "FULL" (all axes)
|
|
|
|
PG_REGISTER_ARRAY_WITH_RESET_FN(pidProfile_t, PID_PROFILE_COUNT, pidProfiles, PG_PID_PROFILE, 9);
|
|
|
|
void resetPidProfile(pidProfile_t *pidProfile)
|
|
{
|
|
RESET_CONFIG(pidProfile_t, pidProfile,
|
|
.pid = {
|
|
[PID_ROLL] = { 42, 60, 35, 70 },
|
|
[PID_PITCH] = { 46, 70, 38, 75 },
|
|
[PID_YAW] = { 35, 100, 0, 0 },
|
|
[PID_LEVEL] = { 50, 50, 75, 0 },
|
|
[PID_MAG] = { 40, 0, 0, 0 },
|
|
},
|
|
.pidSumLimit = PIDSUM_LIMIT,
|
|
.pidSumLimitYaw = PIDSUM_LIMIT_YAW,
|
|
.yaw_lowpass_hz = 0,
|
|
.dterm_notch_hz = 0,
|
|
.dterm_notch_cutoff = 0,
|
|
.itermWindupPointPercent = 100,
|
|
.vbatPidCompensation = 0,
|
|
.pidAtMinThrottle = PID_STABILISATION_ON,
|
|
.levelAngleLimit = 55,
|
|
.feedForwardTransition = 0,
|
|
.yawRateAccelLimit = 0,
|
|
.rateAccelLimit = 0,
|
|
.itermThrottleThreshold = 250,
|
|
.itermAcceleratorGain = 5000,
|
|
.crash_time = 500, // ms
|
|
.crash_delay = 0, // ms
|
|
.crash_recovery_angle = 10, // degrees
|
|
.crash_recovery_rate = 100, // degrees/second
|
|
.crash_dthreshold = 50, // degrees/second/second
|
|
.crash_gthreshold = 400, // degrees/second
|
|
.crash_setpoint_threshold = 350, // degrees/second
|
|
.crash_recovery = PID_CRASH_RECOVERY_OFF, // off by default
|
|
.horizon_tilt_effect = 75,
|
|
.horizon_tilt_expert_mode = false,
|
|
.crash_limit_yaw = 200,
|
|
.itermLimit = 400,
|
|
.throttle_boost = 5,
|
|
.throttle_boost_cutoff = 15,
|
|
.iterm_rotation = false,
|
|
.smart_feedforward = false,
|
|
.iterm_relax = ITERM_RELAX_RP,
|
|
.iterm_relax_cutoff = ITERM_RELAX_CUTOFF_DEFAULT,
|
|
.iterm_relax_type = ITERM_RELAX_SETPOINT,
|
|
.acro_trainer_angle_limit = 20,
|
|
.acro_trainer_lookahead_ms = 50,
|
|
.acro_trainer_debug_axis = FD_ROLL,
|
|
.acro_trainer_gain = 75,
|
|
.abs_control_gain = 0,
|
|
.abs_control_limit = 90,
|
|
.abs_control_error_limit = 20,
|
|
.abs_control_cutoff = 11,
|
|
.antiGravityMode = ANTI_GRAVITY_SMOOTH,
|
|
.dterm_lowpass_hz = 150, // NOTE: dynamic lpf is enabled by default so this setting is actually
|
|
// overridden and the static lowpass 1 is disabled. We can't set this
|
|
// value to 0 otherwise Configurator versions 10.4 and earlier will also
|
|
// reset the lowpass filter type to PT1 overriding the desired BIQUAD setting.
|
|
.dterm_lowpass2_hz = 150, // second Dterm LPF ON by default
|
|
.dterm_filter_type = FILTER_BIQUAD,
|
|
.dterm_filter2_type = FILTER_BIQUAD,
|
|
.dyn_lpf_dterm_min_hz = 150,
|
|
.dyn_lpf_dterm_max_hz = 250,
|
|
.launchControlMode = LAUNCH_CONTROL_MODE_NORMAL,
|
|
.launchControlThrottlePercent = 20,
|
|
.launchControlAngleLimit = 0,
|
|
.launchControlGain = 40,
|
|
.launchControlAllowTriggerReset = true,
|
|
.use_integrated_yaw = false,
|
|
.integrated_yaw_relax = 200,
|
|
.thrustLinearization = 0,
|
|
.d_min = { 20, 22, 0 }, // roll, pitch, yaw
|
|
.d_min_gain = 27,
|
|
.d_min_advance = 20,
|
|
.motor_output_limit = 100,
|
|
.auto_profile_cell_count = AUTO_PROFILE_CELL_COUNT_STAY,
|
|
.transient_throttle_limit = 15,
|
|
);
|
|
#ifndef USE_D_MIN
|
|
pidProfile->pid[PID_ROLL].D = 30;
|
|
pidProfile->pid[PID_PITCH].D = 32;
|
|
#endif
|
|
}
|
|
|
|
void pgResetFn_pidProfiles(pidProfile_t *pidProfiles)
|
|
{
|
|
for (int i = 0; i < PID_PROFILE_COUNT; i++) {
|
|
resetPidProfile(&pidProfiles[i]);
|
|
}
|
|
}
|
|
|
|
static void pidSetTargetLooptime(uint32_t pidLooptime)
|
|
{
|
|
targetPidLooptime = pidLooptime;
|
|
dT = targetPidLooptime * 1e-6f;
|
|
pidFrequency = 1.0f / dT;
|
|
}
|
|
|
|
static FAST_RAM float itermAccelerator = 1.0f;
|
|
|
|
void pidSetItermAccelerator(float newItermAccelerator)
|
|
{
|
|
itermAccelerator = newItermAccelerator;
|
|
}
|
|
|
|
bool pidOsdAntiGravityActive(void)
|
|
{
|
|
return (itermAccelerator > antiGravityOsdCutoff);
|
|
}
|
|
|
|
void pidStabilisationState(pidStabilisationState_e pidControllerState)
|
|
{
|
|
pidStabilisationEnabled = (pidControllerState == PID_STABILISATION_ON) ? true : false;
|
|
}
|
|
|
|
const angle_index_t rcAliasToAngleIndexMap[] = { AI_ROLL, AI_PITCH };
|
|
|
|
typedef union dtermLowpass_u {
|
|
pt1Filter_t pt1Filter;
|
|
biquadFilter_t biquadFilter;
|
|
} dtermLowpass_t;
|
|
|
|
static FAST_RAM_ZERO_INIT float previousPidSetpoint[XYZ_AXIS_COUNT];
|
|
|
|
static FAST_RAM_ZERO_INIT filterApplyFnPtr dtermNotchApplyFn;
|
|
static FAST_RAM_ZERO_INIT biquadFilter_t dtermNotch[XYZ_AXIS_COUNT];
|
|
static FAST_RAM_ZERO_INIT filterApplyFnPtr dtermLowpassApplyFn;
|
|
static FAST_RAM_ZERO_INIT dtermLowpass_t dtermLowpass[XYZ_AXIS_COUNT];
|
|
static FAST_RAM_ZERO_INIT filterApplyFnPtr dtermLowpass2ApplyFn;
|
|
static FAST_RAM_ZERO_INIT dtermLowpass_t dtermLowpass2[XYZ_AXIS_COUNT];
|
|
static FAST_RAM_ZERO_INIT filterApplyFnPtr ptermYawLowpassApplyFn;
|
|
static FAST_RAM_ZERO_INIT pt1Filter_t ptermYawLowpass;
|
|
|
|
#if defined(USE_ITERM_RELAX)
|
|
static FAST_RAM_ZERO_INIT pt1Filter_t windupLpf[XYZ_AXIS_COUNT];
|
|
static FAST_RAM_ZERO_INIT uint8_t itermRelax;
|
|
static FAST_RAM_ZERO_INIT uint8_t itermRelaxType;
|
|
static uint8_t itermRelaxCutoff;
|
|
static FAST_RAM_ZERO_INIT float itermRelaxSetpointThreshold;
|
|
#endif
|
|
|
|
#if defined(USE_ABSOLUTE_CONTROL)
|
|
STATIC_UNIT_TESTED FAST_RAM_ZERO_INIT float axisError[XYZ_AXIS_COUNT];
|
|
static FAST_RAM_ZERO_INIT float acGain;
|
|
static FAST_RAM_ZERO_INIT float acLimit;
|
|
static FAST_RAM_ZERO_INIT float acErrorLimit;
|
|
static FAST_RAM_ZERO_INIT float acCutoff;
|
|
static FAST_RAM_ZERO_INIT pt1Filter_t acLpf[XYZ_AXIS_COUNT];
|
|
#endif
|
|
|
|
#if defined(USE_D_MIN)
|
|
static FAST_RAM_ZERO_INIT biquadFilter_t dMinRange[XYZ_AXIS_COUNT];
|
|
static FAST_RAM_ZERO_INIT pt1Filter_t dMinLowpass[XYZ_AXIS_COUNT];
|
|
#endif
|
|
|
|
#ifdef USE_RC_SMOOTHING_FILTER
|
|
static FAST_RAM_ZERO_INIT pt1Filter_t setpointDerivativePt1[XYZ_AXIS_COUNT];
|
|
static FAST_RAM_ZERO_INIT biquadFilter_t setpointDerivativeBiquad[XYZ_AXIS_COUNT];
|
|
static FAST_RAM_ZERO_INIT bool setpointDerivativeLpfInitialized;
|
|
static FAST_RAM_ZERO_INIT uint8_t rcSmoothingDebugAxis;
|
|
static FAST_RAM_ZERO_INIT uint8_t rcSmoothingFilterType;
|
|
#endif // USE_RC_SMOOTHING_FILTER
|
|
|
|
#ifdef USE_AIRMODE_LPF
|
|
static FAST_RAM_ZERO_INIT pt1Filter_t airmodeThrottleLpf1;
|
|
static FAST_RAM_ZERO_INIT pt1Filter_t airmodeThrottleLpf2;
|
|
#endif
|
|
|
|
static FAST_RAM_ZERO_INIT pt1Filter_t antiGravityThrottleLpf;
|
|
|
|
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
|
|
dtermNotchApplyFn = nullFilterApply;
|
|
dtermLowpassApplyFn = nullFilterApply;
|
|
ptermYawLowpassApplyFn = nullFilterApply;
|
|
return;
|
|
}
|
|
|
|
const uint32_t pidFrequencyNyquist = 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) {
|
|
dtermNotchApplyFn = (filterApplyFnPtr)biquadFilterApply;
|
|
const float notchQ = filterGetNotchQ(dTermNotchHz, pidProfile->dterm_notch_cutoff);
|
|
for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
|
|
biquadFilterInit(&dtermNotch[axis], dTermNotchHz, targetPidLooptime, notchQ, FILTER_NOTCH);
|
|
}
|
|
} else {
|
|
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:
|
|
dtermLowpassApplyFn = (filterApplyFnPtr)pt1FilterApply;
|
|
for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
|
|
pt1FilterInit(&dtermLowpass[axis].pt1Filter, pt1FilterGain(dterm_lowpass_hz, dT));
|
|
}
|
|
break;
|
|
case FILTER_BIQUAD:
|
|
#ifdef USE_DYN_LPF
|
|
dtermLowpassApplyFn = (filterApplyFnPtr)biquadFilterApplyDF1;
|
|
#else
|
|
dtermLowpassApplyFn = (filterApplyFnPtr)biquadFilterApply;
|
|
#endif
|
|
for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
|
|
biquadFilterInitLPF(&dtermLowpass[axis].biquadFilter, dterm_lowpass_hz, targetPidLooptime);
|
|
}
|
|
break;
|
|
default:
|
|
dtermLowpassApplyFn = nullFilterApply;
|
|
break;
|
|
}
|
|
} else {
|
|
dtermLowpassApplyFn = nullFilterApply;
|
|
}
|
|
|
|
//2nd Dterm Lowpass Filter
|
|
if (pidProfile->dterm_lowpass2_hz == 0 || pidProfile->dterm_lowpass2_hz > pidFrequencyNyquist) {
|
|
dtermLowpass2ApplyFn = nullFilterApply;
|
|
} else {
|
|
switch (pidProfile->dterm_filter2_type) {
|
|
case FILTER_PT1:
|
|
dtermLowpass2ApplyFn = (filterApplyFnPtr)pt1FilterApply;
|
|
for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
|
|
pt1FilterInit(&dtermLowpass2[axis].pt1Filter, pt1FilterGain(pidProfile->dterm_lowpass2_hz, dT));
|
|
}
|
|
break;
|
|
case FILTER_BIQUAD:
|
|
dtermLowpass2ApplyFn = (filterApplyFnPtr)biquadFilterApply;
|
|
for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
|
|
biquadFilterInitLPF(&dtermLowpass2[axis].biquadFilter, pidProfile->dterm_lowpass2_hz, targetPidLooptime);
|
|
}
|
|
break;
|
|
default:
|
|
dtermLowpass2ApplyFn = nullFilterApply;
|
|
break;
|
|
}
|
|
}
|
|
|
|
if (pidProfile->yaw_lowpass_hz == 0 || pidProfile->yaw_lowpass_hz > pidFrequencyNyquist) {
|
|
ptermYawLowpassApplyFn = nullFilterApply;
|
|
} else {
|
|
ptermYawLowpassApplyFn = (filterApplyFnPtr)pt1FilterApply;
|
|
pt1FilterInit(&ptermYawLowpass, pt1FilterGain(pidProfile->yaw_lowpass_hz, dT));
|
|
}
|
|
|
|
#if defined(USE_THROTTLE_BOOST)
|
|
pt1FilterInit(&throttleLpf, pt1FilterGain(pidProfile->throttle_boost_cutoff, dT));
|
|
#endif
|
|
#if defined(USE_ITERM_RELAX)
|
|
if (itermRelax) {
|
|
for (int i = 0; i < XYZ_AXIS_COUNT; i++) {
|
|
pt1FilterInit(&windupLpf[i], pt1FilterGain(itermRelaxCutoff, dT));
|
|
}
|
|
}
|
|
#endif
|
|
#if defined(USE_ABSOLUTE_CONTROL)
|
|
if (itermRelax) {
|
|
for (int i = 0; i < XYZ_AXIS_COUNT; i++) {
|
|
pt1FilterInit(&acLpf[i], pt1FilterGain(acCutoff, 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(&dMinRange[axis], D_MIN_RANGE_HZ, targetPidLooptime);
|
|
pt1FilterInit(&dMinLowpass[axis], pt1FilterGain(D_MIN_LOWPASS_HZ, dT));
|
|
}
|
|
#endif
|
|
#if defined(USE_AIRMODE_LPF)
|
|
if (pidProfile->transient_throttle_limit) {
|
|
pt1FilterInit(&airmodeThrottleLpf1, pt1FilterGain(7.0f, dT));
|
|
pt1FilterInit(&airmodeThrottleLpf2, pt1FilterGain(20.0f, dT));
|
|
}
|
|
#endif
|
|
|
|
pt1FilterInit(&antiGravityThrottleLpf, pt1FilterGain(ANTI_GRAVITY_THROTTLE_FILTER_CUTOFF, dT));
|
|
}
|
|
|
|
#ifdef USE_RC_SMOOTHING_FILTER
|
|
void pidInitSetpointDerivativeLpf(uint16_t filterCutoff, uint8_t debugAxis, uint8_t filterType)
|
|
{
|
|
rcSmoothingDebugAxis = debugAxis;
|
|
rcSmoothingFilterType = filterType;
|
|
if ((filterCutoff > 0) && (rcSmoothingFilterType != RC_SMOOTHING_DERIVATIVE_OFF)) {
|
|
setpointDerivativeLpfInitialized = true;
|
|
for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
|
|
switch (rcSmoothingFilterType) {
|
|
case RC_SMOOTHING_DERIVATIVE_PT1:
|
|
pt1FilterInit(&setpointDerivativePt1[axis], pt1FilterGain(filterCutoff, dT));
|
|
break;
|
|
case RC_SMOOTHING_DERIVATIVE_BIQUAD:
|
|
biquadFilterInitLPF(&setpointDerivativeBiquad[axis], filterCutoff, targetPidLooptime);
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
void pidUpdateSetpointDerivativeLpf(uint16_t filterCutoff)
|
|
{
|
|
if ((filterCutoff > 0) && (rcSmoothingFilterType != RC_SMOOTHING_DERIVATIVE_OFF)) {
|
|
for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
|
|
switch (rcSmoothingFilterType) {
|
|
case RC_SMOOTHING_DERIVATIVE_PT1:
|
|
pt1FilterUpdateCutoff(&setpointDerivativePt1[axis], pt1FilterGain(filterCutoff, dT));
|
|
break;
|
|
case RC_SMOOTHING_DERIVATIVE_BIQUAD:
|
|
biquadFilterUpdateLPF(&setpointDerivativeBiquad[axis], filterCutoff, targetPidLooptime);
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
#endif // USE_RC_SMOOTHING_FILTER
|
|
|
|
typedef struct pidCoefficient_s {
|
|
float Kp;
|
|
float Ki;
|
|
float Kd;
|
|
float Kf;
|
|
} pidCoefficient_t;
|
|
|
|
static FAST_RAM_ZERO_INIT pidCoefficient_t pidCoefficient[XYZ_AXIS_COUNT];
|
|
static FAST_RAM_ZERO_INIT float maxVelocity[XYZ_AXIS_COUNT];
|
|
static FAST_RAM_ZERO_INIT float feedForwardTransition;
|
|
static FAST_RAM_ZERO_INIT float levelGain, horizonGain, horizonTransition, horizonCutoffDegrees, horizonFactorRatio;
|
|
static FAST_RAM_ZERO_INIT float itermWindupPointInv;
|
|
static FAST_RAM_ZERO_INIT uint8_t horizonTiltExpertMode;
|
|
static FAST_RAM_ZERO_INIT timeDelta_t crashTimeLimitUs;
|
|
static FAST_RAM_ZERO_INIT timeDelta_t crashTimeDelayUs;
|
|
static FAST_RAM_ZERO_INIT int32_t crashRecoveryAngleDeciDegrees;
|
|
static FAST_RAM_ZERO_INIT float crashRecoveryRate;
|
|
static FAST_RAM_ZERO_INIT float crashDtermThreshold;
|
|
static FAST_RAM_ZERO_INIT float crashGyroThreshold;
|
|
static FAST_RAM_ZERO_INIT float crashSetpointThreshold;
|
|
static FAST_RAM_ZERO_INIT float crashLimitYaw;
|
|
static FAST_RAM_ZERO_INIT float itermLimit;
|
|
#if defined(USE_THROTTLE_BOOST)
|
|
FAST_RAM_ZERO_INIT float throttleBoost;
|
|
pt1Filter_t throttleLpf;
|
|
#endif
|
|
static FAST_RAM_ZERO_INIT bool itermRotation;
|
|
|
|
#if defined(USE_SMART_FEEDFORWARD)
|
|
static FAST_RAM_ZERO_INIT bool smartFeedforward;
|
|
#endif
|
|
|
|
#ifdef USE_LAUNCH_CONTROL
|
|
static FAST_RAM_ZERO_INIT uint8_t launchControlMode;
|
|
static FAST_RAM_ZERO_INIT uint8_t launchControlAngleLimit;
|
|
static FAST_RAM_ZERO_INIT float launchControlKi;
|
|
#endif
|
|
|
|
#ifdef USE_INTEGRATED_YAW_CONTROL
|
|
static FAST_RAM_ZERO_INIT bool useIntegratedYaw;
|
|
static FAST_RAM_ZERO_INIT uint8_t integratedYawRelax;
|
|
#endif
|
|
|
|
void pidResetIterm(void)
|
|
{
|
|
for (int axis = 0; axis < 3; axis++) {
|
|
pidData[axis].I = 0.0f;
|
|
#if defined(USE_ABSOLUTE_CONTROL)
|
|
axisError[axis] = 0.0f;
|
|
#endif
|
|
}
|
|
}
|
|
|
|
#ifdef USE_ACRO_TRAINER
|
|
static FAST_RAM_ZERO_INIT float acroTrainerAngleLimit;
|
|
static FAST_RAM_ZERO_INIT float acroTrainerLookaheadTime;
|
|
static FAST_RAM_ZERO_INIT uint8_t acroTrainerDebugAxis;
|
|
static FAST_RAM_ZERO_INIT bool acroTrainerActive;
|
|
static FAST_RAM_ZERO_INIT int acroTrainerAxisState[2]; // only need roll and pitch
|
|
static FAST_RAM_ZERO_INIT float acroTrainerGain;
|
|
#endif // USE_ACRO_TRAINER
|
|
|
|
#ifdef USE_THRUST_LINEARIZATION
|
|
FAST_RAM_ZERO_INIT float thrustLinearization;
|
|
FAST_RAM_ZERO_INIT float thrustLinearizationReciprocal;
|
|
FAST_RAM_ZERO_INIT float thrustLinearizationB;
|
|
#endif
|
|
|
|
void pidUpdateAntiGravityThrottleFilter(float throttle)
|
|
{
|
|
if (antiGravityMode == ANTI_GRAVITY_SMOOTH) {
|
|
antiGravityThrottleHpf = throttle - pt1FilterApply(&antiGravityThrottleLpf, throttle);
|
|
}
|
|
}
|
|
|
|
#ifdef USE_DYN_LPF
|
|
static FAST_RAM uint8_t dynLpfFilter = DYN_LPF_NONE;
|
|
static FAST_RAM_ZERO_INIT uint16_t dynLpfMin;
|
|
static FAST_RAM_ZERO_INIT uint16_t dynLpfMax;
|
|
#endif
|
|
|
|
#ifdef USE_D_MIN
|
|
static FAST_RAM_ZERO_INIT float dMinPercent[XYZ_AXIS_COUNT];
|
|
static FAST_RAM_ZERO_INIT float dMinGyroGain;
|
|
static FAST_RAM_ZERO_INIT float dMinSetpointGain;
|
|
#endif
|
|
|
|
void pidInitConfig(const pidProfile_t *pidProfile)
|
|
{
|
|
if (pidProfile->feedForwardTransition == 0) {
|
|
feedForwardTransition = 0;
|
|
} else {
|
|
feedForwardTransition = 100.0f / pidProfile->feedForwardTransition;
|
|
}
|
|
for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
|
|
pidCoefficient[axis].Kp = PTERM_SCALE * pidProfile->pid[axis].P;
|
|
pidCoefficient[axis].Ki = ITERM_SCALE * pidProfile->pid[axis].I;
|
|
pidCoefficient[axis].Kd = DTERM_SCALE * pidProfile->pid[axis].D;
|
|
pidCoefficient[axis].Kf = FEEDFORWARD_SCALE * (pidProfile->pid[axis].F / 100.0f);
|
|
}
|
|
#ifdef USE_INTEGRATED_YAW_CONTROL
|
|
if (!pidProfile->use_integrated_yaw)
|
|
#endif
|
|
{
|
|
pidCoefficient[FD_YAW].Ki *= 2.5f;
|
|
}
|
|
|
|
levelGain = pidProfile->pid[PID_LEVEL].P / 10.0f;
|
|
horizonGain = pidProfile->pid[PID_LEVEL].I / 10.0f;
|
|
horizonTransition = (float)pidProfile->pid[PID_LEVEL].D;
|
|
horizonTiltExpertMode = pidProfile->horizon_tilt_expert_mode;
|
|
horizonCutoffDegrees = (175 - pidProfile->horizon_tilt_effect) * 1.8f;
|
|
horizonFactorRatio = (100 - pidProfile->horizon_tilt_effect) * 0.01f;
|
|
maxVelocity[FD_ROLL] = maxVelocity[FD_PITCH] = pidProfile->rateAccelLimit * 100 * dT;
|
|
maxVelocity[FD_YAW] = pidProfile->yawRateAccelLimit * 100 * dT;
|
|
itermWindupPointInv = 1.0f;
|
|
if (pidProfile->itermWindupPointPercent < 100) {
|
|
const float itermWindupPoint = pidProfile->itermWindupPointPercent / 100.0f;
|
|
itermWindupPointInv = 1.0f / (1.0f - itermWindupPoint);
|
|
}
|
|
itermAcceleratorGain = pidProfile->itermAcceleratorGain;
|
|
crashTimeLimitUs = pidProfile->crash_time * 1000;
|
|
crashTimeDelayUs = pidProfile->crash_delay * 1000;
|
|
crashRecoveryAngleDeciDegrees = pidProfile->crash_recovery_angle * 10;
|
|
crashRecoveryRate = pidProfile->crash_recovery_rate;
|
|
crashGyroThreshold = pidProfile->crash_gthreshold;
|
|
crashDtermThreshold = pidProfile->crash_dthreshold;
|
|
crashSetpointThreshold = pidProfile->crash_setpoint_threshold;
|
|
crashLimitYaw = pidProfile->crash_limit_yaw;
|
|
itermLimit = pidProfile->itermLimit;
|
|
#if defined(USE_THROTTLE_BOOST)
|
|
throttleBoost = pidProfile->throttle_boost * 0.1f;
|
|
#endif
|
|
itermRotation = pidProfile->iterm_rotation;
|
|
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.
|
|
antiGravityOsdCutoff = 1.0f;
|
|
if (antiGravityMode == ANTI_GRAVITY_SMOOTH) {
|
|
antiGravityOsdCutoff += ((itermAcceleratorGain - 1000) / 1000.0f) * 0.25f;
|
|
}
|
|
|
|
#if defined(USE_SMART_FEEDFORWARD)
|
|
smartFeedforward = pidProfile->smart_feedforward;
|
|
#endif
|
|
|
|
#if defined(USE_ITERM_RELAX)
|
|
itermRelax = pidProfile->iterm_relax;
|
|
itermRelaxType = pidProfile->iterm_relax_type;
|
|
itermRelaxCutoff = pidProfile->iterm_relax_cutoff;
|
|
// adapt setpoint threshold to user changes from default cutoff value
|
|
itermRelaxSetpointThreshold = ITERM_RELAX_SETPOINT_THRESHOLD * ITERM_RELAX_CUTOFF_DEFAULT / itermRelaxCutoff;
|
|
#endif
|
|
|
|
#ifdef USE_ACRO_TRAINER
|
|
acroTrainerAngleLimit = pidProfile->acro_trainer_angle_limit;
|
|
acroTrainerLookaheadTime = (float)pidProfile->acro_trainer_lookahead_ms / 1000.0f;
|
|
acroTrainerDebugAxis = pidProfile->acro_trainer_debug_axis;
|
|
acroTrainerGain = (float)pidProfile->acro_trainer_gain / 10.0f;
|
|
#endif // USE_ACRO_TRAINER
|
|
|
|
#if defined(USE_ABSOLUTE_CONTROL)
|
|
acGain = (float)pidProfile->abs_control_gain;
|
|
acLimit = (float)pidProfile->abs_control_limit;
|
|
acErrorLimit = (float)pidProfile->abs_control_error_limit;
|
|
acCutoff = (float)pidProfile->abs_control_cutoff;
|
|
#endif
|
|
|
|
#ifdef USE_DYN_LPF
|
|
if (pidProfile->dyn_lpf_dterm_min_hz > 0) {
|
|
switch (pidProfile->dterm_filter_type) {
|
|
case FILTER_PT1:
|
|
dynLpfFilter = DYN_LPF_PT1;
|
|
break;
|
|
case FILTER_BIQUAD:
|
|
dynLpfFilter = DYN_LPF_BIQUAD;
|
|
break;
|
|
default:
|
|
dynLpfFilter = DYN_LPF_NONE;
|
|
break;
|
|
}
|
|
} else {
|
|
dynLpfFilter = DYN_LPF_NONE;
|
|
}
|
|
dynLpfMin = pidProfile->dyn_lpf_dterm_min_hz;
|
|
dynLpfMax = pidProfile->dyn_lpf_dterm_max_hz;
|
|
#endif
|
|
|
|
#ifdef USE_LAUNCH_CONTROL
|
|
launchControlMode = pidProfile->launchControlMode;
|
|
if (sensors(SENSOR_ACC)) {
|
|
launchControlAngleLimit = pidProfile->launchControlAngleLimit;
|
|
} else {
|
|
launchControlAngleLimit = 0;
|
|
}
|
|
launchControlKi = ITERM_SCALE * pidProfile->launchControlGain;
|
|
#endif
|
|
|
|
#ifdef USE_INTEGRATED_YAW_CONTROL
|
|
useIntegratedYaw = pidProfile->use_integrated_yaw;
|
|
integratedYawRelax = pidProfile->integrated_yaw_relax;
|
|
#endif
|
|
|
|
#ifdef USE_THRUST_LINEARIZATION
|
|
thrustLinearization = pidProfile->thrustLinearization / 100.0f;
|
|
if (thrustLinearization != 0.0f) {
|
|
thrustLinearizationReciprocal = 1.0f / thrustLinearization;
|
|
thrustLinearizationB = (1.0f - thrustLinearization) / (2.0f * 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)) {
|
|
dMinPercent[axis] = dMin / (float)(pidProfile->pid[axis].D);
|
|
} else {
|
|
dMinPercent[axis] = 0;
|
|
}
|
|
}
|
|
dMinGyroGain = pidProfile->d_min_gain * D_MIN_GAIN_FACTOR / D_MIN_LOWPASS_HZ;
|
|
dMinSetpointGain = pidProfile->d_min_gain * D_MIN_SETPOINT_GAIN_FACTOR * pidProfile->d_min_advance * pidFrequency / (100 * D_MIN_LOWPASS_HZ);
|
|
// lowpass included inversely in gain since stronger lowpass decreases peak effect
|
|
#endif
|
|
#if defined(USE_AIRMODE_LPF)
|
|
airmodeThrottleOffsetLimit = pidProfile->transient_throttle_limit / 100.0f;
|
|
#endif
|
|
}
|
|
|
|
void pidInit(const pidProfile_t *pidProfile)
|
|
{
|
|
pidSetTargetLooptime(gyro.targetLooptime * pidConfig()->pid_process_denom); // Initialize pid looptime
|
|
pidInitFilters(pidProfile);
|
|
pidInitConfig(pidProfile);
|
|
#ifdef USE_RPM_FILTER
|
|
rpmFilterInit(rpmFilterConfig());
|
|
#endif
|
|
}
|
|
|
|
#ifdef USE_ACRO_TRAINER
|
|
void pidAcroTrainerInit(void)
|
|
{
|
|
acroTrainerAxisState[FD_ROLL] = 0;
|
|
acroTrainerAxisState[FD_PITCH] = 0;
|
|
}
|
|
#endif // USE_ACRO_TRAINER
|
|
|
|
#ifdef USE_THRUST_LINEARIZATION
|
|
float pidCompensateThrustLinearization(float throttle)
|
|
{
|
|
if (thrustLinearization != 0.0f) {
|
|
throttle = throttle * (throttle * thrustLinearization + 1.0f - thrustLinearization);
|
|
}
|
|
return throttle;
|
|
}
|
|
|
|
float pidApplyThrustLinearization(float motorOutput)
|
|
{
|
|
if (thrustLinearization != 0.0f) {
|
|
if (motorOutput > 0.0f) {
|
|
motorOutput = sqrtf(motorOutput * thrustLinearizationReciprocal +
|
|
thrustLinearizationB * thrustLinearizationB) - thrustLinearizationB;
|
|
}
|
|
}
|
|
return motorOutput;
|
|
}
|
|
#endif
|
|
|
|
void pidCopyProfile(uint8_t dstPidProfileIndex, uint8_t srcPidProfileIndex)
|
|
{
|
|
if ((dstPidProfileIndex < PID_PROFILE_COUNT-1 && srcPidProfileIndex < PID_PROFILE_COUNT-1)
|
|
&& dstPidProfileIndex != srcPidProfileIndex
|
|
) {
|
|
memcpy(pidProfilesMutable(dstPidProfileIndex), pidProfilesMutable(srcPidProfileIndex), sizeof(pidProfile_t));
|
|
}
|
|
}
|
|
|
|
#if defined(USE_ACC)
|
|
// calculates strength of horizon leveling; 0 = none, 1.0 = most leveling
|
|
STATIC_UNIT_TESTED float calcHorizonLevelStrength(void)
|
|
{
|
|
// start with 1.0 at center stick, 0.0 at max stick deflection:
|
|
float horizonLevelStrength = 1.0f - MAX(getRcDeflectionAbs(FD_ROLL), getRcDeflectionAbs(FD_PITCH));
|
|
|
|
// 0 at level, 90 at vertical, 180 at inverted (degrees):
|
|
const float currentInclination = MAX(ABS(attitude.values.roll), ABS(attitude.values.pitch)) / 10.0f;
|
|
|
|
// horizonTiltExpertMode: 0 = leveling always active when sticks centered,
|
|
// 1 = leveling can be totally off when inverted
|
|
if (horizonTiltExpertMode) {
|
|
if (horizonTransition > 0 && horizonCutoffDegrees > 0) {
|
|
// if d_level > 0 and horizonTiltEffect < 175
|
|
// horizonCutoffDegrees: 0 to 125 => 270 to 90 (represents where leveling goes to zero)
|
|
// inclinationLevelRatio (0.0 to 1.0) is smaller (less leveling)
|
|
// for larger inclinations; 0.0 at horizonCutoffDegrees value:
|
|
const float inclinationLevelRatio = constrainf((horizonCutoffDegrees-currentInclination) / horizonCutoffDegrees, 0, 1);
|
|
// apply configured horizon sensitivity:
|
|
// when stick is near center (horizonLevelStrength ~= 1.0)
|
|
// H_sensitivity value has little effect,
|
|
// when stick is deflected (horizonLevelStrength near 0.0)
|
|
// H_sensitivity value has more effect:
|
|
horizonLevelStrength = (horizonLevelStrength - 1) * 100 / horizonTransition + 1;
|
|
// apply inclination ratio, which may lower leveling
|
|
// to zero regardless of stick position:
|
|
horizonLevelStrength *= inclinationLevelRatio;
|
|
} else { // d_level=0 or horizon_tilt_effect>=175 means no leveling
|
|
horizonLevelStrength = 0;
|
|
}
|
|
} else { // horizon_tilt_expert_mode = 0 (leveling always active when sticks centered)
|
|
float sensitFact;
|
|
if (horizonFactorRatio < 1.01f) { // if horizonTiltEffect > 0
|
|
// horizonFactorRatio: 1.0 to 0.0 (larger means more leveling)
|
|
// inclinationLevelRatio (0.0 to 1.0) is smaller (less leveling)
|
|
// for larger inclinations, goes to 1.0 at inclination==level:
|
|
const float inclinationLevelRatio = (180-currentInclination)/180 * (1.0f-horizonFactorRatio) + horizonFactorRatio;
|
|
// apply ratio to configured horizon sensitivity:
|
|
sensitFact = horizonTransition * inclinationLevelRatio;
|
|
} else { // horizonTiltEffect=0 for "old" functionality
|
|
sensitFact = horizonTransition;
|
|
}
|
|
|
|
if (sensitFact <= 0) { // zero means no leveling
|
|
horizonLevelStrength = 0;
|
|
} else {
|
|
// when stick is near center (horizonLevelStrength ~= 1.0)
|
|
// sensitFact value has little effect,
|
|
// when stick is deflected (horizonLevelStrength near 0.0)
|
|
// sensitFact value has more effect:
|
|
horizonLevelStrength = ((horizonLevelStrength - 1) * (100 / sensitFact)) + 1;
|
|
}
|
|
}
|
|
return constrainf(horizonLevelStrength, 0, 1);
|
|
}
|
|
|
|
STATIC_UNIT_TESTED float pidLevel(int axis, const pidProfile_t *pidProfile, const rollAndPitchTrims_t *angleTrim, float currentPidSetpoint) {
|
|
// calculate error angle and limit the angle to the max inclination
|
|
// rcDeflection is in range [-1.0, 1.0]
|
|
float angle = pidProfile->levelAngleLimit * getRcDeflection(axis);
|
|
#ifdef USE_GPS_RESCUE
|
|
angle += gpsRescueAngle[axis] / 100; // ANGLE IS IN CENTIDEGREES
|
|
#endif
|
|
angle = constrainf(angle, -pidProfile->levelAngleLimit, pidProfile->levelAngleLimit);
|
|
const float errorAngle = angle - ((attitude.raw[axis] - angleTrim->raw[axis]) / 10.0f);
|
|
if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(GPS_RESCUE_MODE)) {
|
|
// ANGLE mode - control is angle based
|
|
currentPidSetpoint = errorAngle * levelGain;
|
|
} else {
|
|
// HORIZON mode - mix of ANGLE and ACRO modes
|
|
// mix in errorAngle to currentPidSetpoint to add a little auto-level feel
|
|
const float horizonLevelStrength = calcHorizonLevelStrength();
|
|
currentPidSetpoint = currentPidSetpoint + (errorAngle * horizonGain * horizonLevelStrength);
|
|
}
|
|
return currentPidSetpoint;
|
|
}
|
|
|
|
static timeUs_t crashDetectedAtUs;
|
|
|
|
static void handleCrashRecovery(
|
|
const pidCrashRecovery_e crash_recovery, const rollAndPitchTrims_t *angleTrim,
|
|
const int axis, const timeUs_t currentTimeUs, const float gyroRate, float *currentPidSetpoint, float *errorRate)
|
|
{
|
|
if (inCrashRecoveryMode && cmpTimeUs(currentTimeUs, crashDetectedAtUs) > crashTimeDelayUs) {
|
|
if (crash_recovery == PID_CRASH_RECOVERY_BEEP) {
|
|
BEEP_ON;
|
|
}
|
|
if (axis == FD_YAW) {
|
|
*errorRate = constrainf(*errorRate, -crashLimitYaw, crashLimitYaw);
|
|
} else {
|
|
// on roll and pitch axes calculate currentPidSetpoint and errorRate to level the aircraft to recover from crash
|
|
if (sensors(SENSOR_ACC)) {
|
|
// errorAngle is deviation from horizontal
|
|
const float errorAngle = -(attitude.raw[axis] - angleTrim->raw[axis]) / 10.0f;
|
|
*currentPidSetpoint = errorAngle * levelGain;
|
|
*errorRate = *currentPidSetpoint - gyroRate;
|
|
}
|
|
}
|
|
// reset iterm, since accumulated error before crash is now meaningless
|
|
// and iterm windup during crash recovery can be extreme, especially on yaw axis
|
|
pidData[axis].I = 0.0f;
|
|
if (cmpTimeUs(currentTimeUs, crashDetectedAtUs) > crashTimeLimitUs
|
|
|| (getMotorMixRange() < 1.0f
|
|
&& fabsf(gyro.gyroADCf[FD_ROLL]) < crashRecoveryRate
|
|
&& fabsf(gyro.gyroADCf[FD_PITCH]) < crashRecoveryRate
|
|
&& fabsf(gyro.gyroADCf[FD_YAW]) < crashRecoveryRate)) {
|
|
if (sensors(SENSOR_ACC)) {
|
|
// check aircraft nearly level
|
|
if (ABS(attitude.raw[FD_ROLL] - angleTrim->raw[FD_ROLL]) < crashRecoveryAngleDeciDegrees
|
|
&& ABS(attitude.raw[FD_PITCH] - angleTrim->raw[FD_PITCH]) < crashRecoveryAngleDeciDegrees) {
|
|
inCrashRecoveryMode = false;
|
|
BEEP_OFF;
|
|
}
|
|
} else {
|
|
inCrashRecoveryMode = false;
|
|
BEEP_OFF;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
static void detectAndSetCrashRecovery(
|
|
const pidCrashRecovery_e crash_recovery, const int axis,
|
|
const timeUs_t currentTimeUs, const float delta, const float errorRate)
|
|
{
|
|
// if crash recovery is on and accelerometer enabled and there is no gyro overflow, then check for a crash
|
|
// no point in trying to recover if the crash is so severe that the gyro overflows
|
|
if ((crash_recovery || FLIGHT_MODE(GPS_RESCUE_MODE)) && !gyroOverflowDetected()) {
|
|
if (ARMING_FLAG(ARMED)) {
|
|
if (getMotorMixRange() >= 1.0f && !inCrashRecoveryMode
|
|
&& fabsf(delta) > crashDtermThreshold
|
|
&& fabsf(errorRate) > crashGyroThreshold
|
|
&& fabsf(getSetpointRate(axis)) < crashSetpointThreshold) {
|
|
inCrashRecoveryMode = true;
|
|
crashDetectedAtUs = currentTimeUs;
|
|
}
|
|
if (inCrashRecoveryMode && cmpTimeUs(currentTimeUs, crashDetectedAtUs) < crashTimeDelayUs && (fabsf(errorRate) < crashGyroThreshold
|
|
|| fabsf(getSetpointRate(axis)) > crashSetpointThreshold)) {
|
|
inCrashRecoveryMode = false;
|
|
BEEP_OFF;
|
|
}
|
|
} else if (inCrashRecoveryMode) {
|
|
inCrashRecoveryMode = false;
|
|
BEEP_OFF;
|
|
}
|
|
}
|
|
}
|
|
#endif // USE_ACC
|
|
|
|
#ifdef USE_ACRO_TRAINER
|
|
|
|
int acroTrainerSign(float x)
|
|
{
|
|
return x > 0 ? 1 : -1;
|
|
}
|
|
|
|
// Acro Trainer - Manipulate the setPoint to limit axis angle while in acro mode
|
|
// There are three states:
|
|
// 1. Current angle has exceeded limit
|
|
// Apply correction to return to limit (similar to pidLevel)
|
|
// 2. Future overflow has been projected based on current angle and gyro rate
|
|
// Manage the setPoint to control the gyro rate as the actual angle approaches the limit (try to prevent overshoot)
|
|
// 3. If no potential overflow is detected, then return the original setPoint
|
|
|
|
// Use the FAST_CODE_NOINLINE directive to avoid this code from being inlined into ITCM RAM. We accept the
|
|
// performance decrease when Acro Trainer mode is active under the assumption that user is unlikely to be
|
|
// expecting ultimate flight performance at very high loop rates when in this mode.
|
|
static FAST_CODE_NOINLINE float applyAcroTrainer(int axis, const rollAndPitchTrims_t *angleTrim, float setPoint)
|
|
{
|
|
float ret = setPoint;
|
|
|
|
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);
|
|
const float currentAngle = (attitude.raw[axis] - angleTrim->raw[axis]) / 10.0f;
|
|
const int angleSign = acroTrainerSign(currentAngle);
|
|
|
|
if ((acroTrainerAxisState[axis] != 0) && (acroTrainerAxisState[axis] != setpointSign)) { // stick has reversed - stop limiting
|
|
acroTrainerAxisState[axis] = 0;
|
|
}
|
|
|
|
// Limit and correct the angle when it exceeds the limit
|
|
if ((fabsf(currentAngle) > acroTrainerAngleLimit) && (acroTrainerAxisState[axis] == 0)) {
|
|
if (angleSign == setpointSign) {
|
|
acroTrainerAxisState[axis] = angleSign;
|
|
resetIterm = true;
|
|
}
|
|
}
|
|
|
|
if (acroTrainerAxisState[axis] != 0) {
|
|
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
|
|
// gyro angular rate using a sliding window based on gyro rate (faster rotation means larger window.
|
|
// 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;
|
|
projectedAngle = (gyro.gyroADCf[axis] * checkInterval) + currentAngle;
|
|
const int projectedAngleSign = acroTrainerSign(projectedAngle);
|
|
if ((fabsf(projectedAngle) > acroTrainerAngleLimit) && (projectedAngleSign == setpointSign)) {
|
|
ret = ((acroTrainerAngleLimit * projectedAngleSign) - projectedAngle) * acroTrainerGain;
|
|
resetIterm = true;
|
|
}
|
|
}
|
|
|
|
if (resetIterm) {
|
|
pidData[axis].I = 0;
|
|
}
|
|
|
|
if (axis == acroTrainerDebugAxis) {
|
|
DEBUG_SET(DEBUG_ACRO_TRAINER, 0, lrintf(currentAngle * 10.0f));
|
|
DEBUG_SET(DEBUG_ACRO_TRAINER, 1, acroTrainerAxisState[axis]);
|
|
DEBUG_SET(DEBUG_ACRO_TRAINER, 2, lrintf(ret));
|
|
DEBUG_SET(DEBUG_ACRO_TRAINER, 3, lrintf(projectedAngle * 10.0f));
|
|
}
|
|
}
|
|
|
|
return ret;
|
|
}
|
|
#endif // USE_ACRO_TRAINER
|
|
|
|
static float accelerationLimit(int axis, float currentPidSetpoint)
|
|
{
|
|
static float previousSetpoint[XYZ_AXIS_COUNT];
|
|
const float currentVelocity = currentPidSetpoint - previousSetpoint[axis];
|
|
|
|
if (fabsf(currentVelocity) > maxVelocity[axis]) {
|
|
currentPidSetpoint = (currentVelocity > 0) ? previousSetpoint[axis] + maxVelocity[axis] : previousSetpoint[axis] - maxVelocity[axis];
|
|
}
|
|
|
|
previousSetpoint[axis] = currentPidSetpoint;
|
|
return currentPidSetpoint;
|
|
}
|
|
|
|
static void rotateVector(float v[XYZ_AXIS_COUNT], float rotation[XYZ_AXIS_COUNT])
|
|
{
|
|
// rotate v around rotation vector rotation
|
|
// rotation in radians, all elements must be small
|
|
for (int i = 0; i < XYZ_AXIS_COUNT; i++) {
|
|
int i_1 = (i + 1) % 3;
|
|
int i_2 = (i + 2) % 3;
|
|
float newV = v[i_1] + v[i_2] * rotation[i];
|
|
v[i_2] -= v[i_1] * rotation[i];
|
|
v[i_1] = newV;
|
|
}
|
|
}
|
|
|
|
STATIC_UNIT_TESTED void rotateItermAndAxisError()
|
|
{
|
|
if (itermRotation
|
|
#if defined(USE_ABSOLUTE_CONTROL)
|
|
|| acGain > 0 || debugMode == DEBUG_AC_ERROR
|
|
#endif
|
|
) {
|
|
const float gyroToAngle = dT * RAD;
|
|
float rotationRads[XYZ_AXIS_COUNT];
|
|
for (int i = FD_ROLL; i <= FD_YAW; i++) {
|
|
rotationRads[i] = gyro.gyroADCf[i] * gyroToAngle;
|
|
}
|
|
#if defined(USE_ABSOLUTE_CONTROL)
|
|
if (acGain > 0 || debugMode == DEBUG_AC_ERROR) {
|
|
rotateVector(axisError, rotationRads);
|
|
}
|
|
#endif
|
|
if (itermRotation) {
|
|
float v[XYZ_AXIS_COUNT];
|
|
for (int i = 0; i < XYZ_AXIS_COUNT; i++) {
|
|
v[i] = pidData[i].I;
|
|
}
|
|
rotateVector(v, rotationRads );
|
|
for (int i = 0; i < XYZ_AXIS_COUNT; i++) {
|
|
pidData[i].I = v[i];
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
#ifdef USE_RC_SMOOTHING_FILTER
|
|
float FAST_CODE applyRcSmoothingDerivativeFilter(int axis, float pidSetpointDelta)
|
|
{
|
|
float ret = pidSetpointDelta;
|
|
if (axis == rcSmoothingDebugAxis) {
|
|
DEBUG_SET(DEBUG_RC_SMOOTHING, 1, lrintf(pidSetpointDelta * 100.0f));
|
|
}
|
|
if (setpointDerivativeLpfInitialized) {
|
|
switch (rcSmoothingFilterType) {
|
|
case RC_SMOOTHING_DERIVATIVE_PT1:
|
|
ret = pt1FilterApply(&setpointDerivativePt1[axis], pidSetpointDelta);
|
|
break;
|
|
case RC_SMOOTHING_DERIVATIVE_BIQUAD:
|
|
ret = biquadFilterApplyDF1(&setpointDerivativeBiquad[axis], pidSetpointDelta);
|
|
break;
|
|
}
|
|
if (axis == rcSmoothingDebugAxis) {
|
|
DEBUG_SET(DEBUG_RC_SMOOTHING, 2, lrintf(ret * 100.0f));
|
|
}
|
|
}
|
|
return ret;
|
|
}
|
|
#endif // USE_RC_SMOOTHING_FILTER
|
|
|
|
#ifdef USE_SMART_FEEDFORWARD
|
|
void FAST_CODE applySmartFeedforward(int axis)
|
|
{
|
|
if (smartFeedforward) {
|
|
if (pidData[axis].P * pidData[axis].F > 0) {
|
|
if (fabsf(pidData[axis].F) > fabsf(pidData[axis].P)) {
|
|
pidData[axis].P = 0;
|
|
} else {
|
|
pidData[axis].F = 0;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
#endif // USE_SMART_FEEDFORWARD
|
|
|
|
#if defined(USE_ITERM_RELAX)
|
|
#if defined(USE_ABSOLUTE_CONTROL)
|
|
STATIC_UNIT_TESTED void applyAbsoluteControl(const int axis, const float gyroRate, float *currentPidSetpoint, float *itermErrorRate)
|
|
{
|
|
if (acGain > 0 || debugMode == DEBUG_AC_ERROR) {
|
|
const float setpointLpf = pt1FilterApply(&acLpf[axis], *currentPidSetpoint);
|
|
const float setpointHpf = fabsf(*currentPidSetpoint - setpointLpf);
|
|
float acErrorRate = 0;
|
|
const float gmaxac = setpointLpf + 2 * setpointHpf;
|
|
const float gminac = setpointLpf - 2 * setpointHpf;
|
|
if (gyroRate >= gminac && gyroRate <= gmaxac) {
|
|
const float acErrorRate1 = gmaxac - gyroRate;
|
|
const float acErrorRate2 = gminac - gyroRate;
|
|
if (acErrorRate1 * axisError[axis] < 0) {
|
|
acErrorRate = acErrorRate1;
|
|
} else {
|
|
acErrorRate = acErrorRate2;
|
|
}
|
|
if (fabsf(acErrorRate * dT) > fabsf(axisError[axis]) ) {
|
|
acErrorRate = -axisError[axis] * pidFrequency;
|
|
}
|
|
} else {
|
|
acErrorRate = (gyroRate > gmaxac ? gmaxac : gminac ) - gyroRate;
|
|
}
|
|
|
|
if (isAirmodeActivated()) {
|
|
axisError[axis] = constrainf(axisError[axis] + acErrorRate * dT,
|
|
-acErrorLimit, acErrorLimit);
|
|
const float acCorrection = constrainf(axisError[axis] * acGain, -acLimit, acLimit);
|
|
*currentPidSetpoint += acCorrection;
|
|
*itermErrorRate += acCorrection;
|
|
DEBUG_SET(DEBUG_AC_CORRECTION, axis, lrintf(acCorrection * 10));
|
|
if (axis == FD_ROLL) {
|
|
DEBUG_SET(DEBUG_ITERM_RELAX, 3, lrintf(acCorrection * 10));
|
|
}
|
|
}
|
|
DEBUG_SET(DEBUG_AC_ERROR, axis, lrintf(axisError[axis] * 10));
|
|
}
|
|
}
|
|
#endif
|
|
|
|
STATIC_UNIT_TESTED void applyItermRelax(const int axis, const float iterm,
|
|
const float gyroRate, float *itermErrorRate, float *currentPidSetpoint)
|
|
{
|
|
const float setpointLpf = pt1FilterApply(&windupLpf[axis], *currentPidSetpoint);
|
|
const float setpointHpf = fabsf(*currentPidSetpoint - setpointLpf);
|
|
|
|
if (itermRelax) {
|
|
if (axis < FD_YAW || itermRelax == ITERM_RELAX_RPY || itermRelax == ITERM_RELAX_RPY_INC) {
|
|
const float itermRelaxFactor = MAX(0, 1 - setpointHpf / itermRelaxSetpointThreshold);
|
|
const bool isDecreasingI =
|
|
((iterm > 0) && (*itermErrorRate < 0)) || ((iterm < 0) && (*itermErrorRate > 0));
|
|
if ((itermRelax >= ITERM_RELAX_RP_INC) && isDecreasingI) {
|
|
// Do Nothing, use the precalculed itermErrorRate
|
|
} else if (itermRelaxType == ITERM_RELAX_SETPOINT) {
|
|
*itermErrorRate *= itermRelaxFactor;
|
|
} else if (itermRelaxType == ITERM_RELAX_GYRO ) {
|
|
*itermErrorRate = fapplyDeadband(setpointLpf - gyroRate, setpointHpf);
|
|
} else {
|
|
*itermErrorRate = 0.0f;
|
|
}
|
|
|
|
if (axis == FD_ROLL) {
|
|
DEBUG_SET(DEBUG_ITERM_RELAX, 0, lrintf(setpointHpf));
|
|
DEBUG_SET(DEBUG_ITERM_RELAX, 1, lrintf(itermRelaxFactor * 100.0f));
|
|
DEBUG_SET(DEBUG_ITERM_RELAX, 2, lrintf(*itermErrorRate));
|
|
}
|
|
}
|
|
|
|
#if defined(USE_ABSOLUTE_CONTROL)
|
|
applyAbsoluteControl(axis, gyroRate, currentPidSetpoint, itermErrorRate);
|
|
#endif
|
|
}
|
|
}
|
|
#endif
|
|
|
|
#ifdef USE_AIRMODE_LPF
|
|
void pidUpdateAirmodeLpf(float currentOffset)
|
|
{
|
|
if (airmodeThrottleOffsetLimit == 0.0f) {
|
|
return;
|
|
}
|
|
|
|
float offsetHpf = currentOffset * 2.5f;
|
|
offsetHpf = offsetHpf - pt1FilterApply(&airmodeThrottleLpf2, offsetHpf);
|
|
|
|
// During high frequency oscillation 2 * currentOffset averages to the offset required to avoid mirroring of the waveform
|
|
pt1FilterApply(&airmodeThrottleLpf1, offsetHpf);
|
|
// Bring offset up immediately so the filter only applies to the decline
|
|
if (currentOffset * airmodeThrottleLpf1.state >= 0 && fabsf(currentOffset) > airmodeThrottleLpf1.state) {
|
|
airmodeThrottleLpf1.state = currentOffset;
|
|
}
|
|
airmodeThrottleLpf1.state = constrainf(airmodeThrottleLpf1.state, -airmodeThrottleOffsetLimit, airmodeThrottleOffsetLimit);
|
|
}
|
|
|
|
float pidGetAirmodeThrottleOffset()
|
|
{
|
|
return airmodeThrottleLpf1.state;
|
|
}
|
|
#endif
|
|
|
|
#ifdef USE_LAUNCH_CONTROL
|
|
#define LAUNCH_CONTROL_MAX_RATE 100.0f
|
|
#define LAUNCH_CONTROL_MIN_RATE 5.0f
|
|
#define LAUNCH_CONTROL_ANGLE_WINDOW 10.0f // The remaining angle degrees where rate dampening starts
|
|
|
|
static float applyLaunchControl(int axis, const rollAndPitchTrims_t *angleTrim)
|
|
{
|
|
float ret = 0.0f;
|
|
|
|
// Scale the rates based on stick deflection only. Fixed rates with a max of 100deg/sec
|
|
// reached at 50% stick deflection. This keeps the launch control positioning consistent
|
|
// regardless of the user's rates.
|
|
if ((axis == FD_PITCH) || (launchControlMode != LAUNCH_CONTROL_MODE_PITCHONLY)) {
|
|
const float stickDeflection = constrainf(getRcDeflection(axis), -0.5f, 0.5f);
|
|
ret = LAUNCH_CONTROL_MAX_RATE * stickDeflection * 2;
|
|
}
|
|
|
|
#if defined(USE_ACC)
|
|
// If ACC is enabled and a limit angle is set, then try to limit forward tilt
|
|
// to that angle and slow down the rate as the limit is approached to reduce overshoot
|
|
if ((axis == FD_PITCH) && (launchControlAngleLimit > 0) && (ret > 0)) {
|
|
const float currentAngle = (attitude.raw[axis] - angleTrim->raw[axis]) / 10.0f;
|
|
if (currentAngle >= launchControlAngleLimit) {
|
|
ret = 0.0f;
|
|
} else {
|
|
//for the last 10 degrees scale the rate from the current input to 5 dps
|
|
const float angleDelta = launchControlAngleLimit - currentAngle;
|
|
if (angleDelta <= LAUNCH_CONTROL_ANGLE_WINDOW) {
|
|
ret = scaleRangef(angleDelta, 0, LAUNCH_CONTROL_ANGLE_WINDOW, LAUNCH_CONTROL_MIN_RATE, ret);
|
|
}
|
|
}
|
|
}
|
|
#else
|
|
UNUSED(angleTrim);
|
|
#endif
|
|
|
|
return ret;
|
|
}
|
|
#endif
|
|
|
|
// Betaflight pid controller, which will be maintained in the future with additional features specialised for current (mini) multirotor usage.
|
|
// Based on 2DOF reference design (matlab)
|
|
void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTimeUs)
|
|
{
|
|
static float previousGyroRateDterm[XYZ_AXIS_COUNT];
|
|
|
|
#if defined(USE_ACC)
|
|
static timeUs_t levelModeStartTimeUs = 0;
|
|
static bool gpsRescuePreviousState = false;
|
|
#endif
|
|
|
|
const float tpaFactor = getThrottlePIDAttenuation();
|
|
|
|
#if defined(USE_ACC)
|
|
const rollAndPitchTrims_t *angleTrim = &accelerometerConfig()->accelerometerTrims;
|
|
#else
|
|
UNUSED(pidProfile);
|
|
UNUSED(currentTimeUs);
|
|
#endif
|
|
|
|
#ifdef USE_TPA_MODE
|
|
const float tpaFactorKp = (currentControlRateProfile->tpaMode == TPA_MODE_PD) ? tpaFactor : 1.0f;
|
|
#else
|
|
const float tpaFactorKp = tpaFactor;
|
|
#endif
|
|
|
|
#ifdef USE_YAW_SPIN_RECOVERY
|
|
const bool yawSpinActive = gyroYawSpinDetected();
|
|
#endif
|
|
|
|
const bool launchControlActive = isLaunchControlActive();
|
|
|
|
#if defined(USE_ACC)
|
|
const bool gpsRescueIsActive = FLIGHT_MODE(GPS_RESCUE_MODE);
|
|
const bool levelModeActive = FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE) || gpsRescueIsActive;
|
|
|
|
// Keep track of when we entered a self-level mode so that we can
|
|
// add a guard time before crash recovery can activate.
|
|
// Also reset the guard time whenever GPS Rescue is activated.
|
|
if (levelModeActive) {
|
|
if ((levelModeStartTimeUs == 0) || (gpsRescueIsActive && !gpsRescuePreviousState)) {
|
|
levelModeStartTimeUs = currentTimeUs;
|
|
}
|
|
} else {
|
|
levelModeStartTimeUs = 0;
|
|
}
|
|
gpsRescuePreviousState = gpsRescueIsActive;
|
|
#endif
|
|
|
|
// Dynamic i component,
|
|
if ((antiGravityMode == ANTI_GRAVITY_SMOOTH) && antiGravityEnabled) {
|
|
itermAccelerator = 1 + fabsf(antiGravityThrottleHpf) * 0.01f * (itermAcceleratorGain - 1000);
|
|
DEBUG_SET(DEBUG_ANTI_GRAVITY, 1, lrintf(antiGravityThrottleHpf * 1000));
|
|
}
|
|
DEBUG_SET(DEBUG_ANTI_GRAVITY, 0, lrintf(itermAccelerator * 1000));
|
|
|
|
// gradually scale back integration when above windup point
|
|
float dynCi = dT * itermAccelerator;
|
|
if (itermWindupPointInv > 1.0f) {
|
|
dynCi *= constrainf((1.0f - getMotorMixRange()) * itermWindupPointInv, 0.0f, 1.0f);
|
|
}
|
|
|
|
// Precalculate gyro deta for D-term here, this allows loop unrolling
|
|
float gyroRateDterm[XYZ_AXIS_COUNT];
|
|
for (int axis = FD_ROLL; axis <= FD_YAW; ++axis) {
|
|
gyroRateDterm[axis] = gyro.gyroADCf[axis];
|
|
#ifdef USE_RPM_FILTER
|
|
gyroRateDterm[axis] = rpmFilterDterm(axis,gyroRateDterm[axis]);
|
|
#endif
|
|
gyroRateDterm[axis] = dtermNotchApplyFn((filter_t *) &dtermNotch[axis], gyroRateDterm[axis]);
|
|
gyroRateDterm[axis] = dtermLowpassApplyFn((filter_t *) &dtermLowpass[axis], gyroRateDterm[axis]);
|
|
gyroRateDterm[axis] = dtermLowpass2ApplyFn((filter_t *) &dtermLowpass2[axis], gyroRateDterm[axis]);
|
|
}
|
|
|
|
rotateItermAndAxisError();
|
|
#ifdef USE_RPM_FILTER
|
|
rpmFilterUpdate();
|
|
#endif
|
|
|
|
|
|
// ----------PID controller----------
|
|
for (int axis = FD_ROLL; axis <= FD_YAW; ++axis) {
|
|
|
|
float currentPidSetpoint = getSetpointRate(axis);
|
|
if (maxVelocity[axis]) {
|
|
currentPidSetpoint = accelerationLimit(axis, currentPidSetpoint);
|
|
}
|
|
// Yaw control is GYRO based, direct sticks control is applied to rate PID
|
|
#if defined(USE_ACC)
|
|
if (levelModeActive && (axis != FD_YAW)) {
|
|
currentPidSetpoint = pidLevel(axis, pidProfile, angleTrim, currentPidSetpoint);
|
|
}
|
|
#endif
|
|
|
|
#ifdef USE_ACRO_TRAINER
|
|
if ((axis != FD_YAW) && acroTrainerActive && !inCrashRecoveryMode && !launchControlActive) {
|
|
currentPidSetpoint = applyAcroTrainer(axis, angleTrim, currentPidSetpoint);
|
|
}
|
|
#endif // USE_ACRO_TRAINER
|
|
|
|
#ifdef USE_LAUNCH_CONTROL
|
|
if (launchControlActive) {
|
|
#if defined(USE_ACC)
|
|
currentPidSetpoint = applyLaunchControl(axis, angleTrim);
|
|
#else
|
|
currentPidSetpoint = applyLaunchControl(axis, NULL);
|
|
#endif
|
|
}
|
|
#endif
|
|
|
|
// Handle yaw spin recovery - zero the setpoint on yaw to aid in recovery
|
|
// It's not necessary to zero the set points for R/P because the PIDs will be zeroed below
|
|
#ifdef USE_YAW_SPIN_RECOVERY
|
|
if ((axis == FD_YAW) && yawSpinActive) {
|
|
currentPidSetpoint = 0.0f;
|
|
}
|
|
#endif // USE_YAW_SPIN_RECOVERY
|
|
|
|
// -----calculate error rate
|
|
const float gyroRate = gyro.gyroADCf[axis]; // Process variable from gyro output in deg/sec
|
|
float errorRate = currentPidSetpoint - gyroRate; // r - y
|
|
#if defined(USE_ACC)
|
|
handleCrashRecovery(
|
|
pidProfile->crash_recovery, angleTrim, axis, currentTimeUs, gyroRate,
|
|
¤tPidSetpoint, &errorRate);
|
|
#endif
|
|
|
|
const float previousIterm = pidData[axis].I;
|
|
float itermErrorRate = errorRate;
|
|
|
|
#if defined(USE_ITERM_RELAX)
|
|
if (!launchControlActive && !inCrashRecoveryMode) {
|
|
applyItermRelax(axis, previousIterm, gyroRate, &itermErrorRate, ¤tPidSetpoint);
|
|
errorRate = currentPidSetpoint - gyroRate;
|
|
}
|
|
#endif
|
|
|
|
// --------low-level gyro-based PID based on 2DOF PID controller. ----------
|
|
// 2-DOF PID controller with optional filter on derivative term.
|
|
// b = 1 and only c (feedforward weight) can be tuned (amount derivative on measurement or error).
|
|
|
|
// -----calculate P component
|
|
pidData[axis].P = pidCoefficient[axis].Kp * errorRate * tpaFactorKp;
|
|
if (axis == FD_YAW) {
|
|
pidData[axis].P = ptermYawLowpassApplyFn((filter_t *) &ptermYawLowpass, pidData[axis].P);
|
|
}
|
|
|
|
// -----calculate I component
|
|
#ifdef USE_LAUNCH_CONTROL
|
|
// if launch control is active override the iterm gains
|
|
const float Ki = launchControlActive ? launchControlKi : pidCoefficient[axis].Ki;
|
|
#else
|
|
const float Ki = pidCoefficient[axis].Ki;
|
|
#endif
|
|
pidData[axis].I = constrainf(previousIterm + Ki * itermErrorRate * dynCi, -itermLimit, itermLimit);
|
|
|
|
// -----calculate pidSetpointDelta
|
|
float pidSetpointDelta = 0;
|
|
pidSetpointDelta = currentPidSetpoint - previousPidSetpoint[axis];
|
|
previousPidSetpoint[axis] = currentPidSetpoint;
|
|
|
|
#ifdef USE_RC_SMOOTHING_FILTER
|
|
pidSetpointDelta = applyRcSmoothingDerivativeFilter(axis, pidSetpointDelta);
|
|
#endif // USE_RC_SMOOTHING_FILTER
|
|
|
|
// -----calculate D component
|
|
// disable D if launch control is active
|
|
if ((pidCoefficient[axis].Kd > 0) && !launchControlActive){
|
|
|
|
// Divide rate change by dT to get differential (ie dr/dt).
|
|
// dT is fixed and calculated from the target PID loop time
|
|
// This is done to avoid DTerm spikes that occur with dynamically
|
|
// calculated deltaT whenever another task causes the PID
|
|
// loop execution to be delayed.
|
|
const float delta =
|
|
- (gyroRateDterm[axis] - previousGyroRateDterm[axis]) * pidFrequency;
|
|
|
|
#if defined(USE_ACC)
|
|
if (cmpTimeUs(currentTimeUs, levelModeStartTimeUs) > CRASH_RECOVERY_DETECTION_DELAY_US) {
|
|
detectAndSetCrashRecovery(pidProfile->crash_recovery, axis, currentTimeUs, delta, errorRate);
|
|
}
|
|
#endif
|
|
|
|
float dMinFactor = 1.0f;
|
|
#if defined(USE_D_MIN)
|
|
if (dMinPercent[axis] > 0) {
|
|
float dMinGyroFactor = biquadFilterApply(&dMinRange[axis], delta);
|
|
dMinGyroFactor = fabsf(dMinGyroFactor) * dMinGyroGain;
|
|
const float dMinSetpointFactor = (fabsf(pidSetpointDelta)) * dMinSetpointGain;
|
|
dMinFactor = MAX(dMinGyroFactor, dMinSetpointFactor);
|
|
dMinFactor = dMinPercent[axis] + (1.0f - dMinPercent[axis]) * dMinFactor;
|
|
dMinFactor = pt1FilterApply(&dMinLowpass[axis], dMinFactor);
|
|
dMinFactor = MIN(dMinFactor, 1.0f);
|
|
if (axis == FD_ROLL) {
|
|
DEBUG_SET(DEBUG_D_MIN, 0, lrintf(dMinGyroFactor * 100));
|
|
DEBUG_SET(DEBUG_D_MIN, 1, lrintf(dMinSetpointFactor * 100));
|
|
DEBUG_SET(DEBUG_D_MIN, 2, lrintf(pidCoefficient[axis].Kd * dMinFactor * 10 / DTERM_SCALE));
|
|
} else if (axis == FD_PITCH) {
|
|
DEBUG_SET(DEBUG_D_MIN, 3, lrintf(pidCoefficient[axis].Kd * dMinFactor * 10 / DTERM_SCALE));
|
|
}
|
|
}
|
|
#endif
|
|
pidData[axis].D = pidCoefficient[axis].Kd * delta * tpaFactor * dMinFactor;
|
|
} else {
|
|
pidData[axis].D = 0;
|
|
}
|
|
previousGyroRateDterm[axis] = gyroRateDterm[axis];
|
|
|
|
// -----calculate feedforward component
|
|
// Only enable feedforward for rate mode and if launch control is inactive
|
|
const float feedforwardGain = (flightModeFlags || launchControlActive) ? 0.0f : pidCoefficient[axis].Kf;
|
|
if (feedforwardGain > 0) {
|
|
// no transition if feedForwardTransition == 0
|
|
float transition = feedForwardTransition > 0 ? MIN(1.f, getRcDeflectionAbs(axis) * feedForwardTransition) : 1;
|
|
pidData[axis].F = feedforwardGain * transition * pidSetpointDelta * pidFrequency;
|
|
|
|
#if defined(USE_SMART_FEEDFORWARD)
|
|
applySmartFeedforward(axis);
|
|
#endif
|
|
} else {
|
|
pidData[axis].F = 0;
|
|
}
|
|
|
|
#ifdef USE_YAW_SPIN_RECOVERY
|
|
if (yawSpinActive) {
|
|
pidData[axis].I = 0; // in yaw spin always disable I
|
|
if (axis <= FD_PITCH) {
|
|
// zero PIDs on pitch and roll leaving yaw P to correct spin
|
|
pidData[axis].P = 0;
|
|
pidData[axis].D = 0;
|
|
pidData[axis].F = 0;
|
|
}
|
|
}
|
|
#endif // USE_YAW_SPIN_RECOVERY
|
|
|
|
#ifdef USE_LAUNCH_CONTROL
|
|
// Disable P/I appropriately based on the launch control mode
|
|
if (launchControlActive) {
|
|
// if not using FULL mode then disable I accumulation on yaw as
|
|
// yaw has a tendency to windup. Otherwise limit yaw iterm accumulation.
|
|
const int launchControlYawItermLimit = (launchControlMode == LAUNCH_CONTROL_MODE_FULL) ? LAUNCH_CONTROL_YAW_ITERM_LIMIT : 0;
|
|
pidData[FD_YAW].I = constrainf(pidData[FD_YAW].I, -launchControlYawItermLimit, launchControlYawItermLimit);
|
|
|
|
// for pitch-only mode we disable everything except pitch P/I
|
|
if (launchControlMode == LAUNCH_CONTROL_MODE_PITCHONLY) {
|
|
pidData[FD_ROLL].P = 0;
|
|
pidData[FD_ROLL].I = 0;
|
|
pidData[FD_YAW].P = 0;
|
|
// don't let I go negative (pitch backwards) as front motors are limited in the mixer
|
|
pidData[FD_PITCH].I = MAX(0.0f, pidData[FD_PITCH].I);
|
|
}
|
|
}
|
|
#endif
|
|
// calculating the PID sum
|
|
const float pidSum = pidData[axis].P + pidData[axis].I + pidData[axis].D + pidData[axis].F;
|
|
#ifdef USE_INTEGRATED_YAW_CONTROL
|
|
if (axis == FD_YAW && useIntegratedYaw) {
|
|
pidData[axis].Sum += pidSum * dT * 100.0f;
|
|
pidData[axis].Sum -= pidData[axis].Sum * integratedYawRelax / 100000.0f * dT / 0.000125f;
|
|
} else
|
|
#endif
|
|
{
|
|
pidData[axis].Sum = pidSum;
|
|
}
|
|
}
|
|
|
|
// Disable PID control if at zero throttle or if gyro overflow detected
|
|
// This may look very innefficient, but it is done on purpose to always show real CPU usage as in flight
|
|
if (!pidStabilisationEnabled || gyroOverflowDetected()) {
|
|
for (int axis = FD_ROLL; axis <= FD_YAW; ++axis) {
|
|
pidData[axis].P = 0;
|
|
pidData[axis].I = 0;
|
|
pidData[axis].D = 0;
|
|
pidData[axis].F = 0;
|
|
|
|
pidData[axis].Sum = 0;
|
|
}
|
|
} else if (zeroThrottleItermReset) {
|
|
pidResetIterm();
|
|
}
|
|
}
|
|
|
|
bool crashRecoveryModeActive(void)
|
|
{
|
|
return inCrashRecoveryMode;
|
|
}
|
|
|
|
#ifdef USE_ACRO_TRAINER
|
|
void pidSetAcroTrainerState(bool newState)
|
|
{
|
|
if (acroTrainerActive != newState) {
|
|
if (newState) {
|
|
pidAcroTrainerInit();
|
|
}
|
|
acroTrainerActive = newState;
|
|
}
|
|
}
|
|
#endif // USE_ACRO_TRAINER
|
|
|
|
void pidSetAntiGravityState(bool newState)
|
|
{
|
|
if (newState != antiGravityEnabled) {
|
|
// reset the accelerator on state changes
|
|
itermAccelerator = 1.0f;
|
|
}
|
|
antiGravityEnabled = newState;
|
|
}
|
|
|
|
bool pidAntiGravityEnabled(void)
|
|
{
|
|
return antiGravityEnabled;
|
|
}
|
|
|
|
#ifdef USE_DYN_LPF
|
|
void dynLpfDTermUpdate(float throttle)
|
|
{
|
|
if (dynLpfFilter != DYN_LPF_NONE) {
|
|
const unsigned int cutoffFreq = fmax(dynThrottle(throttle) * dynLpfMax, dynLpfMin);
|
|
|
|
if (dynLpfFilter == DYN_LPF_PT1) {
|
|
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
|
|
pt1FilterUpdateCutoff(&dtermLowpass[axis].pt1Filter, pt1FilterGain(cutoffFreq, dT));
|
|
}
|
|
} else if (dynLpfFilter == DYN_LPF_BIQUAD) {
|
|
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
|
|
biquadFilterUpdateLPF(&dtermLowpass[axis].biquadFilter, cutoffFreq, targetPidLooptime);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
#endif
|
|
|
|
void pidSetItermReset(bool enabled)
|
|
{
|
|
zeroThrottleItermReset = enabled;
|
|
}
|
|
|
|
float pidGetPreviousSetpoint(int axis)
|
|
{
|
|
return previousPidSetpoint[axis];
|
|
}
|