stm32F4xx_fft.c

#include "stm32f4xx.h"
#include "stm32F4xx_fft.h"
#include <stdlib.h>
#include "arm_math.h"

#include "stm32adc.h"
#include "gcode_parser.h"
#include "PGA_Ctrl.h"

int32_t DetectFeq(u32* samples, u16 size);
extern int32_t Maxpeak1, Maxpeak2, Maxpeakk;
extern int16_t ADCBuffer[FFT_LENGTH2];
extern int16_t ADCBuffer1[FFT_LENGTH];
extern int16_t ADCBuffer2[FFT_LENGTH];
extern int Peak_send[20][2];
extern int32_t Maxpeak_d;
extern float Freq_Send;
/* ------------------------------------------------------------------
* Global variables for FFT Bin Example
* ------------------------------------------------------------------- */

#if FFT_LENGTH == 256

#ifdef USE32BITFFT
// max. ((2^31)/4096) - 1
/*const int32_t HannTable256[512]={20,79,178,316,493,710,967,1262,1597,1971,2385,2837,3329,3859,4429,5037,5684,
								  6370,7094,7856,8657,9496,10373,11288,12241,13231,14259,15324,16426,17566,18742,
								  19954,21204,22489,23811,25169,26562,27991,29455,30954,32487,34056,35659,37296,
								  38966,40671,42408,44179,45983,47819,49687,51588,53520,55484,57478,59504,61560,
								  63646,65762,67908,70083,72287,74519,76780,79069,81385,83728,86099,88496,90918,
								  93367,95841,98341,100865,103413,105985,108580,111199,113841,116504,119190,121897,
								  124626,127375,130144,132933,135742,138570,141416,144281,147163,150063,152979,
								  155912,158861,161826,164805,167799,170808,173830,176866,179914,182975,186047,
								  189131,192226,195332,198448,201573,204708,207851,211002,214161,217327,220500,
								  223679,226864,230054,233249,236449,239652,242859,246069,249281,252495,255710,
								  258927,262143,265360,268577,271792,275006,278218,281428,284635,287838,291038,
								  294233,297423,300608,303787,306960,310126,313285,316436,319579,322714,325839,
								  328955,332061,335156,338240,341312,344373,347421,350457,353479,356488,359482,
								  362461,365426,368375,371308,374224,377124,380006,382871,385717,388545,391354,
								  394143,396912,399661,402390,405097,407783,410446,413088,415707,418302,420874,
								  423422,425946,428446,430920,433369,435791,438188,440559,442902,445218,447507,
								  449768,452000,454204,456379,458525,460641,462727,464783,466809,468803,470767,
								  472699,474600,476468,478304,480108,481879,483616,485321,486991,488628,490231,
								  491800,493333,494832,496296,497725,499118,500476,501798,503083,504333,505545,
								  506721,507861,508963,510028,511056,512046,512999,513914,514791,515630,516431,
								  517193,517917,518603,519250,519858,520428,520958,521450,521902,522316,522690,
								  523025,523320,523577,523794,523971,524109,524208,524267,524287,524267,524208,
								  524109,523971,523794,523577,523320,523025,522690,522316,521902,521450,520958,
								  520428,519858,519250,518603,517917,517193,516431,515630,514791,513914,512999,
								  512046,511056,510028,508963,507861,506721,505545,504333,503083,501798,500476,
								  499118,497725,496296,494832,493333,491800,490231,488628,486991,485321,483616,
								  481879,480108,478304,476468,474600,472699,470767,468803,466809,464783,462727,
								  460641,458525,456379,454204,452000,449768,447507,445218,442902,440559,438188,
								  435791,433369,430920,428446,425946,423422,420874,418302,415707,413088,410446,
								  407783,405097,402390,399661,396912,394143,391354,388545,385717,382871,380006,
								  377124,374224,371308,368375,365426,362461,359482,356488,353479,350457,347421,
								  344373,341312,338240,335156,332061,328955,325839,322714,319579,316436,313285,
								  310126,306960,303787,300608,297423,294233,291038,287838,284635,281428,278218,
								  275006,271792,268577,265360,262144,258927,255710,252495,249281,246069,242859,
								  239652,236449,233249,230054,226864,223679,220500,217327,214161,211002,207851,
								  204708,201573,198448,195332,192226,189131,186047,182975,179914,176866,173830,
								  170808,167799,164805,161826,158861,155912,152979,150063,147163,144281,141416,
								  138570,135742,132933,130144,127375,124626,121897,119190,116504,113841,111199,
								  108580,105985,103413,100865,98341,95841,93367,90918,88496,86099,83728,81385,
								  79069,76780,74519,72287,70083,67908,65762,63646,61560,59504,57478,55484,53520,
								  51588,49687,47819,45983,44179,42408,40671,38966,37296,35659,34056,32487,30954,
								  29455,27991,26562,25169,23811,22489,21204,19954,18742,17566,16426,15324,14259,
								  13231,12241,11288,10373,9496,8657,7856,7094,6370,5684,5037,4429,3859,3329,2837,
								  2385,1971,1597,1262,967,710,493,316,178,79,20,0};*/
#else
/*const int16_t HannTable256[512]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
								  0,0,0,0,0,0,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,
								  1,1,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,3,3,3,3,3,3,3,3,3,3,
								  3,3,3,3,3,3,3,3,3,3,3,3,3,3,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,5,
								  5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,6,6,6,6,6,6,6,6,6,6,6,6,6,
								  6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,
								  7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,
								  7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,6,6,6,6,
								  6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,5,5,5,5,5,5,5,5,
								  5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,
								  4,4,4,4,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,2,2,2,2,2,2,2,2,2,2,2,
								  2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,
								  1,1,1,1,1,1,1,1,1,1,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
								  0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};*/
#endif

#else

#ifdef USE32BITFFT
#if FFT_LENGTH == 1024
const int32_t HanningTable1024[1024]={	0,5,20,44,79,123,178,242,316,400,493,597,710,834,967,1110,1262,1425,1597,1779,1971,
										2173,2385,2606,2837,3078,3329,3589,3859,4139,4429,4728,5037,5356,5684,6022,6370,6727,
										7094,7470,7856,8252,8657,9072,9496,9930,10373,10826,11288,11759,12241,12731,13231,13740,
										14259,14787,15324,15870,16426,16991,17566,18149,18742,19344,19955,20575,21204,21842,22489,
										23146,23811,24485,25169,25861,26562,27272,27991,28718,29455,30200,30954,31716,32487,33267,
										34056,34853,35659,36473,37296,38127,38966,39814,40671,41535,42408,43290,44179,45077,45983,
										46897,47819,48749,49688,50634,51588,52550,53520,54498,55484,56477,57478,58487,59504,60528,
										61560,62599,63646,64701,65762,66832,67908,68992,70083,71181,72287,73400,74519,75646,76780,
										77921,79069,80224,81385,82553,83729,84910,86099,87294,88496,89704,90919,92140,93367,94601,
										95842,97088,98341,99600,100865,102136,103413,104696,105985,107280,108581,109887,111199,112517,
										113841,115170,116505,117845,119190,120541,121898,123259,124626,125998,127375,128757,130144,
										131537,132934,134336,135742,137154,138570,139991,141417,142847,144281,145720,147163,148611,
										150063,151519,152979,154444,155912,157385,158861,160342,161826,163314,164805,166301,167800,
										169302,170808,172318,173830,175346,176866,178388,179914,181443,182975,184510,186048,187588,
										189132,190678,192227,193778,195332,196889,198448,200010,201574,203140,204708,206278,207851,
										209426,211002,212581,214161,215743,217327,218913,220500,222089,223679,225271,226864,228459,
										230055,231652,233250,234849,236449,238051,239653,241256,242859,244464,246069,247675,249281,
										250888,252495,254103,255711,257319,258927,260536,262144,263752,265361,266969,268577,270185,
										271793,273400,275007,276613,278219,279824,281429,283032,284635,286237,287839,289439,291038,
										292636,294233,295829,297424,299017,300609,302199,303788,305375,306961,308545,310127,311707,
										313286,314862,316437,318010,319580,321148,322714,324278,325840,327399,328956,330510,332061,
										333610,335156,336700,338240,339778,341313,342845,344374,345900,347422,348942,350458,351970,
										353480,354986,356488,357987,359483,360974,362462,363946,365427,366903,368376,369844,371309,
										372769,374225,375677,377125,378568,380007,381441,382871,384297,385718,387134,388546,389952,
										391354,392751,394144,395531,396913,398290,399662,401029,402390,403747,405098,406443,407783,
										409118,410447,411771,413089,414401,415707,417008,418303,419592,420875,422152,423423,424688,
										425947,427200,428446,429687,430921,432148,433369,434584,435792,436994,438189,439378,440559,
										441735,442903,444064,445219,446367,447508,448642,449769,450888,452001,453107,454205,455296,
										456380,457456,458526,459587,460642,461689,462728,463760,464784,465801,466810,467811,468804,
										469790,470768,471738,472700,473654,474600,475539,476469,477391,478305,479211,480109,480998,
										481880,482753,483617,484474,485322,486161,486992,487815,488629,489435,490232,491021,491801,
										492572,493334,494088,494833,495570,496297,497016,497726,498427,499119,499803,500477,501142,
										501799,502446,503084,503713,504333,504944,505546,506139,506722,507297,507862,508418,508964,
										509501,510029,510548,511057,511557,512047,512529,513000,513462,513915,514358,514792,515216,
										515631,516036,516432,516818,517194,517561,517918,518266,518604,518932,519251,519560,519859,
										520149,520429,520699,520959,521210,521451,521682,521903,522115,522317,522509,522691,522863,
										523026,523178,523321,523454,523578,523691,523795,523888,523972,524046,524110,524165,524209,
										524244,524268,524283,524288,524283,524268,524244,524209,524165,524110,524046,523972,523888,
										523795,523691,523578,523454,523321,523178,523026,522863,522691,522509,522317,522115,521903,
										521682,521451,521210,520959,520699,520429,520149,519859,519560,519251,518932,518604,518266,
										517918,517561,517194,516818,516432,516036,515631,515216,514792,514358,513915,513462,513000,
										512529,512047,511557,511057,510548,510029,509501,508964,508418,507862,507297,506722,506139,
										505546,504944,504333,503713,503084,502446,501799,501142,500477,499803,499119,498427,497726,
										497016,496297,495570,494833,494088,493334,492572,491801,491021,490232,489435,488629,487815,
										486992,486161,485322,484474,483617,482753,481880,480998,480109,479211,478305,477391,476469,
										475539,474600,473654,472700,471738,470768,469790,468804,467811,466810,465801,464784,463760,
										462728,461689,460642,459587,458526,457456,456380,455296,454205,453107,452001,450888,449769,
										448642,447508,446367,445219,444064,442903,441735,440559,439378,438189,436994,435792,434584,
										433369,432148,430921,429687,428446,427200,425947,424688,423423,422152,420875,419592,418303,
										417008,415707,414401,413089,411771,410447,409118,407783,406443,405098,403747,402390,401029,
										399662,398290,396913,395531,394144,392751,391354,389952,388546,387134,385718,384297,382871,
										381441,380007,378568,377125,375677,374225,372769,371309,369844,368376,366903,365427,363946,
										362462,360974,359483,357987,356488,354986,353480,351970,350458,348942,347422,345900,344374,
										342845,341313,339778,338240,336700,335156,333610,332061,330510,328956,327399,325840,324278,
										322714,321148,319580,318010,316437,314862,313286,311707,310127,308545,306961,305375,303788,
										302199,300609,299017,297424,295829,294233,292636,291038,289439,287839,286237,284635,283032,
										281429,279824,278219,276613,275007,273400,271793,270185,268577,266969,265361,263752,262144,
										260536,258927,257319,255711,254103,252495,250888,249281,247675,246069,244464,242859,241256,
										239653,238051,236449,234849,233250,231652,230055,228459,226864,225271,223679,222089,220500,
										218913,217327,215743,214161,212581,211002,209426,207851,206278,204708,203140,201574,200010,
										198448,196889,195332,193778,192227,190678,189132,187588,186048,184510,182975,181443,179914,
										178388,176866,175346,173830,172318,170808,169302,167800,166301,164805,163314,161826,160342,
										158861,157385,155912,154444,152979,151519,150063,148611,147163,145720,144281,142847,141417,
										139991,138570,137154,135742,134336,132934,131537,130144,128757,127375,125998,124626,123259,
										121898,120541,119190,117845,116505,115170,113841,112517,111199,109887,108581,107280,105985,
										104696,103413,102136,100865,99600,98341,97088,95842,94601,93367,92140,90919,89704,88496,87294,
										86099,84910,83729,82553,81385,80224,79069,77921,76780,75646,74519,73400,72287,71181,70083,
										68992,67908,66832,65762,64701,63646,62599,61560,60528,59504,58487,57478,56477,55484,54498,
										53520,52550,51588,50634,49688,48749,47819,46897,45983,45077,44179,43290,42408,41535,40671,
										39814,38966,38127,37296,36473,35659,34853,34056,33267,32487,31716,30954,30200,29455,28718,
										27991,27272,26562,25861,25169,24485,23811,23146,22489,21842,21204,20575,19955,19344,18742,
										18149,17566,16991,16426,15870,15324,14787,14259,13740,13231,12731,12241,11759,11288,10826,
										10373,9930,9496,9072,8657,8252,7856,7470,7094,6727,6370,6022,5684,5356,5037,4728,4429,4139,
										3859,3589,3329,3078,2837,2606,2385,2173,1971,1779,1597,1425,1262,1110,967,834,710,597,493,
										400,316,242,178,123,79,44,20,5};

/*const int32_t HannTable1024[2048]={1,5,11,20,31,44,60,79,100,123,149,178,208,242,278,316,356,400,445,493,544,597,
									652,710,771,834,899,967,1037,1110,1185,1262,1342,1425,1510,1597,1687,1779,1874,
									1971,2071,2173,2278,2385,2494,2606,2721,2837,2957,3078,3202,3329,3458,3589,3723,
									3859,3998,4139,4283,4429,4577,4728,4881,5037,5195,5356,5519,5684,5852,6022,6195,
									6370,6547,6727,6909,7094,7281,7470,7662,7856,8053,8252,8453,8657,8863,9072,9283,
									9496,9712,9930,10150,10373,10598,10826,11056,11288,11522,11759,11999,12241,
									12485,12731,12980,13231,13484,13740,13998,14259,14521,14787,15054,15324,15596,
									15870,16147,16426,16708,16991,17277,17566,17856,18149,18444,18742,19041,19344,
									19648,19954,20263,20575,20888,21204,21522,21842,22165,22489,22816,23146,23477,
									23811,24147,24485,24826,25169,25514,25861,26210,26562,26916,27272,27630,27991,
									28353,28718,29085,29455,29826,30200,30576,30954,31334,31716,32101,32487,32876,
									33267,33661,34056,34453,34853,35255,35659,36065,36473,36883,37296,37710,38127,
									38545,38966,39389,39814,40241,40671,41102,41535,41971,42408,42848,43290,43733,
									44179,44627,45077,45529,45983,46439,46897,47357,47819,48283,48749,49217,49687,
									50160,50634,51110,51588,52068,52550,53034,53520,54008,54498,54990,55484,55979,
									56477,56977,57478,57982,58487,58995,59504,60015,60528,61043,61560,62079,62599,
									63122,63646,64172,64700,65230,65762,66296,66831,67369,67908,68449,68992,69536,
									70083,70631,71181,71733,72287,72842,73400,73959,74519,75082,75646,76212,76780,
									77350,77921,78494,79069,79645,80223,80803,81385,81968,82553,83140,83728,84318,
									84910,85504,86099,86695,87294,87894,88496,89099,89704,90310,90918,91528,92140,
									92753,93367,93983,94601,95221,95841,96464,97088,97714,98341,98969,99600,100231,
									100865,101499,102136,102773,103413,104054,104696,105340,105985,106632,107280,
									107929,108580,109233,109887,110542,111199,111857,112517,113178,113841,114504,
									115170,115836,116504,117174,117845,118517,119190,119865,120541,121219,121897,
									122577,123259,123942,124626,125311,125998,126686,127375,128065,128757,129450,
									130144,130840,131536,132234,132933,133634,134335,135038,135742,136447,137154,
									137861,138570,139280,139991,140703,141416,142131,142846,143563,144281,145000,
									145720,146441,147163,147886,148611,149336,150063,150790,151519,152249,152979,
									153711,154444,155177,155912,156648,157385,158122,158861,159601,160341,161083,
									161826,162569,163313,164059,164805,165552,166300,167049,167799,168550,169302,
									170054,170808,171562,172317,173073,173830,174588,175346,176105,176866,177626,
									178388,179151,179914,180678,181443,182208,182975,183742,184509,185278,186047,
									186817,187588,188359,189131,189904,190678,191452,192226,193002,193778,194555,
									195332,196110,196889,197668,198448,199228,200009,200791,201573,202356,203139,
									203923,204708,205493,206278,207064,207851,208638,209425,210213,211002,211791,
									212580,213370,214161,214952,215743,216535,217327,218120,218913,219706,220500,
									221294,222089,222884,223679,224475,225271,226067,226864,227661,228459,229256,
									230054,230853,231651,232450,233249,234049,234849,235649,236449,237249,238050,
									238851,239652,240454,241255,242057,242859,243661,244464,245266,246069,246871,
									247674,248478,249281,250084,250888,251691,252495,253298,254102,254906,255710,
									256514,257318,258122,258927,259731,260535,261339,262143,262948,263752,264556,
									265360,266165,266969,267773,268577,269381,270185,270989,271792,272596,273399,
									274203,275006,275809,276613,277416,278218,279021,279823,280626,281428,282230,
									283032,283833,284635,285436,286237,287038,287838,288638,289438,290238,291038,
									291837,292636,293434,294233,295031,295828,296626,297423,298220,299016,299812,
									300608,301403,302198,302993,303787,304581,305374,306167,306960,307752,308544,
									309335,310126,310917,311707,312496,313285,314074,314862,315649,316436,317223,
									318009,318794,319579,320364,321148,321931,322714,323496,324278,325059,325839,
									326619,327398,328177,328955,329732,330509,331285,332061,332835,333609,334383,
									335156,335928,336699,337470,338240,339009,339778,340545,341312,342079,342844,
									343609,344373,345136,345899,346661,347421,348182,348941,349699,350457,351214,
									351970,352725,353479,354233,354985,355737,356488,357238,357987,358735,359482,
									360228,360974,361718,362461,363204,363946,364686,365426,366165,366902,367639,
									368375,369110,369843,370576,371308,372038,372768,373497,374224,374951,375676,
									376401,377124,377846,378567,379287,380006,380724,381441,382156,382871,383584,
									384296,385007,385717,386426,387133,387840,388545,389249,389952,390653,391354,
									392053,392751,393447,394143,394837,395530,396222,396912,397601,398289,398976,
									399661,400345,401028,401710,402390,403068,403746,404422,405097,405770,406442,
									407113,407783,408451,409117,409783,410446,411109,411770,412430,413088,413745,
									414400,415054,415707,416358,417007,417655,418302,418947,419591,420233,420874,
									421514,422151,422788,423422,424056,424687,425318,425946,426573,427199,427823,
									428446,429066,429686,430304,430920,431534,432147,432759,433369,433977,434583,
									435188,435791,436393,436993,437592,438188,438783,439377,439969,440559,441147,
									441734,442319,442902,443484,444064,444642,445218,445793,446366,446937,447507,
									448075,448641,449205,449768,450328,450887,451445,452000,452554,453106,453656,
									454204,454751,455295,455838,456379,456918,457456,457991,458525,459057,459587,
									460115,460641,461165,461688,462208,462727,463244,463759,464272,464783,465292,
									465800,466305,466809,467310,467810,468308,468803,469297,469789,470279,470767,
									471253,471737,472219,472699,473177,473653,474127,474600,475070,475538,476004,
									476468,476930,477390,477848,478304,478758,479210,479660,480108,480554,480997,
									481439,481879,482316,482752,483185,483616,484046,484473,484898,485321,485742,
									486160,486577,486991,487404,487814,488222,488628,489032,489434,489834,490231,
									490626,491020,491411,491800,492186,492571,492953,493333,493711,494087,494461,
									494832,495202,495569,495934,496296,496657,497015,497371,497725,498077,498426,
									498773,499118,499461,499802,500140,500476,500810,501141,501471,501798,502122,
									502445,502765,503083,503399,503712,504024,504333,504639,504943,505246,505545,
									505843,506138,506431,506721,507010,507296,507579,507861,508140,508417,508691,
									508963,509233,509500,509766,510028,510289,510547,510803,511056,511307,511556,
									511802,512046,512288,512528,512765,512999,513231,513461,513689,513914,514137,
									514357,514575,514791,515004,515215,515424,515630,515834,516035,516234,516431,
									516625,516817,517006,517193,517378,517560,517740,517917,518092,518265,518435,
									518603,518768,518931,519092,519250,519406,519559,519710,519858,520004,520148,
									520289,520428,520564,520698,520829,520958,521085,521209,521330,521450,521566,
									521681,521793,521902,522009,522114,522216,522316,522413,522508,522600,522690,
									522777,522862,522945,523025,523102,523177,523250,523320,523388,523453,523516,
									523577,523635,523690,523743,523794,523842,523887,523931,523971,524009,524045,
									524079,524109,524138,524164,524187,524208,524227,524243,524256,524267,524276,
									524282,524286,524287,524286,524282,524276,524267,524256,524243,524227,524208,
									524187,524164,524138,524109,524079,524045,524009,523971,523931,523887,523842,
									523794,523743,523690,523635,523577,523516,523453,523388,523320,523250,523177,
									523102,523025,522945,522862,522777,522690,522600,522508,522413,522316,522216,
									522114,522009,521902,521793,521681,521566,521450,521330,521209,521085,520958,
									520829,520698,520564,520428,520289,520148,520004,519858,519710,519559,519406,
									519250,519092,518931,518768,518603,518435,518265,518092,517917,517740,517560,
									517378,517193,517006,516817,516625,516431,516234,516035,515834,515630,515424,
									515215,515004,514791,514575,514357,514137,513914,513689,513461,513231,512999,
									512765,512528,512288,512046,511802,511556,511307,511056,510803,510547,510289,
									510028,509766,509500,509233,508963,508691,508417,508140,507861,507579,507296,
									507010,506721,506431,506138,505843,505545,505246,504943,504639,504333,504024,
									503712,503399,503083,502765,502445,502122,501798,501471,501141,500810,500476,
									500140,499802,499461,499118,498773,498426,498077,497725,497371,497015,496657,
									496296,495934,495569,495202,494832,494461,494087,493711,493333,492953,492571,
									492186,491800,491411,491020,490626,490231,489834,489434,489032,488628,488222,
									487814,487404,486991,486577,486160,485742,485321,484898,484473,484046,483616,
									483185,482752,482316,481879,481439,480997,480554,480108,479660,479210,478758,
									478304,477848,477390,476930,476468,476004,475538,475070,474600,474127,473653,
									473177,472699,472219,471737,471253,470767,470279,469789,469297,468803,468308,
									467810,467310,466809,466305,465800,465292,464783,464272,463759,463244,462727,
									462208,461688,461165,460641,460115,459587,459057,458525,457991,457456,456918,
									456379,455838,455295,454751,454204,453656,453106,452554,452000,451445,450887,
									450328,449768,449205,448641,448075,447507,446937,446366,445793,445218,444642,
									444064,443484,442902,442319,441734,441147,440559,439969,439377,438783,438188,
									437592,436993,436393,435791,435188,434583,433977,433369,432759,432147,431534,
									430920,430304,429686,429066,428446,427823,427199,426573,425946,425318,424687,
									424056,423422,422788,422151,421514,420874,420233,419591,418947,418302,417655,
									417007,416358,415707,415054,414400,413745,413088,412430,411770,411109,410446,
									409783,409117,408451,407783,407113,406442,405770,405097,404422,403746,403068,
									402390,401710,401028,400345,399661,398976,398289,397601,396912,396222,395530,
									394837,394143,393447,392751,392053,391354,390653,389952,389249,388545,387840,
									387133,386426,385717,385007,384296,383584,382871,382156,381441,380724,380006,
									379287,378567,377846,377124,376401,375676,374951,374224,373497,372768,372038,
									371308,370576,369843,369110,368375,367639,366902,366165,365426,364686,363946,
									363204,362461,361718,360974,360228,359482,358735,357987,357238,356488,355737,
									354985,354233,353479,352725,351970,351214,350457,349699,348941,348182,347421,
									346661,345899,345136,344373,343609,342844,342079,341312,340545,339778,339009,
									338240,337470,336699,335928,335156,334383,333609,332835,332061,331285,330509,
									329732,328955,328177,327398,326619,325839,325059,324278,323496,322714,321931,
									321148,320364,319579,318794,318009,317223,316436,315649,314862,314074,313285,
									312496,311707,310917,310126,309335,308544,307752,306960,306167,305374,304581,
									303787,302993,302198,301403,300608,299812,299016,298220,297423,296626,295828,
									295031,294233,293434,292636,291837,291038,290238,289438,288638,287838,287038,
									286237,285436,284635,283833,283032,282230,281428,280626,279823,279021,278218,
									277416,276613,275809,275006,274203,273399,272596,271792,270989,270185,269381,
									268577,267773,266969,266165,265360,264556,263752,262948,262144,261339,260535,
									259731,258927,258122,257318,256514,255710,254906,254102,253298,252495,251691,
									250888,250084,249281,248478,247674,246871,246069,245266,244464,243661,242859,
									242057,241255,240454,239652,238851,238050,237249,236449,235649,234849,234049,
									233249,232450,231651,230853,230054,229256,228459,227661,226864,226067,225271,
									224475,223679,222884,222089,221294,220500,219706,218913,218120,217327,216535,
									215743,214952,214161,213370,212580,211791,211002,210213,209425,208638,207851,
									207064,206278,205493,204708,203923,203139,202356,201573,200791,200009,199228,
									198448,197668,196889,196110,195332,194555,193778,193002,192226,191452,190678,
									189904,189131,188359,187588,186817,186047,185278,184509,183742,182975,182208,
									181443,180678,179914,179151,178388,177626,176866,176105,175346,174588,173830,
									173073,172317,171562,170808,170054,169302,168550,167799,167049,166300,165552,
									164805,164059,163313,162569,161826,161083,160341,159601,158861,158122,157385,
									156648,155912,155177,154444,153711,152979,152249,151519,150790,150063,149336,
									148611,147886,147163,146441,145720,145000,144281,143563,142846,142131,141416,
									140703,139991,139280,138570,137861,137154,136447,135742,135038,134335,133634,
									132933,132234,131536,130840,130144,129450,128757,128065,127375,126686,125998,
									125311,124626,123942,123259,122577,121897,121219,120541,119865,119190,118517,
									117845,117174,116504,115836,115170,114504,113841,113178,112517,111857,111199,
									110542,109887,109233,108580,107929,107280,106632,105985,105340,104696,104054,
									103413,102773,102136,101499,100865,100231,99600,98969,98341,97714,97088,96464,
									95841,95221,94601,93983,93367,92753,92140,91528,90918,90310,89704,89099,88496,
									87894,87294,86695,86099,85504,84910,84318,83728,83140,82553,81968,81385,80803,
									80223,79645,79069,78494,77921,77350,76780,76212,75646,75082,74519,73959,73400,
									72842,72287,71733,71181,70631,70083,69536,68992,68449,67908,67369,66831,66296,
									65762,65230,64700,64172,63646,63122,62599,62079,61560,61043,60528,60015,59504,
									58995,58487,57982,57478,56977,56477,55979,55484,54990,54498,54008,53520,53034,
									52550,52068,51588,51110,50634,50160,49687,49217,48749,48283,47819,47357,46897,
									46439,45983,45529,45077,44627,44179,43733,43290,42848,42408,41971,41535,41102,
									40671,40241,39814,39389,38966,38545,38127,37710,37296,36883,36473,36065,35659,
									35255,34853,34453,34056,33661,33267,32876,32487,32101,31716,31334,30954,30576,
									30200,29826,29455,29085,28718,28353,27991,27630,27272,26916,26562,26210,25861,
									25514,25169,24826,24485,24147,23811,23477,23146,22816,22489,22165,21842,21522,
									21204,20888,20575,20263,19954,19648,19344,19041,18742,18444,18149,17856,17566,
									17277,16991,16708,16426,16147,15870,15596,15324,15054,14787,14521,14259,13998,
									13740,13484,13231,12980,12731,12485,12241,11999,11759,11522,11288,11056,10826,
									10598,10373,10150,9930,9712,9496,9283,9072,8863,8657,8453,8252,8053,7856,7662,
									7470,7281,7094,6909,6727,6547,6370,6195,6022,5852,5684,5519,5356,5195,5037,4881,
									4728,4577,4429,4283,4139,3998,3859,3723,3589,3458,3329,3202,3078,2957,2837,2721,
									2606,2494,2385,2278,2173,2071,1971,1874,1779,1687,1597,1510,1425,1342,1262,1185,
									1110,1037,967,899,834,771,710,652,597,544,493,445,400,356,316,278,242,208,178,
									149,123,100,79,60,44,31,20,11,5,1,0};*/

#else
/*const int16_t HanningTable1024[1024] ={	0,0,0,0,0,0,0,0,0,0,0,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,2,2,2,2,2,2,2,
										2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,4,4,4,
										4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,
										5,5,5,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,7,7,7,7,7,7,7,7,7,7,7,7,7,
										7,7,7,7,7,7,7,7,7,7,7,8,8,8,8,8,8,8,8,8,8,8,8,8,8,8,8,8,8,8,8,8,8,8,8,8,8,9,9,9,
										9,9,9,9,9,9,9,9,9,9,9,9,9,9,9,9,9,9,9,9,9,9,9,9,10,10,10,10,10,10,10,10,10,10,10,
										10,10,10,10,10,10,10,10,10,10,10,10,10,10,10,10,10,10,11,11,11,11,11,11,11,11,11,
										11,11,11,11,11,11,11,11,11,11,11,11,11,11,11,11,11,11,11,11,11,11,11,12,12,12,12,
										12,12,12,12,12,12,12,12,12,12,12,12,12,12,12,12,12,12,12,12,12,12,12,12,12,12,12,
										12,12,12,12,12,12,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,
										13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,14,14,14,14,14,
										14,14,14,14,14,14,14,14,14,14,14,14,14,14,14,14,14,14,14,14,14,14,14,14,14,14,14,
										14,14,14,14,14,14,14,14,14,14,14,14,14,14,14,14,14,14,14,14,14,14,14,14,14,14,14,
										14,14,14,14,15,15,15,15,15,15,15,15,15,15,15,15,15,15,15,15,15,15,15,15,15,15,15,
										15,15,15,15,15,15,15,15,15,15,15,15,15,15,15,15,15,15,15,15,15,15,15,15,15,15,15,
										15,15,15,15,15,15,15,15,15,15,15,15,15,15,15,15,15,15,15,15,15,15,15,15,15,15,15,
										15,15,15,15,15,15,15,15,15,15,15,15,15,15,15,15,15,15,15,15,15,15,15,15,15,15,15,
										15,15,15,15,15,15,15,15,15,15,15,15,15,15,15,15,15,15,15,15,15,15,15,15,15,15,15,
										15,15,15,15,15,15,15,15,15,15,15,15,15,15,15,15,15,15,15,15,15,15,15,15,15,15,15,
										15,15,15,15,15,15,15,15,15,15,15,14,14,14,14,14,14,14,14,14,14,14,14,14,14,14,14,
										14,14,14,14,14,14,14,14,14,14,14,14,14,14,14,14,14,14,14,14,14,14,14,14,14,14,14,
										14,14,14,14,14,14,14,14,14,14,14,14,14,14,14,14,14,14,14,14,13,13,13,13,13,13,13,
										13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,
										13,13,13,13,13,13,13,13,13,12,12,12,12,12,12,12,12,12,12,12,12,12,12,12,12,12,12,
										12,12,12,12,12,12,12,12,12,12,12,12,12,12,12,12,12,12,12,11,11,11,11,11,11,11,11,
										11,11,11,11,11,11,11,11,11,11,11,11,11,11,11,11,11,11,11,11,11,11,11,11,10,10,10,
										10,10,10,10,10,10,10,10,10,10,10,10,10,10,10,10,10,10,10,10,10,10,10,10,10,10,9,
										9,9,9,9,9,9,9,9,9,9,9,9,9,9,9,9,9,9,9,9,9,9,9,9,9,9,8,8,8,8,8,8,8,8,8,8,8,8,8,8,
										8,8,8,8,8,8,8,8,8,8,8,8,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,6,6,6,6,
										6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,
										5,5,5,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,3,3,3,3,3,3,3,3,3,3,3,3,3,3,
										3,3,3,3,3,3,3,3,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,1,1,1,1,1,1,1,1,1,1,
										1,1,1,1,1,1,1,1,1,1,1,1,0,0,0,0,0,0,0,0,0,0,0}*/

/*const int16_t HannTable1024[2048]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
								  0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
								  0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
								  0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
								  0,0,0,0,0,0,0,0,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,
								  1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,
								  1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,
								  1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,
								  2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,
								  2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,3,3,3,
								  3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,
								  3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,
								  3,3,3,3,3,3,3,3,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,
								  4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,
								  4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,
								  5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,
								  5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,6,6,6,6,
								  6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,
								  6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,
								  6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,
								  6,6,6,6,6,6,6,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,
								  7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,
								  7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,
								  7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,
								  7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,
								  7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,
								  7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,
								  7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,
								  7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,
								  6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,
								  6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,
								  6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,5,5,5,5,5,5,5,
								  5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,
								  5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,
								  5,5,5,5,5,5,5,5,5,5,5,5,5,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,
								  4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,
								  4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,
								  3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,
								  3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,2,2,2,2,2,2,2,2,
								  2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,
								  2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,
								  2,2,2,2,2,2,2,2,2,2,2,2,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,
								  1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,
								  1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,
								  1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
								  0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
								  0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
								  0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
								  0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};*/
#endif
#endif

#endif

extern uint32_t Freq_Value[8];
extern uint32_t Freq_index;

/********************************************************************//**
  * @brief  Calls the CMSIS FFT routine
 *********************************************************************/
#ifdef USE32BITFFT
void fft_q31(uint32_t * pSrc, uint32_t * pDest, uint32_t fftSize, uint32_t ifftFlag, uint32_t doBitReverse)
{
	arm_status status;
	arm_cfft_radix4_instance_q31 S;
	// float32_t maxValue;
	// int i;
	// extern volatile u32 DeBuf[];


	status = ARM_MATH_SUCCESS;

	// Initialize the CFFT/CIFFT module
	status = arm_cfft_radix4_init_q31(&S, fftSize, ifftFlag, doBitReverse);

	// Process the data through the CFFT/CIFFT module
	arm_cfft_radix4_q31(&S, (q31_t *)pSrc);

	// Process the data through the Complex Magnitude Module for
	// calculating the magnitude at each bin
	arm_cmplx_mag_q31((q31_t *)pSrc, (q31_t *)pDest, fftSize);

	// Calculates maxValue and returns corresponding BIN value
	//arm_max_f32(testOutput, fftSize, &maxValue, &testIndex);

	//if(testIndex !=  refIndex)
	//{
	//	status = ARM_MATH_TEST_FAILURE;
	//}
#if 0
	if( status != ARM_MATH_SUCCESS);
		__asm volatile ( "bkpt 0" );
#endif
}

#else
void fft_q15(uint16_t * pSrc, uint16_t * pDest, uint32_t fftSize, uint32_t ifftFlag, uint32_t doBitReverse)
{
	arm_status status;
	arm_cfft_radix4_instance_q15 S;
	float32_t maxValue;
	int i;
	extern volatile u32 DeBuf1[];
	extern volatile u32 DeBuf2[];


	status = ARM_MATH_SUCCESS;

	// Initialize the CFFT/CIFFT module
	status = arm_cfft_radix4_init_q15(&S, fftSize, ifftFlag, doBitReverse);

	// Process the data through the CFFT/CIFFT module
	arm_cfft_radix4_q15(&S, (q15_t *)pSrc);

	// Process the data through the Complex Magnitude Module for
	// calculating the magnitude at each bin
	arm_cmplx_mag_q15((q15_t *)pSrc, (q15_t *)pDest, fftSize);

	// Calculates maxValue and returns corresponding BIN value
	//arm_max_f32(testOutput, fftSize, &maxValue, &testIndex);

	//if(testIndex !=  refIndex)
	//{
	//	status = ARM_MATH_TEST_FAILURE;
	//}
#if 0
	if( status != ARM_MATH_SUCCESS);
		__asm volatile ( "bkpt 0" );
#endif
}
#endif



int32_t DetectFreq(u32* samples, u16 size){
	int i,maxi;
	u32 max=0;
	for(i=0;i<size/2;i++)
		if(samples[i]>max) {
			max = samples[i];
			maxi = i;
		}
	Maxpeakk = max;
	return maxi;//*Freq_Value[Freq_index]/size;
}
// Detect significant frequency peaks in spectrum
// Needs FFT samples from Hanning windowed time signal
#ifdef USE32BITFFT
uint16_t Spectrum_Peakdetect(u32* samples, u16 size, u32 treshold){
#else
uint16_t Spectrum_Peakdetect(u16* samples, u16 size, u16 treshold){
#endif

#ifdef USE32BITFFT
	extern uint32_t peakValList[];
	extern u32 fft_output1[];
	extern u32 fft_output2[];
#else
	extern uint16_t peakValList[];
	extern u16 fft_output1[];
	extern u16 fft_output2[];
#endif

	extern float peakIdcsList[];
	float exc;
	float currentPeakVal_dB, Z, Xma, e; // n;
	uint32_t cfar_idcs[] = {2,3,4,5};//,6,7,8,9,10,11,12,13};
	int32_t 	nrPeakIdcsList, idx0, idx1, idx3,
				 Ima, i,j,k,offs;
	uint32_t summe;

	// volatile uint16_t debug;

	int16_t idx2, currentPeakIdx;
	u32 treshold_lin;
	// Debug

	treshold_lin = treshold;
	nrPeakIdcsList = 0;
	currentPeakVal_dB = 0;
	currentPeakIdx = -1;

	//stop_idx = FFT_LENGTH/2;
	idx3 = 0;	// Index um Elemente an Peak-Liste anzuhängen

	// Reset Peaklist
	for(i=0;i<MAXPEAK;i++){
		peakIdcsList[i]=0;
		Peak_send[i][0]=0;
		Peak_send[i][1]=0;
	}

	for (idx0 = 0; idx0 < (FFT_LENGTH/2); idx0++){
	    summe = 0;
	    for (idx1=0;idx1 < (sizeof(cfar_idcs)/sizeof(cfar_idcs[0]));idx1++){
	        idx2 = idx0 + cfar_idcs[idx1];
	        if (idx2 >= FFT_LENGTH ) idx2 = idx2 - FFT_LENGTH;
	        summe = summe + samples[idx2];
	        idx2 = idx0 - cfar_idcs[idx1];
	        if (idx2 < 0) idx2 = idx2 + FFT_LENGTH;
	        summe = summe + samples[idx2];
	    }

	    temp_TresholdVek[idx0] = (summe + treshold_lin)/(2*(sizeof(cfar_idcs)/sizeof(cfar_idcs[0])));    // Element an Vektor hängen


	    if ((2* (sizeof(cfar_idcs)/sizeof(cfar_idcs[0])) * samples[idx0]) >= (summe + treshold_lin)){
	        if (currentPeakIdx == -1){
	            // isSegment = TRUE;
	            currentPeakVal_dB = samples[idx0];
	            currentPeakIdx = idx0;
	        }
	        else{
	            if (samples[idx0] > currentPeakVal_dB){
	                currentPeakVal_dB = samples[idx0];
	                currentPeakIdx = idx0;
	            }
	        }
	    }
	    else{
	    	if (currentPeakIdx >= 0){
	    	            // Found peak:
	    	            // Interpolation for hanning

	    	            //Xabs2 = abs(Y);
	    	            Ima = currentPeakIdx;
	    	            Xma = sqrt(samples[Ima]);
	    	            //arm_sqrt_f32(samples[Ima], &Xma);

	    	            if (Ima == 0){
	    	                // left border
	    	                if (samples[FFT_LENGTH-1] <= samples[1]){
	    	                    // positive epsilon
	    	                    Z = sqrt(samples[1]);
	    	                	//arm_sqrt_f32(samples[1], &Z);
	    	                    e = (2*Z-Xma) / (Z+Xma);
	    	                }
	    	                else{
	    	                    // negative epsilon
	    	                    Z = sqrt(samples[FFT_LENGTH-1]);
	    	                	//arm_sqrt_f32(samples[FFT_LENGTH-1], &Z);
	    	                    e = FFT_LENGTH - (2*Z-Xma) / (Z+Xma);
	    	                }
	    	            }
	    	            else if (Ima == (FFT_LENGTH-1)){
	    	                // right border
	    	                if (samples[FFT_LENGTH-2] < samples[0]){
	    	                    // positive epsilon
	    	                    Z = sqrt(samples[0]);
	    	                	//arm_sqrt_f32(samples[0], &Z);
	    	                    e = (2*Z-Xma) / (Z+Xma);
	    	                }
	    	                else{
	    	                    // negative epsilon
	    	                    Z = sqrt(samples[FFT_LENGTH-2]);
	    	                	//arm_sqrt_f32(samples[FFT_LENGTH-2], &Z);
	    	                    e = -(2*Z-Xma) / (Z+Xma);
	    	                }
	    	            }
	    	            else{
	    	                // inside
	    	                if (samples[Ima-1] < samples[Ima+1]){
	    	                    // positives epsilon
	    	                    Z = sqrt(samples[Ima+1]);
	    	                	//arm_sqrt_f32(samples[Ima+1], &Z);
	    	                    e = (2*Z-Xma) / (Z+Xma);
	    	                }
	    	                else{
	    	                    // always negative epsilon
	    	                    Z = sqrt(samples[Ima-1]);
	    	                	//arm_sqrt_f32(samples[Ima-1], &Z);
	    	                    e = -(2*Z-Xma) / (Z+Xma);
	    	                }
	    	            }

	    	            // n = (Ima + e);

	    	            peakIdcsList[idx3] = (Ima + e); //n;
	    	            peakValList[idx3] = currentPeakVal_dB; // Element anhängen
	    	            if (idx3 < MAXPEAK-1) idx3++;
	    	            nrPeakIdcsList = nrPeakIdcsList + 1;
	    	            currentPeakIdx = - 1;
	    	}
	    	}
	}
	for(i=0;i<idx3;i++){
		k=(int)(peakIdcsList[i]);
		offs=0;
		if(samples[k+1]>samples[k]) offs=1;
		if((k>0)&&(samples[k-1]>samples[k])) offs=-1;
		k=k+offs;
		Peak_send[i][0]=(int)(k);
		Peak_send[i][1]=(int)((float)samples[k]/Maxpeak_d*254);
	}
	for(i=0;i<idx3-1;i++)
		for(j=i+1;j<idx3;j++)
			if(Peak_send[i][1]<Peak_send[j][1]){
				k=Peak_send[i][0]; Peak_send[i][0]=Peak_send[j][0]; Peak_send[j][0]=k;
				k=Peak_send[i][1]; Peak_send[i][1]=Peak_send[j][1]; Peak_send[j][1]=k;
				exc=peakIdcsList[i]; peakIdcsList[i]=peakIdcsList[j]; peakIdcsList[j]=exc;
			}
	Freq_Send=peakIdcsList[0]/FFT_LENGTH*Freq_Value[Freq_index];
	if (idx3 == 0) return 0;
		else return (uint16_t)idx3;	// No of peaks
}


// Filter ADC input data with Hanning Window
#ifdef USE32BITFFT
void Hanning_Window(int16_t* dataIn, int32_t* dataOut, int samples){
#else
void Hanning_Window(int16_t* dataIn, int16_t* dataOut, int samples){
#endif
	uint16_t i;

	for(i=0;i<FFT_LENGTH;i++){
#ifdef USE32BITFFT
#if FFT_LENGTH == 256
		dataOut[i] = HannTable256[i] * (u32)dataIn[i];
#else
		dataOut[i << 1] = HanningTable1024[i] * (u32)dataIn[i << 1];
		dataOut[(i << 1) + 1] = 0;
#endif
#else
#if FFT_LENGTH == 256
		dataOut[i] = HannTable256[i] * dataIn[i];
#else
		dataOut[i] = HanningTable1024[i] * dataIn[i];
#endif
#endif
		}
}


// Filter ADC input data with Hanning Window
#ifdef USE32BITFFT
void Hanning_Window_neu(int32_t* dataIn, int32_t* dataOut, int samples){
#else
void Hanning_Window_neu(int16_t* dataIn, int16_t* dataOut, int samples){
#endif
	uint16_t i;

	for(i=0;i<FFT_LENGTH;i++){
#ifdef USE32BITFFT
#if FFT_LENGTH == 256
		dataOut[i] = HannTable256[i] * (u32)dataIn[i];
#else
		dataOut[i << 1] = HanningTable1024[i] * (u32)dataIn[i];
		dataOut[(i << 1) + 1] = 0;
#endif
#else
#if FFT_LENGTH == 256
		dataOut[i] = HannTable256[i] * dataIn[i];
#else
		dataOut[i] = HanningTable1024[i] * dataIn[i];
#endif
#endif
		}
}




// Calculate rotation signal frequency in Hz
uint32_t rotfreq(uint16_t harmonics){
	return (u32)(peakIdcsList[harmonics]*samplerate/FFT_LENGTH/2);
}

uint32_t rotfreqFundamental(void){
	uint16_t harmonics;

	harmonics = 0;
	while ((peakIdcsList[harmonics] >= 512 || peakIdcsList[harmonics] <= 2) && harmonics < no_of_peaks-1) harmonics++; // omit DC
	return (u32)(peakIdcsList[harmonics]*samplerate/FFT_LENGTH/2);
}

// Calculate rotation speed in RPM
uint32_t rotSpeed(uint16_t harmonics){
	return (u32)(peakIdcsList[harmonics]*samplerate/FFT_LENGTH/2)/bladecnt*60;
}

// Calculate rotation speed in RPM
uint32_t rotSpeedFundamental( void ){
	uint16_t harmonics;

	harmonics = 0;
	while ((peakIdcsList[harmonics] >= 512 || peakIdcsList[harmonics] <= 2) && harmonics < no_of_peaks-1) harmonics++; // omit DC
	return (u32)(peakIdcsList[harmonics]*samplerate/FFT_LENGTH/2)/bladecnt*60;
}


void Auto_Gain_AD(float Freq)
{
	int i;
	int16_t max, max_peak1, min_peak1, max_peak2, min_peak2, x, ADC_Span1, ADC_Span2,ADC_Span;
	static uint16_t gain_idx = PGAGAIN;
	uint16_t gain_idx_n=1;

	max_peak1 = max_peak2 = -9000;
    min_peak1 = min_peak2 = 9000;

	for(i=0;i<FFT_LENGTH;i++)
	{
			x = i << 1;
			if(ADCBuffer1[x] > max_peak1) max_peak1 = ADCBuffer1[x];
			if(ADCBuffer1[x] < min_peak1) min_peak1 = ADCBuffer1[x];
			if(ADCBuffer2[x] > max_peak2) max_peak2 = ADCBuffer2[x];
			if(ADCBuffer2[x] < min_peak2) min_peak2 = ADCBuffer2[x];
	}
	if((Freq>0)&&(Freq<2400)&&(autogain == 1)){

		max = (max_peak1 - min_peak1);
		if((max_peak2 - min_peak2)>max) max = max_peak2 - min_peak2;
		ADC_Span1 = (max_peak1 - min_peak1) / PGAgainIDX[gain_idx];
		ADC_Span2 = (max_peak2 - min_peak2) / PGAgainIDX[gain_idx];
		if(ADC_Span1>ADC_Span2) ADC_Span = ADC_Span1;
						   else ADC_Span = ADC_Span2;

		// adjust gain
		if(max<3500){
			if(ADC_Span < 70) gain_idx_n = 6;
			else if(ADC_Span < 175) gain_idx_n = 5;
			else if(ADC_Span < 350) gain_idx_n = 4;
			else if(ADC_Span < 700) gain_idx_n = 3;
			else if(ADC_Span < 1750) gain_idx_n = 2;
			else gain_idx_n = 1;
		}
		else gain_idx_n=1;
		if(gain_idx != gain_idx_n){
			gain_idx = gain_idx_n;
			ConfPGA(PGAConfig_Index[gain_idx]);
		}

	}

}


void remove_DC_Vib(int16_t *data_in, int total){
	int16_t i, x, DC_Wert;
	int32_t sum = 0;

	for(i=0;i<total;i++)
		sum += data_in[i];

	DC_Wert = sum / (total);   //Average

	for(i=0;i<total;i++)
		data_in[i] -= DC_Wert;

}

void remove_DC_Vib1(int32_t *data_in, int total){
	int16_t i, x, DC_Wert;
	int32_t sum = 0;

	for(i=0;i<total;i++)
		sum += data_in[i];

	DC_Wert = sum / total;   //Average

	for(i=0;i<total;i++)
		data_in[i] -= DC_Wert;

}