Merge pull request #4103 from martinbudden/bf_pid_tidy3

Minor tidy of PID code

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;

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;
+    }
 }

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;

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);

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));
-    }
-}

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