Minor tidy of PID code
This commit is contained in:
parent
247a88be50
commit
c1a99817f4
|
@ -420,7 +420,7 @@ static long cmsx_CopyPidProfile(displayPort_t *pDisplay, const void *ptr)
|
|||
UNUSED(ptr);
|
||||
|
||||
if (cmsx_dstPidProfile > 0) {
|
||||
copyPidProfile(cmsx_dstPidProfile - 1, getCurrentPidProfileIndex());
|
||||
pidCopyProfile(cmsx_dstPidProfile - 1, getCurrentPidProfileIndex());
|
||||
}
|
||||
|
||||
return 0;
|
||||
|
|
|
@ -295,6 +295,7 @@ float firFilterLastInput(const firFilter_t *filter)
|
|||
|
||||
void firFilterDenoiseInit(firFilterDenoise_t *filter, uint8_t gyroSoftLpfHz, uint16_t targetLooptime)
|
||||
{
|
||||
memset(filter, 0, sizeof(firFilterDenoise_t));
|
||||
filter->targetCount = constrain(lrintf((1.0f / (0.000001f * (float)targetLooptime)) / gyroSoftLpfHz), 1, MAX_FIR_DENOISE_WINDOW_SIZE);
|
||||
}
|
||||
|
||||
|
@ -303,12 +304,14 @@ float firFilterDenoiseUpdate(firFilterDenoise_t *filter, float input)
|
|||
{
|
||||
filter->state[filter->index] = input;
|
||||
filter->movingSum += filter->state[filter->index++];
|
||||
if (filter->index == filter->targetCount)
|
||||
if (filter->index == filter->targetCount) {
|
||||
filter->index = 0;
|
||||
}
|
||||
filter->movingSum -= filter->state[filter->index];
|
||||
|
||||
if (filter->targetCount >= filter->filledCount)
|
||||
if (filter->targetCount >= filter->filledCount) {
|
||||
return filter->movingSum / filter->targetCount;
|
||||
else
|
||||
} else {
|
||||
return filter->movingSum / ++filter->filledCount + 1;
|
||||
}
|
||||
}
|
||||
|
|
|
@ -44,7 +44,7 @@ typedef struct biquadFilter_s {
|
|||
float x1, x2, y1, y2;
|
||||
} biquadFilter_t;
|
||||
|
||||
typedef struct firFilterDenoise_s{
|
||||
typedef struct firFilterDenoise_s {
|
||||
int filledCount;
|
||||
int targetCount;
|
||||
int index;
|
||||
|
|
|
@ -1326,7 +1326,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
|
|||
uint8_t dstProfileIndex = sbufReadU8(src);
|
||||
uint8_t srcProfileIndex = sbufReadU8(src);
|
||||
if (value == 0) {
|
||||
copyPidProfile(dstProfileIndex, srcProfileIndex);
|
||||
pidCopyProfile(dstProfileIndex, srcProfileIndex);
|
||||
}
|
||||
else if (value == 1) {
|
||||
copyControlRateProfile(dstProfileIndex, srcProfileIndex);
|
||||
|
|
|
@ -126,7 +126,7 @@ void pgResetFn_pidProfiles(pidProfile_t *pidProfiles)
|
|||
}
|
||||
}
|
||||
|
||||
void pidSetTargetLooptime(uint32_t pidLooptime)
|
||||
static void pidSetTargetLooptime(uint32_t pidLooptime)
|
||||
{
|
||||
targetPidLooptime = pidLooptime;
|
||||
dT = (float)targetPidLooptime * 0.000001f;
|
||||
|
@ -199,8 +199,6 @@ void pidInitFilters(const pidProfile_t *pidProfile)
|
|||
if (pidProfile->dterm_lpf_hz == 0 || pidProfile->dterm_lpf_hz > pidFrequencyNyquist) {
|
||||
dtermLpfApplyFn = nullFilterApply;
|
||||
} else {
|
||||
memset(&dtermFilterLpfUnion, 0, sizeof(dtermFilterLpfUnion));
|
||||
|
||||
switch (pidProfile->dterm_filter_type) {
|
||||
default:
|
||||
dtermLpfApplyFn = nullFilterApply;
|
||||
|
@ -294,6 +292,16 @@ void pidInit(const pidProfile_t *pidProfile)
|
|||
pidInitMixer(pidProfile);
|
||||
}
|
||||
|
||||
|
||||
void pidCopyProfile(uint8_t dstPidProfileIndex, uint8_t srcPidProfileIndex)
|
||||
{
|
||||
if ((dstPidProfileIndex < MAX_PROFILE_COUNT-1 && srcPidProfileIndex < MAX_PROFILE_COUNT-1)
|
||||
&& dstPidProfileIndex != srcPidProfileIndex
|
||||
) {
|
||||
memcpy(pidProfilesMutable(dstPidProfileIndex), pidProfilesMutable(srcPidProfileIndex), sizeof(pidProfile_t));
|
||||
}
|
||||
}
|
||||
|
||||
// calculates strength of horizon leveling; 0 = none, 1.0 = most leveling
|
||||
static float calcHorizonLevelStrength(void)
|
||||
{
|
||||
|
@ -371,7 +379,8 @@ static float pidLevel(int axis, const pidProfile_t *pidProfile, const rollAndPit
|
|||
return currentPidSetpoint;
|
||||
}
|
||||
|
||||
static float accelerationLimit(int axis, float currentPidSetpoint) {
|
||||
static float accelerationLimit(int axis, float currentPidSetpoint)
|
||||
{
|
||||
static float previousSetpoint[3];
|
||||
const float currentVelocity = currentPidSetpoint- previousSetpoint[axis];
|
||||
|
||||
|
@ -523,11 +532,3 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an
|
|||
}
|
||||
}
|
||||
}
|
||||
|
||||
void copyPidProfile(const uint8_t dstPidProfileIndex, const uint8_t srcPidProfileIndex) {
|
||||
if ((dstPidProfileIndex < MAX_PROFILE_COUNT-1 && srcPidProfileIndex < MAX_PROFILE_COUNT-1)
|
||||
&& dstPidProfileIndex != srcPidProfileIndex
|
||||
) {
|
||||
memcpy(pidProfilesMutable(dstPidProfileIndex), pidProfilesMutable(srcPidProfileIndex), sizeof(pidProfile_t));
|
||||
}
|
||||
}
|
||||
|
|
|
@ -128,11 +128,10 @@ extern uint8_t PIDweight[3];
|
|||
|
||||
void pidResetErrorGyroState(void);
|
||||
void pidStabilisationState(pidStabilisationState_e pidControllerState);
|
||||
void pidSetTargetLooptime(uint32_t pidLooptime);
|
||||
void pidSetItermAccelerator(float newItermAccelerator);
|
||||
void pidInitFilters(const pidProfile_t *pidProfile);
|
||||
void pidInitConfig(const pidProfile_t *pidProfile);
|
||||
void pidInit(const pidProfile_t *pidProfile);
|
||||
void copyPidProfile(const uint8_t dstPidProfileIndex, const uint8_t srcPidProfileIndex);
|
||||
void pidCopyProfile(uint8_t dstPidProfileIndex, uint8_t srcPidProfileIndex);
|
||||
|
||||
#endif
|
||||
|
|
Loading…
Reference in New Issue