2 * This file is part of Cleanflight.
4 * Cleanflight is free software: you can redistribute it and/or modify
5 * it under the terms of the GNU General Public License as published by
6 * the Free Software Foundation, either version 3 of the License, or
7 * (at your option) any later version.
9 * Cleanflight is distributed in the hope that it will be useful,
10 * but WITHOUT ANY WARRANTY; without even the implied warranty of
11 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12 * GNU General Public License for more details.
14 * You should have received a copy of the GNU General Public License
15 * along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
21 #define GYRO_I_MAX 256 // Gyro I limiter
22 #define YAW_P_LIMIT_MIN 100 // Maximum value for yaw P limiter
23 #define YAW_P_LIMIT_MAX 500 // Maximum value for yaw P limiter
25 #define ITERM_RESET_THRESHOLD 15
26 #define DYNAMIC_PTERM_STICK_THRESHOLD 400
43 PID_CONTROLLER_MWREWRITE
= 1,
44 PID_CONTROLLER_LUX_FLOAT
,
46 } pidControllerType_e
;
50 DELTA_FROM_MEASUREMENT
56 RESET_ITERM_AND_REDUCE_PID
57 } pidErrorResetOption_e
;
60 SUPEREXPO_YAW_OFF
= 0,
65 #define IS_PID_CONTROLLER_FP_BASED(pidController) (pidController == 2)
67 typedef struct pidProfile_s
{
68 uint8_t pidController
; // 1 = rewrite from http://www.multiwii.com/forum/viewtopic.php?f=8&t=3671, 2 = Luggi09s new baseflight pid
70 uint8_t P8
[PID_ITEM_COUNT
];
71 uint8_t I8
[PID_ITEM_COUNT
];
72 uint8_t D8
[PID_ITEM_COUNT
];
74 uint8_t H_sensitivity
;
76 uint16_t dterm_lpf_hz
; // Delta Filter in hz
77 uint16_t yaw_lpf_hz
; // Additional yaw filter when yaw axis too noisy
78 uint16_t rollPitchItermResetRate
; // Experimental threshold for resetting iterm for pitch and roll on certain rates
79 uint8_t rollPitchItermResetAlways
; // Reset Iterm also without SUPER EXPO
80 uint16_t yawItermResetRate
; // Experimental threshold for resetting iterm for yaw on certain rates
82 uint8_t dterm_average_count
; // Configurable delta count for dterm
83 uint8_t dynamic_pterm
;
86 uint8_t gtune_lolimP
[3]; // [0..200] Lower limit of P during G tune
87 uint8_t gtune_hilimP
[3]; // [0..200] Higher limit of P during G tune. 0 Disables tuning for that axis.
88 uint8_t gtune_pwr
; // [0..10] Strength of adjustment
89 uint16_t gtune_settle_time
; // [200..1000] Settle time in ms
90 uint8_t gtune_average_cycles
; // [8..128] Number of looptime cycles used for gyro average calculation
94 extern int16_t axisPID
[XYZ_AXIS_COUNT
];
95 extern int32_t axisPID_P
[3], axisPID_I
[3], axisPID_D
[3];
96 bool antiWindupProtection
;
97 extern uint32_t targetPidLooptime
;
99 void pidSetController(pidControllerType_e type
);
100 void pidResetErrorAngle(void);
101 void pidResetErrorGyroState(uint8_t resetOption
);
102 void setTargetPidLooptime(uint8_t pidProcessDenom
);