Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-17 21:35:37 +03:00
Code Issues Wiki Activity

Log Dterm boost factor

Browse source
This commit is contained in:
Pawel Spychalski (DzikuVx) 2021-06-20 12:04:59 +02:00
parent cbf0c361ee
commit 724c1aa65f
1 changed files with 4 additions and 5 deletions
Download patch file Download diff file Expand all files Collapse all files

9
src/main/flight/pid.c
View file

@ -68,6 +68,7 @@ FILE_COMPILE_FOR_SPEED
#include "programming/logic_condition.h" #include "programming/logic_condition.h"
typedef struct { typedef struct {
uint8_t axis;
float kP; // Proportional gain float kP; // Proportional gain
float kI; // Integral gain float kI; // Integral gain
float kD; // Derivative gain float kD; // Derivative gain
@ -78,11 +79,6 @@ typedef struct {
float gyroRate; float gyroRate;
float rateTarget; float rateTarget;
// Buffer for derivative calculation
#define PID_GYRO_RATE_BUF_LENGTH 5
float gyroRateBuf[PID_GYRO_RATE_BUF_LENGTH];
firFilter_t gyroRateFilter;
// Rate integrator // Rate integrator
float errorGyroIf; float errorGyroIf;
float errorGyroIfLimit; float errorGyroIfLimit;
@ -714,6 +710,8 @@ static float FAST_CODE applyDBoost(pidState_t *pidState, float dT) {
dBoost = pt1FilterApply4(&pidState->dBoostLpf, dBoost, D_BOOST_LPF_HZ, dT); dBoost = pt1FilterApply4(&pidState->dBoostLpf, dBoost, D_BOOST_LPF_HZ, dT);
dBoost = constrainf(dBoost, 0.0f, dBoostFactor); dBoost = constrainf(dBoost, 0.0f, dBoostFactor);
DEBUG_SET(DEBUG_ALWAYS, pidState->axis, dBoost * 100);
return dBoost; return dBoost;
} }
#else #else
@ -1199,6 +1197,7 @@ void pidInit(void)
#endif #endif
for (uint8_t axis = FD_ROLL; axis <= FD_YAW; axis++) { for (uint8_t axis = FD_ROLL; axis <= FD_YAW; axis++) {
pidState[axis].axis = axis;
if (axis == FD_YAW) { if (axis == FD_YAW) {
pidState[axis].pidSumLimit = pidProfile()->pidSumLimitYaw; pidState[axis].pidSumLimit = pidProfile()->pidSumLimitYaw;
if (yawLpfHz) { if (yawLpfHz) {