Filter Cleanup CF coding style// Remove Old pt1 for acc etc // F1 slower acc update

src/main/common/filter.c

@@ -30,37 +30,22 @@
 #define M_LN2_FLOAT	0.69314718055994530942f
 #define M_PI_FLOAT	3.14159265358979323846f
 
-
-#define BIQUAD_GAIN 6.0f          /* gain in db */
 #define BIQUAD_BANDWIDTH 1.9f     /* bandwidth in octaves */
 
-// PT1 Low Pass filter (when no dT specified it will be calculated from the cycleTime)
-float filterApplyPt1(float input, filterStatePt1_t *filter, uint8_t f_cut, float dT) {
-
-	// Pre calculate and store RC
-	if (!filter->RC) {
-		filter->RC = 1.0f / ( 2.0f * (float)M_PI * f_cut );
-	}
-
-    filter->state = filter->state + dT / (filter->RC + dT) * (input - filter->state);
-
-    return filter->state;
-}
-
 /* sets up a biquad Filter */
-biquad_t *BiQuadNewLpf(uint8_t filterCutFreq)
+void BiQuadNewLpf(uint8_t filterCutFreq, biquad_t *newState, float refreshRate)
 {
 	float samplingRate;
-    samplingRate = 1 / (targetLooptime * 0.000001f);
 
-    biquad_t *newState;
+    if (!refreshRate) {
+        samplingRate = 1 / (targetLooptime * 0.000001f);
+    } else {
+        samplingRate = refreshRate;
+    }
+
     float omega, sn, cs, alpha;
     float a0, a1, a2, b0, b1, b2;
 
-    newState = malloc(sizeof(biquad_t));
-    if (newState == NULL)
-        return NULL;
-
     /* setup variables */
     omega = 2 * M_PI_FLOAT * (float) filterCutFreq / samplingRate;
     sn = sinf(omega);
@@ -84,8 +69,6 @@ biquad_t *BiQuadNewLpf(uint8_t filterCutFreq)
     /* zero initial samples */
     newState->x1 = newState->x2 = 0;
     newState->y1 = newState->y2 = 0;
-
-    return newState;
 }
 
 /* Computes a biquad_t filter on a sample */

src/main/common/filter.h

@@ -15,20 +15,11 @@
  * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
  */
 
-#define FILTER_TAPS 14
-
-typedef struct filterStatePt1_s {
-	float state;
-	float RC;
-	float constdT;
-} filterStatePt1_t;
-
 /* this holds the data required to update samples thru a filter */
 typedef struct biquad_s {
     float a0, a1, a2, a3, a4;
     float x1, x2, y1, y2;
 } biquad_t;
 
-float filterApplyPt1(float input, filterStatePt1_t *filter, uint8_t f_cut, float dt);
 float applyBiQuadFilter(float sample, biquad_t *state);
-biquad_t *BiQuadNewLpf(uint8_t filterCutFreq);
+void BiQuadNewLpf(uint8_t filterCutFreq, biquad_t *newState, float refreshRate);

src/main/config/config.c

@@ -493,7 +493,7 @@ static void resetConf(void)
     resetRollAndPitchTrims(&currentProfile->accelerometerTrims);
 
     currentProfile->mag_declination = 0;
-    currentProfile->acc_cut_hz = 15;
+    currentProfile->acc_lpf_hz = 20;
     currentProfile->accz_lpf_cutoff = 5.0f;
     currentProfile->accDeadband.xy = 40;
     currentProfile->accDeadband.z = 40;
@@ -748,7 +748,7 @@ void activateConfig(void)
         &currentProfile->pidProfile
     );
 
-    useGyroConfig(&masterConfig.gyroConfig, &currentProfile->pidProfile.gyro_lpf_hz);
+    useGyroConfig(&masterConfig.gyroConfig, currentProfile->pidProfile.gyro_lpf_hz);
 
 #ifdef TELEMETRY
     telemetryUseConfig(&masterConfig.telemetryConfig);
@@ -778,7 +778,7 @@ void activateConfig(void)
 
     imuRuntimeConfig.dcm_kp = masterConfig.dcm_kp / 10000.0f;
     imuRuntimeConfig.dcm_ki = masterConfig.dcm_ki / 10000.0f;
-    imuRuntimeConfig.acc_cut_hz = currentProfile->acc_cut_hz;
+    imuRuntimeConfig.acc_cut_hz = currentProfile->acc_lpf_hz;
     imuRuntimeConfig.acc_unarmedcal = currentProfile->acc_unarmedcal;
     imuRuntimeConfig.small_angle = masterConfig.small_angle;
 

src/main/config/config_profile.h

@@ -28,7 +28,7 @@ typedef struct profile_s {
     rollAndPitchTrims_t accelerometerTrims; // accelerometer trim
 
     // sensor-related stuff
-    uint8_t acc_cut_hz;                     // Set the Low Pass Filter factor for ACC. Reducing this value would reduce ACC noise (visible in GUI), but would increase ACC lag time. Zero = no filter
+    uint8_t acc_lpf_hz;                     // Set the Low Pass Filter factor for ACC. Reducing this value would reduce ACC noise (visible in GUI), but would increase ACC lag time. Zero = no filter
     float accz_lpf_cutoff;                  // cutoff frequency for the low pass filter used on the acc z-axis for althold in Hz
     accDeadband_t accDeadband;
 

src/main/flight/imu.c

@@ -377,7 +377,8 @@ static bool isMagnetometerHealthy(void)
 
 static void imuCalculateEstimatedAttitude(void)
 {
-    static filterStatePt1_t accLPFState[3];
+    static biquad_t accLPFState[3];
+    static bool accStateIsSet;
     static uint32_t previousIMUUpdateTime;
     float rawYawError = 0;
     int32_t axis;
@@ -391,8 +392,13 @@ static void imuCalculateEstimatedAttitude(void)
 
     // Smooth and use only valid accelerometer readings
     for (axis = 0; axis < 3; axis++) {
-        if (imuRuntimeConfig->acc_cut_hz > 0) {
+        if (imuRuntimeConfig->acc_cut_hz) {
-            accSmooth[axis] = filterApplyPt1(accADC[axis], &accLPFState[axis], imuRuntimeConfig->acc_cut_hz, deltaT * 1e-6f);
+            if (accStateIsSet) {
+                accSmooth[axis] = lrintf(applyBiQuadFilter((float) accADC[axis], &accLPFState[axis]));
+            } else {
+                for (axis = 0; axis < 3; axis++) BiQuadNewLpf(imuRuntimeConfig->acc_cut_hz, &accLPFState[axis], 1000);
+                accStateIsSet = true;
+            }
         } else {
             accSmooth[axis] = accADC[axis];
         }

src/main/flight/pid.c

@@ -113,7 +113,7 @@ void airModePlus(airModePlus_t *axisState, int axis, pidProfile_t *pidProfile) {
 const angle_index_t rcAliasToAngleIndexMap[] = { AI_ROLL, AI_PITCH };
 
 static airModePlus_t airModePlusAxisState[3];
-static biquad_t *deltaBiQuadState[3];
+static biquad_t deltaBiQuadState[3];
 static bool deltaStateIsSet;
 
 static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig,
@@ -128,7 +128,7 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
     static float previousErrorGyroIf[3] = { 0.0f, 0.0f, 0.0f };
 
     if (!deltaStateIsSet && pidProfile->dterm_lpf_hz) {
-        for (axis = 0; axis < 3; axis++) deltaBiQuadState[axis] = (biquad_t *)BiQuadNewLpf(pidProfile->dterm_lpf_hz);
+        for (axis = 0; axis < 3; axis++) BiQuadNewLpf(pidProfile->dterm_lpf_hz, &deltaBiQuadState[axis], 0);
         deltaStateIsSet = true;
     }
 
@@ -215,7 +215,7 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
         lastError[axis] = RateError;
 
         if (deltaStateIsSet) {
-            delta = applyBiQuadFilter(delta, deltaBiQuadState[axis]);
+            delta = applyBiQuadFilter(delta, &deltaBiQuadState[axis]);
         }
 
         // Correct difference by cycle time. Cycle time is jittery (can be different 2 times), so calculated difference
@@ -259,7 +259,7 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
     int8_t horizonLevelStrength = 100;
 
     if (!deltaStateIsSet && pidProfile->dterm_lpf_hz) {
-        for (axis = 0; axis < 3; axis++) deltaBiQuadState[axis] = (biquad_t *)BiQuadNewLpf(pidProfile->dterm_lpf_hz);
+        for (axis = 0; axis < 3; axis++) BiQuadNewLpf(pidProfile->dterm_lpf_hz, &deltaBiQuadState[axis], 0);
         deltaStateIsSet = true;
     }
 
@@ -348,7 +348,7 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
         lastError[axis] = RateError;
 
         if (deltaStateIsSet) {
-            delta = lrintf(applyBiQuadFilter((float) delta, deltaBiQuadState[axis]));
+            delta = lrintf(applyBiQuadFilter((float) delta, &deltaBiQuadState[axis]));
         }
 
         // Correct difference by cycle time. Cycle time is jittery (can be different 2 times), so calculated difference

src/main/io/serial_cli.c

@@ -602,7 +602,7 @@ const clivalue_t valueTable[] = {
 #endif
 
     { "acc_hardware",               VAR_UINT8  | MASTER_VALUE,  &masterConfig.acc_hardware, .config.minmax = { 0,  ACC_MAX } },
-    { "acc_cut_hz",                 VAR_UINT8  | PROFILE_VALUE, &masterConfig.profile[0].acc_cut_hz, .config.minmax = { 0,  200 } },
+    { "acc_lpf_hz",                 VAR_UINT8  | PROFILE_VALUE, &masterConfig.profile[0].acc_lpf_hz, .config.minmax = { 0,  200 } },
     { "accxy_deadband",             VAR_UINT8  | PROFILE_VALUE, &masterConfig.profile[0].accDeadband.xy, .config.minmax = { 0,  100 } },
     { "accz_deadband",              VAR_UINT8  | PROFILE_VALUE, &masterConfig.profile[0].accDeadband.z, .config.minmax = { 0,  100 } },
     { "accz_lpf_cutoff",            VAR_FLOAT  | PROFILE_VALUE, &masterConfig.profile[0].accz_lpf_cutoff, .config.minmax = { 1,  20 } },

src/main/main.c

@@ -549,15 +549,21 @@ int main(void) {
 
     setTaskEnabled(TASK_GYROPID, true);
     if(sensors(SENSOR_ACC)) {
+        uint32_t accTargetLooptime = 0;
         setTaskEnabled(TASK_ACCEL, true);
         switch(targetLooptime) {
             case(500):
-                rescheduleTask(TASK_ACCEL, 10000);
+                accTargetLooptime = 10000;
                 break;
             default:
             case(1000):
-                rescheduleTask(TASK_ACCEL, 1000);
+#ifdef STM32F10X
+                accTargetLooptime = 3000;
+#else
+                accTargetLooptime = 1000;
+#endif
         }
+        rescheduleTask(TASK_ACCEL, accTargetLooptime);
     }
     setTaskEnabled(TASK_SERIAL, true);
     setTaskEnabled(TASK_BEEPER, true);

src/main/mw.c

@@ -121,8 +121,6 @@ extern uint32_t currentTime;
 extern uint8_t PIDweight[3];
 
 static bool isRXDataNew;
-static filterStatePt1_t filteredCycleTimeState;
-uint16_t filteredCycleTime;
 
 typedef void (*pidControllerFuncPtr)(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig,
         uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig);            // pid controller function prototype
@@ -179,10 +177,21 @@ void filterRc(void){
     static int16_t deltaRC[4] = { 0, 0, 0, 0 };
     static int16_t factor, rcInterpolationFactor;
     uint16_t rxRefreshRate;
+    static biquad_t filteredCycleTimeState;
+    static bool filterIsSet;
+    uint16_t filteredCycleTime;
 
     // Set RC refresh rate for sampling and channels to filter
     initRxRefreshRate(&rxRefreshRate);
 
+    /* Initialize cycletime filter */
+    if (!filterIsSet) {
+        BiQuadNewLpf(1, &filteredCycleTimeState, 0);
+        filterIsSet = true;
+    }
+
+    filteredCycleTime = applyBiQuadFilter((float) cycleTime, &filteredCycleTimeState);
+
     rcInterpolationFactor = rxRefreshRate / filteredCycleTime + 1;
 
     if (isRXDataNew) {
@@ -657,13 +666,6 @@ void taskMainPidLoop(void)
     cycleTime = getTaskDeltaTime(TASK_SELF);
     dT = (float)targetLooptime * 0.000001f;
 
-    // Calculate average cycle time and average jitter
-    filteredCycleTime = filterApplyPt1(cycleTime, &filteredCycleTimeState, 1, dT);
-
-#if defined JITTER_DEBUG
-    debug[JITTER_DEBUG] = cycleTime - filteredCycleTime;
-#endif
-
     imuUpdateGyroAndAttitude();
 
     annexCode();

src/main/sensors/gyro.c

@@ -41,7 +41,7 @@ int16_t gyroADC[XYZ_AXIS_COUNT];
 int16_t gyroZero[FLIGHT_DYNAMICS_INDEX_COUNT] = { 0, 0, 0 };
 
 static gyroConfig_t *gyroConfig;
-static biquad_t *gyroBiQuadState[3];
+static biquad_t gyroBiQuadState[3];
 static bool gyroFilterStateIsSet;
 static uint8_t gyroLpfCutFreq;
 int axis;
@@ -49,15 +49,15 @@ int axis;
 gyro_t gyro;                      // gyro access functions
 sensor_align_e gyroAlign = 0;
 
-void useGyroConfig(gyroConfig_t *gyroConfigToUse, uint8_t *gyro_lpf_hz)
+void useGyroConfig(gyroConfig_t *gyroConfigToUse, uint8_t gyro_lpf_hz)
 {
     gyroConfig = gyroConfigToUse;
-    gyroLpfCutFreq = *gyro_lpf_hz;
+    gyroLpfCutFreq = gyro_lpf_hz;
 }
 
 void initGyroFilterCoefficients(void) {
     if (gyroLpfCutFreq && targetLooptime) {  /* Initialisation needs to happen once samplingrate is known */
-        for (axis = 0; axis < 3; axis++) gyroBiQuadState[axis] = (biquad_t *)BiQuadNewLpf(gyroLpfCutFreq);
+        for (axis = 0; axis < 3; axis++) BiQuadNewLpf(gyroLpfCutFreq, &gyroBiQuadState[axis], 0);
         gyroFilterStateIsSet = true;
     }
 }
@@ -143,7 +143,7 @@ void gyroUpdate(void)
         if (!gyroFilterStateIsSet) {
             initGyroFilterCoefficients();
         } else {
-            for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) gyroADC[axis] = lrintf(applyBiQuadFilter((float) gyroADC[axis], gyroBiQuadState[axis]));
+            for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) gyroADC[axis] = lrintf(applyBiQuadFilter((float) gyroADC[axis], &gyroBiQuadState[axis]));
         }
     }
 

src/main/sensors/gyro.h

@@ -39,7 +39,7 @@ typedef struct gyroConfig_s {
     uint8_t gyroMovementCalibrationThreshold; // people keep forgetting that moving model while init results in wrong gyro offsets. and then they never reset gyro. so this is now on by default.
 } gyroConfig_t;
 
-void useGyroConfig(gyroConfig_t *gyroConfigToUse, uint8_t *gyro_lpf_hz);
+void useGyroConfig(gyroConfig_t *gyroConfigToUse, uint8_t gyro_lpf_hz);
 void gyroSetCalibrationCycles(uint16_t calibrationCyclesRequired);
 void gyroUpdate(void);
 bool isGyroCalibrationComplete(void);