src/test/unit/lowpass_unittest.cc.txt

   1 /*
   2  * This file is part of Cleanflight.
   3  *
   4  * Cleanflight is free software: you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation, either version 3 of the License, or
   7  * (at your option) any later version.
   8  *
   9  * Cleanflight is distributed in the hope that it will be useful,
  10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12  * GNU General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License
  15  * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
  16  */
  17 #include <stdint.h>
  18 #include <limits.h>
  19
  20 //#define DEBUG_LOWPASS
  21
  22 extern "C" {
  23     #include "flight/lowpass.h"
  24 }
  25
  26 static lowpass_t lowpassFilterReference;
  27 static lowpass_t lowpassFilter;
  28
  29 #include "unittest_macros.h"
  30 #include "gtest/gtest.h"
  31
  32 static float lowpassRef(lowpass_t *filter, float in, int16_t freq)
  33 {
  34     int16_t coefIdx;
  35     float out;
  36
  37     // Check to see if cutoff frequency changed
  38     if (freq != filter->freq) {
  39         filter->init = false;
  40     }
  41
  42     // Initialize if needed
  43     if (!filter->init) {
  44         generateLowpassCoeffs2(freq, filter);
  45         for (coefIdx = 0; coefIdx < LOWPASS_NUM_COEF; coefIdx++) {
  46             filter->xf[coefIdx] = in;
  47             filter->yf[coefIdx] = in;
  48         }
  49         filter->init = true;
  50     }
  51
  52     // Delays
  53     for (coefIdx = LOWPASS_NUM_COEF-1; coefIdx > 0; coefIdx--) {
  54         filter->xf[coefIdx] = filter->xf[coefIdx-1];
  55         filter->yf[coefIdx] = filter->yf[coefIdx-1];
  56     }
  57     filter->xf[0] = in;
  58
  59     // Accumulate result
  60     out = filter->xf[0] * filter->bf[0];
  61     for (coefIdx = 1; coefIdx < LOWPASS_NUM_COEF; coefIdx++) {
  62         out += filter->xf[coefIdx] * filter->bf[coefIdx];
  63         out -= filter->yf[coefIdx] * filter->af[coefIdx];
  64     }
  65     filter->yf[0] = out;
  66
  67     return out;
  68 }
  69
  70 TEST(LowpassTest, Lowpass)
  71 {
  72     int16_t servoCmds[3000];
  73     int16_t expectedOut[3000];
  74     int16_t referenceOut;
  75     int16_t filterOut;
  76     uint16_t sampleIdx;
  77     int16_t freq;
  78
  79     uint16_t sampleCount = sizeof(servoCmds) / sizeof(int16_t);
  80
  81     // generate inputs and expecteds
  82     for (sampleIdx = 0; sampleIdx < sampleCount; sampleIdx++) {
  83         if (sampleIdx < 1000) {
  84             servoCmds[sampleIdx] = 500;
  85         } else if (sampleIdx >= 1000 && sampleIdx < 2000) {
  86             servoCmds[sampleIdx] = 2500;
  87         } else {
  88             servoCmds[sampleIdx] = 1500;
  89         }
  90
  91         if ((sampleIdx >= 900 && sampleIdx < 1000) ||
  92             (sampleIdx >= 1900 && sampleIdx < 2000)||
  93             (sampleIdx >= 2900 && sampleIdx < 3000)) {
  94             expectedOut[sampleIdx] = servoCmds[sampleIdx];
  95         } else {
  96             expectedOut[sampleIdx] = -1;
  97         }
  98     }
  99
 100     // Test all frequencies
 101     for (freq = 10; freq <= 400; freq++) {
 102 #ifdef DEBUG_LOWPASS
 103         printf("*** Testing freq: %d (%f)\n", freq, ((float)freq * 0.001f));
 104 #endif
 105         // Run tests
 106         for (sampleIdx = 0; sampleIdx < sampleCount; sampleIdx++)
 107         {
 108             // Filter under test
 109             filterOut = (int16_t)lowpassFixed(&lowpassFilter, servoCmds[sampleIdx], freq);
 110
 111             // Floating-point reference
 112             referenceOut = (int16_t)(lowpassRef(&lowpassFilterReference, (float)servoCmds[sampleIdx], freq) + 0.5f);
 113
 114             if (expectedOut[sampleIdx] >= 0) {
 115                 EXPECT_EQ(filterOut, expectedOut[sampleIdx]);
 116             }
 117             // Some tolerance
 118             // TODO adjust precision to remove the need for this?
 119             EXPECT_LE(filterOut, referenceOut + 1);
 120             EXPECT_GE(filterOut, referenceOut - 1);
 121         } // for each sample
 122     } // for each freq
 123 }
 124
 125 // STUBS
 126
 127 extern "C" {
 128
 129 }
 130
 131