SigProc.cpp

#include "SigProc.h"

#define N_WAVETABLE_bits           10
#define N_WAVETABLE                (1 << N_WAVETABLE_bits)
#define N_WAVETABLE_HALF           (N_WAVETABLE >> 1)
#define N_WAVETABLE_QUARTER        (N_WAVETABLE >> 2)

// 0 Hz - 8000 Hz, gain = 1, ripple = 0.1 dB
// 10220 Hz - 20480 Hz, gain = 0, attenuation = -73 dB
#define FILTER_TAP_NUM             64
#define FILTER_TAP_NUM_HALF        (FILTER_TAP_NUM >> 1)

#define BARS                       64

volatile int16_t SigProc::sampleRb[RINGBUFFER_SIZE];
volatile uint32_t SigProc::samplePtrIn;
volatile uint32_t SigProc::samplePtrOut;
volatile uint32_t SigProc::RbOvFlag;
volatile byte SigProc::m_adcPin;

const int16_t COS_3WAVE4_TABLE[N_WAVETABLE - N_WAVETABLE_QUARTER] = {
  32767, 32766, 32764, 32761, 32757, 32751, 32744, 32736,
  32727, 32717, 32705, 32692, 32678, 32662, 32646, 32628,
  32609, 32588, 32567, 32544, 32520, 32495, 32468, 32441,
  32412, 32382, 32350, 32318, 32284, 32249, 32213, 32176,
  32137, 32097, 32056, 32014, 31970, 31926, 31880, 31833,
  31785, 31735, 31684, 31633, 31580, 31525, 31470, 31413,
  31356, 31297, 31236, 31175, 31113, 31049, 30984, 30918,
  30851, 30783, 30713, 30643, 30571, 30498, 30424, 30349,
  30272, 30195, 30116, 30036, 29955, 29873, 29790, 29706,
  29621, 29534, 29446, 29358, 29268, 29177, 29085, 28992,
  28897, 28802, 28706, 28608, 28510, 28410, 28309, 28208,
  28105, 28001, 27896, 27790, 27683, 27575, 27466, 27355,
  27244, 27132, 27019, 26905, 26789, 26673, 26556, 26437,
  26318, 26198, 26077, 25954, 25831, 25707, 25582, 25456,
  25329, 25201, 25072, 24942, 24811, 24679, 24546, 24413,
  24278, 24143, 24006, 23869, 23731, 23592, 23452, 23311,
  23169, 23027, 22883, 22739, 22594, 22448, 22301, 22153,
  22004, 21855, 21705, 21554, 21402, 21249, 21096, 20942,
  20787, 20631, 20474, 20317, 20159, 20000, 19840, 19680,
  19519, 19357, 19194, 19031, 18867, 18702, 18537, 18371,
  18204, 18036, 17868, 17699, 17530, 17360, 17189, 17017,
  16845, 16672, 16499, 16325, 16150, 15975, 15799, 15623,
  15446, 15268, 15090, 14911, 14732, 14552, 14372, 14191,
  14009, 13827, 13645, 13462, 13278, 13094, 12909, 12724,
  12539, 12353, 12166, 11980, 11792, 11604, 11416, 11227,
  11038, 10849, 10659, 10469, 10278, 10087, 9895, 9703,
 9511, 9319, 9126, 8932, 8739, 8545, 8351, 8156,
 7961, 7766, 7571, 7375, 7179, 6982, 6786, 6589,
 6392, 6195, 5997, 5799, 5601, 5403, 5205, 5006,
 4807, 4608, 4409, 4210, 4011, 3811, 3611, 3411,
 3211, 3011, 2811, 2610, 2410, 2209, 2009, 1808,
 1607, 1406, 1206, 1005, 804, 603, 402, 201,
   
 0, -200, -401, -602, -803, -1004, -1205, -1405,
 -1606, -1807, -2008, -2208, -2409, -2609, -2810, -3010,
 -3210, -3410, -3610, -3810, -4010, -4209, -4408, -4607,
 -4806, -5005, -5204, -5402, -5600, -5798, -5996, -6194,
 -6391, -6588, -6785, -6981, -7178, -7374, -7570, -7765,
 -7960, -8155, -8350, -8544, -8738, -8931, -9125, -9318,
 -9510, -9702, -9894, -10086, -10277, -10468, -10658, -10848,
 -11037, -11226, -11415, -11603, -11791, -11979, -12165, -12352,
 -12538, -12723, -12908, -13093, -13277, -13461, -13644, -13826,
 -14008, -14190, -14371, -14551, -14731, -14910, -15089, -15267,
 -15445, -15622, -15798, -15974, -16149, -16324, -16498, -16671,
 -16844, -17016, -17188, -17359, -17529, -17698, -17867, -18035,
 -18203, -18370, -18536, -18701, -18866, -19030, -19193, -19356,
 -19518, -19679, -19839, -19999, -20158, -20316, -20473, -20630,
 -20786, -20941, -21095, -21248, -21401, -21553, -21704, -21854,
 -22003, -22152, -22300, -22447, -22593, -22738, -22882, -23026,
 -23168, -23310, -23451, -23591, -23730, -23868, -24005, -24142,
 -24277, -24412, -24545, -24678, -24810, -24941, -25071, -25200,
 -25328, -25455, -25581, -25706, -25830, -25953, -26076, -26197,
 -26317, -26436, -26555, -26672, -26788, -26904, -27018, -27131,
 -27243, -27354, -27465, -27574, -27682, -27789, -27895, -28000,
 -28104, -28207, -28308, -28409, -28509, -28607, -28705, -28801,
 -28896, -28991, -29084, -29176, -29267, -29357, -29445, -29533,
 -29620, -29705, -29789, -29872, -29954, -30035, -30115, -30194,
 -30271, -30348, -30423, -30497, -30570, -30642, -30712, -30782,
 -30850, -30917, -30983, -31048, -31112, -31174, -31235, -31296,
 -31355, -31412, -31469, -31524, -31579, -31632, -31683, -31734,
 -31784, -31832, -31879, -31925, -31969, -32013, -32055, -32096,
 -32136, -32175, -32212, -32248, -32283, -32317, -32349, -32381,
 -32411, -32440, -32467, -32494, -32519, -32543, -32566, -32587,
 -32608, -32627, -32645, -32661, -32677, -32691, -32704, -32716,
 -32726, -32735, -32743, -32750, -32756, -32760, -32763, -32765,

 -32766, -32765, -32763, -32760, -32756, -32750, -32743, -32735,
 -32726, -32716, -32704, -32691, -32677, -32661, -32645, -32627,
 -32608, -32587, -32566, -32543, -32519, -32494, -32467, -32440,
 -32411, -32381, -32349, -32317, -32283, -32248, -32212, -32175,
 -32136, -32096, -32055, -32013, -31969, -31925, -31879, -31832,
 -31784, -31734, -31683, -31632, -31579, -31524, -31469, -31412,
 -31355, -31296, -31235, -31174, -31112, -31048, -30983, -30917,
 -30850, -30782, -30712, -30642, -30570, -30497, -30423, -30348,
 -30271, -30194, -30115, -30035, -29954, -29872, -29789, -29705,
 -29620, -29533, -29445, -29357, -29267, -29176, -29084, -28991,
 -28896, -28801, -28705, -28607, -28509, -28409, -28308, -28207,
 -28104, -28000, -27895, -27789, -27682, -27574, -27465, -27354,
 -27243, -27131, -27018, -26904, -26788, -26672, -26555, -26436,
 -26317, -26197, -26076, -25953, -25830, -25706, -25581, -25455,
 -25328, -25200, -25071, -24941, -24810, -24678, -24545, -24412,
 -24277, -24142, -24005, -23868, -23730, -23591, -23451, -23310,
 -23168, -23026, -22882, -22738, -22593, -22447, -22300, -22152,
 -22003, -21854, -21704, -21553, -21401, -21248, -21095, -20941,
 -20786, -20630, -20473, -20316, -20158, -19999, -19839, -19679,
 -19518, -19356, -19193, -19030, -18866, -18701, -18536, -18370,
 -18203, -18035, -17867, -17698, -17529, -17359, -17188, -17016,
 -16844, -16671, -16498, -16324, -16149, -15974, -15798, -15622,
 -15445, -15267, -15089, -14910, -14731, -14551, -14371, -14190,
 -14008, -13826, -13644, -13461, -13277, -13093, -12908, -12723,
 -12538, -12352, -12165, -11979, -11791, -11603, -11415, -11226,
 -11037, -10848, -10658, -10468, -10277, -10086, -9894, -9702,
 -9510, -9318, -9125, -8931, -8738, -8544, -8350, -8155,
 -7960, -7765, -7570, -7374, -7178, -6981, -6785, -6588,
 -6391, -6194, -5996, -5798, -5600, -5402, -5204, -5005,
 -4806, -4607, -4408, -4209, -4010, -3810, -3610, -3410,
 -3210, -3010, -2810, -2609, -2409, -2208, -2008, -1807,
 -1606, -1405, -1205, -1004, -803, -602, -401, -200
};

const int16_t HANN_WINDOW[N_WAVETABLE_HALF] = {
  0, 0, 1, 3, 5, 8, 11, 15,
  20, 25, 31, 37, 44, 52, 61, 69, 
  79, 89, 100, 111, 123, 136, 149, 163, 
  178, 193, 208, 225, 242, 259, 277, 296, 
  315, 335, 356, 377, 399, 421, 444, 468, 
  492, 517, 542, 568, 595, 622, 650, 678, 
  707, 736, 766, 797, 829, 860, 893, 926, 
  960, 994, 1029, 1064, 1100, 1136, 1174, 1211, 
  1250, 1288, 1328, 1368, 1408, 1449, 1491, 1533, 
  1576, 1619, 1663, 1708, 1753, 1798, 1844, 1891, 
  1938, 1986, 2034, 2083, 2133, 2182, 2233, 2284, 
  2335, 2387, 2440, 2493, 2547, 2601, 2655, 2711, 
  2766, 2823, 2879, 2937, 2994, 3053, 3111, 3170, 
  3230, 3290, 3351, 3412, 3474, 3536, 3599, 3662, 
  3726, 3790, 3855, 3920, 3985, 4051, 4118, 4185, 
  4252, 4320, 4388, 4457, 4526, 4596, 4666, 4737, 
  4808, 4879, 4951, 5023, 5096, 5169, 5243, 5317, 
  5391, 5466, 5541, 5617, 5693, 5769, 5846, 5923, 
  6001, 6079, 6157, 6236, 6315, 6395, 6475, 6555, 
  6636, 6717, 6798, 6880, 6962, 7045, 7128, 7211, 
  7294, 7378, 7463, 7547, 7632, 7717, 7803, 7889, 
  7975, 8061, 8148, 8235, 8323, 8411, 8499, 8587, 
  8676, 8765, 8854, 8943, 9033, 9123, 9214, 9304, 
  9395, 9486, 9578, 9669, 9761, 9853, 9946, 10038, 
  10131, 10224, 10318, 10411, 10505, 10599, 10693, 10788, 
  10883, 10977, 11073, 11168, 11263, 11359, 11455, 11551, 
  11647, 11744, 11840, 11937, 12034, 12131, 12228, 12326, 
  12423, 12521, 12619, 12717, 12815, 12913, 13012, 13110, 
  13209, 13308, 13407, 13506, 13605, 13704, 13803, 13903, 
  14002, 14102, 14201, 14301, 14401, 14501, 14601, 14701, 
  14801, 14901, 15002, 15102, 15202, 15303, 15403, 15503, 
  15604, 15704, 15805, 15906, 16006, 16107, 16207, 16308, 
  
  16409, 16509, 16610, 16711, 16811, 16912, 17012, 17113, 
  17213, 17314, 17414, 17515, 17615, 17715, 17816, 17916, 
  18016, 18116, 18216, 18316, 18416, 18516, 18615, 18715, 
  18815, 18914, 19014, 19113, 19212, 19311, 19410, 19509, 
  19608, 19706, 19805, 19903, 20001, 20099, 20197, 20295, 
  20393, 20490, 20587, 20685, 20782, 20878, 20975, 21072, 
  21168, 21264, 21360, 21456, 21551, 21647, 21742, 21837, 
  21932, 22026, 22121, 22215, 22309, 22403, 22496, 22589, 
  22682, 22775, 22868, 22960, 23052, 23144, 23235, 23326, 
  23417, 23508, 23599, 23689, 23779, 23868, 23958, 24047, 
  24136, 24224, 24312, 24400, 24488, 24575, 24662, 24749, 
  24835, 24921, 25007, 25092, 25178, 25262, 25347, 25431, 
  25514, 25598, 25681, 25764, 25846, 25928, 26009, 26091, 
  26172, 26252, 26332, 26412, 26491, 26570, 26649, 26727, 
  26805, 26882, 26960, 27036, 27112, 27188, 27264, 27339, 
  27413, 27488, 27561, 27635, 27708, 27780, 27852, 27924, 
  27995, 28066, 28136, 28206, 28275, 28344, 28413, 28481, 
  28549, 28616, 28683, 28749, 28815, 28880, 28945, 29009, 
  29073, 29136, 29199, 29262, 29324, 29385, 29446, 29507, 
  29567, 29626, 29685, 29744, 29802, 29859, 29916, 29973, 
  30029, 30084, 30139, 30193, 30247, 30301, 30353, 30406, 
  30457, 30509, 30559, 30610, 30659, 30708, 30757, 30805, 
  30852, 30899, 30946, 30992, 31037, 31082, 31126, 31169, 
  31212, 31255, 31297, 31338, 31379, 31419, 31459, 31498, 
  31537, 31575, 31612, 31649, 31685, 31721, 31756, 31790, 
  31824, 31858, 31890, 31923, 31954, 31985, 32016, 32045, 
  32075, 32103, 32131, 32159, 32186, 32212, 32238, 32263, 
  32287, 32311, 32334, 32357, 32379, 32401, 32421, 32442, 
  32461, 32480, 32499, 32517, 32534, 32550, 32566, 32582, 
  32597, 32611, 32624, 32637, 32650, 32661, 32672, 32683, 
  32693, 32702, 32711, 32719, 32726, 32733, 32739, 32745, 
  32750, 32754, 32758, 32761, 32763, 32765, 32766, 32767
};

/*
const int16_t BLACKMAN_NUTTALL_WINDOW[N_WAVETABLE_HALF] = {
  12, 12, 12, 12, 12, 12, 13, 13,
  13, 14, 14, 15, 15, 16, 17, 17,
  18, 19, 20, 21, 22, 23, 24, 25,
  26, 27, 29, 30, 32, 33, 35, 36,
  38, 40, 42, 44, 45, 48, 50, 52,
  54, 57, 59, 61, 64, 67, 70, 72,
  75, 78, 82, 85, 88, 92, 95, 99,
  103, 106, 110, 114, 119, 123, 127, 132,
  137, 141, 146, 151, 157, 162, 167, 173,
  179, 185, 191, 197, 203, 210, 217, 223,
  230, 238, 245, 252, 260, 268, 276, 284,
  293, 302, 310, 319, 329, 338, 348, 358,
  368, 378, 389, 399, 411, 422, 433, 445,
  457, 469, 482, 494, 507, 521, 534, 548,
  562, 576, 591, 606, 621, 637, 653, 669,
  685, 702, 719, 736, 754, 772, 791, 809,
  828, 848, 867, 888, 908, 929, 950, 972,
  993, 1016, 1038, 1061, 1085, 1109, 1133, 1158,
  1183, 1208, 1234, 1260, 1287, 1314, 1342, 1370,
  1398, 1427, 1457, 1486, 1517, 1547, 1579, 1610,
  1642, 1675, 1708, 1742, 1776, 1810, 1846, 1881,
  1917, 1954, 1991, 2029, 2067, 2105, 2145, 2184,
  2225, 2266, 2307, 2349, 2391, 2435, 2478, 2522,
  2567, 2612, 2658, 2705, 2752, 2799, 2848, 2897,
  2946, 2996, 3047, 3098, 3150, 3202, 3255, 3309,
  3363, 3418, 3474, 3530, 3587, 3644, 3702, 3761,
  3821, 3881, 3941, 4003, 4065, 4127, 4191, 4255,
  4319, 4384, 4450, 4517, 4584, 4652, 4721, 4790,
  4860, 4931, 5002, 5074, 5147, 5220, 5294, 5369,
  5445, 5521, 5597, 5675, 5753, 5832, 5912, 5992,
  6073, 6154, 6237, 6320, 6403, 6488, 6573, 6658,
  6745, 6832, 6920, 7008, 7097, 7187, 7278, 7369,
  
  7461, 7553, 7646, 7740, 7835, 7930, 8026, 8122,
  8220, 8317, 8416, 8515, 8615, 8715, 8817, 8918,
  9021, 9124, 9227, 9332, 9437, 9542, 9648, 9755,
  9863, 9971, 10079, 10188, 10298, 10409, 10520, 10631,
  10743, 10856, 10969, 11083, 11197, 11312, 11428, 11544,
  11660, 11777, 11895, 12013, 12132, 12251, 12370, 12491,
  12611, 12732, 12854, 12976, 13098, 13221, 13344, 13468,
  13592, 13717, 13842, 13967, 14093, 14219, 14346, 14473,
  14600, 14728, 14856, 14984, 15113, 15242, 15371, 15501,
  15631, 15761, 15891, 16022, 16153, 16284, 16416, 16547,
  16679, 16811, 16943, 17076, 17208, 17341, 17474, 17607,
  17741, 17874, 18007, 18141, 18275, 18408, 18542, 18676,
  18810, 18944, 19078, 19212, 19346, 19480, 19614, 19747,
  19881, 20015, 20149, 20283, 20416, 20550, 20683, 20816,
  20949, 21082, 21215, 21347, 21480, 21612, 21744, 21876,
  22007, 22139, 22270, 22400, 22531, 22661, 22791, 22920,
  23049, 23178, 23307, 23435, 23563, 23690, 23817, 23943,
  24069, 24195, 24320, 24445, 24569, 24693, 24816, 24938,
  25060, 25182, 25303, 25423, 25543, 25663, 25781, 25899,
  26017, 26133, 26249, 26365, 26479, 26593, 26707, 26819,
  26931, 27042, 27153, 27262, 27371, 27479, 27586, 27693,
  27798, 27903, 28007, 28110, 28212, 28314, 28414, 28513,
  28612, 28710, 28806, 28902, 28997, 29091, 29184, 29276,
  29367, 29456, 29545, 29633, 29720, 29806, 29890, 29974,
  30057, 30138, 30218, 30298, 30376, 30453, 30529, 30604,
  30677, 30750, 30821, 30891, 30960, 31028, 31094, 31160,
  31224, 31287, 31349, 31409, 31468, 31526, 31583, 31638,
  31693, 31746, 31797, 31848, 31897, 31944, 31991, 32036,
  32080, 32122, 32164, 32204, 32242, 32279, 32315, 32350,
  32383, 32415, 32445, 32474, 32502, 32528, 32554, 32577,
  32599, 32620, 32640, 32658, 32675, 32690, 32704, 32717,
  32728, 32737, 32746, 32753, 32758, 32763, 32765, 32767
};
*/

const int16_t FIRcoeff[FILTER_TAP_NUM_HALF] = {
  -6, 9, 41, 46, -8, -61, -18, 81,
  69, -80, -138, 41, 211, 48, -262, -189,
  260, 374, -170, -573, -40, 742, 398, -817,
  -927, 705, 1682, -240, -2877, -1168, 6090, 13161
};

void SigProc::Begin(Settings *settings, SensorData *sensorData, Statistics *statistics, Publisher *publisher) {
  m_sensorData = sensorData;
  m_settings = settings;
  m_statistics = statistics;
  m_publisher = publisher;
  m_isCapturing = false;
  m_adcPin = m_settings->GetByte("ADCPIN", 33);

  publishInterval = m_settings->GetUInt("PublishInterval", DEFAULT_PUBLISH_INTERVAL);

  // Initialize the ADC
  analogSetAttenuation(ADC_6db);
  analogSetWidth(12);
  analogSetCycles(8);
  analogSetSamples(1);
  analogSetClockDiv(1);
  adcAttachPin(m_adcPin);

  // Initialize the sampling timer
  timer = timerBegin(0, CPU_CLOCK / SAMPLE_RATE, true);
  timerAlarmWrite(timer, 1, true);
  timerAttachInterrupt(timer, &SigProc::onTimer, true);
}

void IRAM_ATTR SigProc::onTimer()
{
  //static int16_t firRb[256];        // can be reduced to FILTER_TAP_NUM if needed
  static int16_t firRb[FILTER_TAP_NUM];
  static uint8_t firRbPtr = 0;
  int32_t FIRsum;

#ifdef DEBUG
  digitalWrite(DEBUG_GPIO_ISR, HIGH);
#endif

  firRb[firRbPtr] = analogRead(m_adcPin) - 2048;     // approx. 10 us

  // 2x decimation
  if (firRbPtr & 0x01) {
    
    //sampleRb[samplePtrIn] = (firRb[firRbPtr] + firRb[(uint8_t)(firRbPtr - 1)]) >> 1;
    FIRsum = 0;
    for (uint16_t i = 0; i < FILTER_TAP_NUM_HALF; i++) {
      FIRsum += FIRcoeff[i] * (firRb[(firRbPtr - i) & (FILTER_TAP_NUM - 1)] + firRb[(firRbPtr - (FILTER_TAP_NUM - 1) + i) & (FILTER_TAP_NUM - 1)]);
    }
    sampleRb[samplePtrIn] = FIRsum >> 15;
    
    samplePtrIn = (++samplePtrIn) & (RINGBUFFER_SIZE - 1);    
    if (samplePtrIn == samplePtrOut) {
      RbOvFlag = 1;
    }
  }

  firRbPtr = (firRbPtr + 1) & (FILTER_TAP_NUM - 1);
  
#ifdef DEBUG
  digitalWrite(DEBUG_GPIO_ISR, LOW);
#endif
}

void SigProc::Handle()
{
  uint16_t clippingCtr_tmp;
  
  while (SnapPending()) {
    
#ifdef DEBUG    
    digitalWrite(DEBUG_GPIO_MAIN, HIGH);
#endif

    m_sensorData->snapshotCtr++;
    clippingCtr_tmp = m_sensorData->clippingCtr;
    Calc();

    // check if neither clipping nor ringbuffer overflows occurred before or during signal processing
    if (RbOvFlag) {
      m_sensorData->RbOvCtr++;
      RbOvFlag = 0;
    } else if (m_sensorData->clippingCtr == clippingCtr_tmp) {
      m_sensorData->snapshotValidCtr++;
      m_statistics->Calc();
    }

    // 25 ms snapshot interval --> 1/0,025 ms = 40 snapshots/s (ringbuffer overflows ignored)
    if (m_sensorData->snapshotCtr >= (40 * publishInterval)) {
      m_statistics->Finalize();
      m_publisher->Publish(m_sensorData);
        
#ifdef DEBUG
      // FOR DEBUG PURPOSE ONLY
      StopCapture();
      //DebugConsoleOutSamples(0, 40);
      DebugConsoleOutBins(0, 7);
      DebugConsoleOutBins(120, 127);
      DebugConsoleOutBins(248, 255);
      DebugConsoleOutBins(376, 383);
      DebugConsoleOutBins(504, 511);
      StartCapture();
#endif

      m_statistics->Reset();
    }

#ifdef DEBUG
    digitalWrite(DEBUG_GPIO_MAIN, LOW);    
#endif

  }
}

void SigProc::StartCapture() {
  samplePtrIn = 0;
  samplePtrOut = 0;
  if (timer) {
    timerAlarmEnable(timer);
    while (samplePtrIn < NR_OF_FFT_SAMPLES) {}
    m_isCapturing = true;
  } 
}

void SigProc::StopCapture() {
  if (timer) {
    timerAlarmDisable(timer);
  }
  m_isCapturing = false;
}

uint8_t SigProc::SnapPending() {
  uint32_t samplePtrIn_tmp;
  uint32_t diff;

  samplePtrIn_tmp = samplePtrIn;
  
  if (samplePtrIn_tmp > samplePtrOut) {
    diff = samplePtrIn_tmp - samplePtrOut;
  } else {
    diff = (samplePtrIn_tmp + RINGBUFFER_SIZE) - samplePtrOut;
  }

  // bufferPtrIn must be at least NR_OF_FFT_SAMPLES ahead
  if (diff >= NR_OF_FFT_SAMPLES) {
    return 1;
  }

  return 0;
}

void SigProc::Calc()
{
  uint16_t lastSampleIdx;
  uint16_t RBidx;
  int16_t sample;
  int32_t sampleSUM;
  uint16_t sampleABS;
  int16_t re[NR_OF_FFT_SAMPLES];
  int16_t im[NR_OF_FFT_SAMPLES];

  // Do not modify the sampleRb content!

  lastSampleIdx = (samplePtrOut + NR_OF_FFT_SAMPLES) & (RINGBUFFER_SIZE - 1);

  // calculate ADC offset
  sampleSUM = 0;
  for (uint16_t i = 0; i < NR_OF_FFT_SAMPLES; i++) {
    RBidx = (samplePtrOut + i) & (RINGBUFFER_SIZE - 1);
    sampleSUM += sampleRb[RBidx];
  }  
  m_sensorData->ADCoffset = sampleSUM >> NR_OF_FFT_SAMPLES_bit;
  
  // process each sample
  for (uint16_t i = 0; i < NR_OF_FFT_SAMPLES; i++) {
    RBidx = (samplePtrOut + i) & (RINGBUFFER_SIZE - 1);
    sample = sampleRb[RBidx];

    if ((sample < -2047) || (sample > 2046)) {
      m_sensorData->clippingCtr++;
    }
    
    sampleABS = abs(sample);
    if (sampleABS > m_sensorData->ADCpeakSample) {
      m_sensorData->ADCpeakSample = sampleABS;
    }
    
    sample -= m_sensorData->ADCoffset;
    
    re[i] = sample << 4;
    im[i] = 0;
  }

  Window(re, NR_OF_FFT_SAMPLES_bit, (int16_t*)HANN_WINDOW);
  FFT(re, im, NR_OF_FFT_SAMPLES_bit);

  for (uint16_t binNr = 0; binNr < NR_OF_BINS; binNr++) {
    m_sensorData->bin[binNr].mag = sqrt(pow(re[binNr], 2) + pow(im[binNr], 2));
  }
 
  samplePtrOut = (samplePtrOut + (NR_OF_FFT_SAMPLES >> 1)) & (RINGBUFFER_SIZE - 1);
}

bool SigProc::IsCapturing() {
  return m_isCapturing;
}

inline int16_t SigProc::MAS(int16_t a, int16_t b) {
  int16_t c;
  c = ((int32_t)a * (int32_t)b) >> 15;
  if (c < 0) {
    c++;
  }
  return c;
}

void SigProc::Window(int16_t *re, uint8_t m, int16_t *windowTable) {
  uint16_t tableIndex;
  uint16_t indexStep;
  uint16_t N;

  N = 1 << m;
  if (N > N_WAVETABLE)
    return;

  indexStep = N_WAVETABLE / N;
  tableIndex = 0;

  for (uint16_t n = 0; n < (N >> 1); n++) {
    re[n] = MAS(re[n], windowTable[tableIndex]);
    tableIndex += indexStep;
  }
  for (uint16_t n = (N >> 1); n < N; n++) {
    tableIndex -= indexStep;
    re[n] = MAS(re[n], windowTable[tableIndex]);
  }
}

void SigProc::FFT(int16_t *fr, int16_t *fi, uint16_t m) {
  uint16_t N;
  uint16_t Nn;
  uint16_t mr;
  uint16_t l; 
  uint16_t i;
  uint16_t j;
  uint8_t k;
  uint8_t shift;
  uint16_t istep;
  int16_t qr, qi, tr, ti, wr, wi;
  
  N = 1 << m;

  if (N > N_WAVETABLE) {
    return;
  }

  mr = 0;
  Nn = N - 1;

  for (m = 1; m <= Nn; m++) {
    l = N;

    do {
      l >>= 1;
    } while ((mr + l) > Nn);

    mr = (mr & (l - 1)) + l;

    if (mr <= m) {
      continue;
    }

    tr = fr[m];
    fr[m] = fr[mr];
    fr[mr] = tr;
    
    ti = fi[m];
    fi[m] = fi[mr];
    fi[mr] = ti;
  }

  l = 1;    
  k = N_WAVETABLE_bits - 1;

  while (l < N) {
    shift = 1;
    istep = l << 1;

    for (m = 0; m < l; m++) {
      j = m << k;

      wr = COS_3WAVE4_TABLE[j];
      wi = COS_3WAVE4_TABLE[N_WAVETABLE_QUARTER + j];
      
      if (shift) {
        wr >>= 1;
        wi >>= 1;
      }

      for (i = m; i < N; i += istep) {
        j = i + l;
          
        tr = MAS(wr, fr[j]) - MAS(wi, fi[j]);
        ti = MAS(wr, fi[j]) + MAS(wi, fr[j]);
          
        qr = fr[i];
        qi = fi[i];
          
        if (shift)  {
          if (qr < 0)
            qr++;
  
          if (qi < 0)
            qi++;

          qr >>= 1;
          qi >>= 1;
        }

        fr[j] = qr - tr;
        fi[j] = qi - ti;
        fr[i] = qr + tr;
        fi[i] = qi + ti;
      }
    }

    k--;
    l = istep;
  }
}

void SigProc::DebugConsoleOutSamples(uint16_t startIdx, uint16_t stopIdx) {
  Serial.printf("\nSnapshots: %d   ADC-offset: %d   Clipping: %d   ADCpeak: %d\n", m_sensorData->snapshotValidCtr, m_sensorData->ADCoffset, m_sensorData->clippingCtr, m_sensorData->ADCpeakSample);

  if ((startIdx >= NR_OF_BINS) || (stopIdx >= NR_OF_BINS)) {
    Serial.println("Invalid sample index!");
    return;
  }
  
  for (uint16_t sampleNr = startIdx; sampleNr <= stopIdx; sampleNr++) {
    Serial.printf("SAMPLE %03d ", sampleNr);
    for (uint8_t x = 0; x < BARS; x++) {
      if (x < (((sampleRb[sampleNr] + 2048) * BARS) / 4096))
        Serial.printf("#");
      else
        Serial.printf("-");
    }
    Serial.printf(" %d\n", sampleRb[sampleNr]);
  }
}

void SigProc::DebugConsoleOutBins(uint16_t startIdx, uint16_t stopIdx) {
  Serial.printf("\nSnapshots: %d\n", m_sensorData->snapshotValidCtr);

  if ((startIdx >= NR_OF_BINS) || (stopIdx >= NR_OF_BINS)) {
    Serial.println("Invalid bin index!");
    return;
  }

  for (uint16_t binNr = startIdx; binNr <= stopIdx; binNr++) {
    Serial.printf("BIN %03d ", binNr);
    for (uint8_t x = 0; x < BARS; x++) {
      //if (x < ((m_sensorData->bin[binNr].magMax * BARS) / 16383))if (x < ((m_sensorData->bin[binNr].magMax * BARS) / 16383))
      if (x < ((m_sensorData->bin[binNr].magMax * BARS) / 16383))if (x < ((m_sensorData->bin[binNr].magMax * BARS)))
        Serial.printf("#");
      else
        Serial.printf("-");
    }
    Serial.printf(" %d\n", m_sensorData->bin[binNr].magMax);
  }
}