From c1a99817f42172794c576efd402641a628c99272 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Sat, 9 Sep 2017 07:03:15 +0100 Subject: [PATCH] Minor tidy of PID code --- src/main/cms/cms_menu_imu.c | 2 +- src/main/common/filter.c | 9 ++++++--- src/main/common/filter.h | 2 +- src/main/fc/fc_msp.c | 2 +- src/main/flight/pid.c | 25 +++++++++++++------------ src/main/flight/pid.h | 3 +-- 6 files changed, 23 insertions(+), 20 deletions(-) diff --git a/src/main/cms/cms_menu_imu.c b/src/main/cms/cms_menu_imu.c index 8107c1046..f364cbca1 100644 --- a/src/main/cms/cms_menu_imu.c +++ b/src/main/cms/cms_menu_imu.c @@ -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; diff --git a/src/main/common/filter.c b/src/main/common/filter.c index 35bc54a79..c9585e857 100644 --- a/src/main/common/filter.c +++ b/src/main/common/filter.c @@ -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; + } } diff --git a/src/main/common/filter.h b/src/main/common/filter.h index 730298cdb..5b282a91e 100644 --- a/src/main/common/filter.h +++ b/src/main/common/filter.h @@ -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; diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 97ebabec0..aae0ec773 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -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); diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index fdd7615f6..c8687ba16 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -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)); - } -} diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 98a5459ac..c48a5eecf 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -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