dsp_test - | Mbed

Elektronikprojekt Grupp 13 » Code » dsp_test

Elektronikprojekt Grupp 13 / Mbed 2 deprecated dsp_test

Dependencies: DHT mbed

dsp_test.cpp@4:a89e836d9faf, 2017-05-12 (annotated)

Committer:: elt14lpo
Date:: Fri May 12 08:01:26 2017 +0000
Revision:: 4:a89e836d9faf
Parent:: 3:3f71950ceb71

Who changed what in which revision?

User	Revision	Line number	New contents of line
elt14lpo	0:c81dc1cb885e	1	#include "mbed.h"
elt14lpo	0:c81dc1cb885e	2	#include <iostream> /* cout */
elt14lpo	4:a89e836d9faf	3	#include <stdio.h> /* printf */
elt14lpo	0:c81dc1cb885e	4	#include <math.h> /* sin */
elt14lpo	0:c81dc1cb885e	5	#include <vector>
elt14lpo	0:c81dc1cb885e	6	#include <stdlib.h> /* abs */
elt14lpo	0:c81dc1cb885e	7	#include <stdio.h>
elt14lpo	0:c81dc1cb885e	8	#include <AnalogIn.h>
elt14lpo	0:c81dc1cb885e	9	#include <stdint.h>
elt14lpo	0:c81dc1cb885e	10	#include <DHT.h>
elt14lpo	0:c81dc1cb885e	11	#include<sstream>
elt14lpo	3:3f71950ceb71	12	#include "luke_correlate_f32.h"
elt14lpo	0:c81dc1cb885e	13	//using namespace std;
elt14lpo	0:c81dc1cb885e	14
elt14lpo	0:c81dc1cb885e	15	/* DEBUG FUNCTION
elt14lpo	0:c81dc1cb885e	16	// ersätter Debug(xyz) med xyz , där xyz är din kod
elt14lpo	0:c81dc1cb885e	17	//För att aktivera:
elt14lpo	0:c81dc1cb885e	18	#define Debug(xyz) xyz
elt14lpo	0:c81dc1cb885e	19
elt14lpo	0:c81dc1cb885e	20	//För att "stänga av":
elt14lpo	0:c81dc1cb885e	21	#define Debug(xyz)
elt14lpo	0:c81dc1cb885e	22
elt14lpo	0:c81dc1cb885e	23	//I din kod, skriv din debug kod liknande så här:
elt14lpo	0:c81dc1cb885e	24	Debug( std::cout << "My text: " << myVariable << std::endl; );
elt14lpo	0:c81dc1cb885e	25
elt14lpo	0:c81dc1cb885e	26	*/
elt14lpo	0:c81dc1cb885e	27
elt14lpo	0:c81dc1cb885e	28	#define Debug(x) x
elt14lpo	0:c81dc1cb885e	29	#define DebugPrintState(y) y
elt14lpo	0:c81dc1cb885e	30	#define DebugArcSin(z) z
elt14lpo	0:c81dc1cb885e	31
elt14lpo	0:c81dc1cb885e	32
elt14lpo	0:c81dc1cb885e	33	//----------VARIABLES HERE
elt14lpo	3:3f71950ceb71	34	const int dataLength = 1000;
elt14lpo	3:3f71950ceb71	35	const int captureLength = 50;
elt14lpo	4:a89e836d9faf	36	int dL = 1000;
elt14lpo	4:a89e836d9faf	37	int cL = 50;
elt14lpo	0:c81dc1cb885e	38	double temp = 22;
elt14lpo	0:c81dc1cb885e	39	double hum = 10;
elt14lpo	0:c81dc1cb885e	40	double micDist = 0.250; //meters
elt14lpo	0:c81dc1cb885e	41	double threshold_1 = 0; //value when going to active mode channel 1 //old hardcoded value = 330
elt14lpo	0:c81dc1cb885e	42	double threshold_2 = 0; //value when going to active mode channel 2 //old hardcoded value = 200
elt14lpo	0:c81dc1cb885e	43	double threshold_adjust = 15; //used to adjust threshold, + for less sensitivity, - for increased sensitivity
elt14lpo	0:c81dc1cb885e	44	bool calibratedStatus = false; //flag to make sure Nuclueo only calibrated once for background noise
elt14lpo	0:c81dc1cb885e	45	bool checkTemp = false; //flag - true to checktemp, false to use predefined values
elt14lpo	0:c81dc1cb885e	46	int positionOfMaxVal_1;
elt14lpo	0:c81dc1cb885e	47	int positionOfMaxVal_2;
elt14lpo	0:c81dc1cb885e	48	const double PI = 3.14159265358979323846;
elt14lpo	0:c81dc1cb885e	49
elt14lpo	0:c81dc1cb885e	50	// State machine
elt14lpo	0:c81dc1cb885e	51	int STATE;
elt14lpo	0:c81dc1cb885e	52	//const int NONE = -1;
elt14lpo	0:c81dc1cb885e	53	const int IDLE = 0;
elt14lpo	0:c81dc1cb885e	54	const int CALIBRATE = 1;
elt14lpo	0:c81dc1cb885e	55	const int TESTNEW = 2;
elt14lpo	0:c81dc1cb885e	56	const int CALC = 3;
elt14lpo	0:c81dc1cb885e	57	const int CALC_ERROR = 4;
elt14lpo	0:c81dc1cb885e	58	const int SEND = 5;
elt14lpo	0:c81dc1cb885e	59	//const int WAIT = 9;
elt14lpo	0:c81dc1cb885e	60
elt14lpo	0:c81dc1cb885e	61	//dataLength behövs kanske inte, vector klassen kan växa med behov
elt14lpo	0:c81dc1cb885e	62	float channel_1 [dataLength];
elt14lpo	0:c81dc1cb885e	63	float channel_2 [dataLength];
elt14lpo	0:c81dc1cb885e	64	int timestamps_1 [dataLength];
elt14lpo	0:c81dc1cb885e	65	int timestamps_2 [dataLength];
elt14lpo	0:c81dc1cb885e	66	float capture_1 [captureLength];
elt14lpo	0:c81dc1cb885e	67	float *p1 = capture_1;
elt14lpo	0:c81dc1cb885e	68	float capture_2 [captureLength];
elt14lpo	0:c81dc1cb885e	69	float *p2 = capture_2;
elt14lpo	0:c81dc1cb885e	70	float capturestamps_1 [captureLength];
elt14lpo	0:c81dc1cb885e	71	float capturestamps_2 [captureLength];
elt14lpo	1:d0884279b41d	72	float dsp_res [dataLength];
elt14lpo	0:c81dc1cb885e	73	float *p_res = dsp_res;
elt14lpo	0:c81dc1cb885e	74
elt14lpo	0:c81dc1cb885e	75	int positiontest = 0;
elt14lpo	0:c81dc1cb885e	76	int test = 9;
elt14lpo	0:c81dc1cb885e	77	std::vector<double> delaytest(test);
elt14lpo	0:c81dc1cb885e	78
elt14lpo	0:c81dc1cb885e	79
elt14lpo	0:c81dc1cb885e	80	AnalogIn mic1(A0);
elt14lpo	0:c81dc1cb885e	81	AnalogIn mic2(A1);
elt14lpo	0:c81dc1cb885e	82	AnalogIn mic3(A2);
elt14lpo	0:c81dc1cb885e	83	DHT sensor(A3, DHT11);
elt14lpo	0:c81dc1cb885e	84
elt14lpo	0:c81dc1cb885e	85	//TIMER
elt14lpo	0:c81dc1cb885e	86	Timer t;
elt14lpo	0:c81dc1cb885e	87
elt14lpo	0:c81dc1cb885e	88	//led can be used for status
elt14lpo	0:c81dc1cb885e	89	DigitalOut led1(LED1);
elt14lpo	0:c81dc1cb885e	90
elt14lpo	0:c81dc1cb885e	91
elt14lpo	0:c81dc1cb885e	92	//----------FUNCTIONS HERE
elt14lpo	0:c81dc1cb885e	93	//Calculating distance between sound and camera
elt14lpo	0:c81dc1cb885e	94	double calcDist(double t, double v)
elt14lpo	0:c81dc1cb885e	95	{
elt14lpo	0:c81dc1cb885e	96	double s = t*v;
elt14lpo	0:c81dc1cb885e	97	return s;
elt14lpo	0:c81dc1cb885e	98	}
elt14lpo	0:c81dc1cb885e	99
elt14lpo	0:c81dc1cb885e	100	//Calculating angle in radians, D distance between mic1 and mic2
elt14lpo	0:c81dc1cb885e	101	double calcAng(double s, double D)
elt14lpo	0:c81dc1cb885e	102	{
elt14lpo	0:c81dc1cb885e	103	return asin(s/D) + PI/2;
elt14lpo	0:c81dc1cb885e	104	}
elt14lpo	0:c81dc1cb885e	105
elt14lpo	0:c81dc1cb885e	106	//Assuming the input value is temp as a number in degrees celcius and humidity as procent
elt14lpo	0:c81dc1cb885e	107	double calcSoundSpeed(double temp, double hum)
elt14lpo	0:c81dc1cb885e	108	{
elt14lpo	0:c81dc1cb885e	109	//Calculations were done in Matlab
elt14lpo	0:c81dc1cb885e	110	double speed = 331.1190 + 0.6016temp + 0.0126hum;
elt14lpo	0:c81dc1cb885e	111	return speed;
elt14lpo	0:c81dc1cb885e	112	}
elt14lpo	0:c81dc1cb885e	113
elt14lpo	0:c81dc1cb885e	114	//translate angle to number for camera
elt14lpo	0:c81dc1cb885e	115	string convertAngToCamNbr(double ang)
elt14lpo	0:c81dc1cb885e	116	{
elt14lpo	0:c81dc1cb885e	117	ang = ang*(180 / PI) + 45; //radianer till grader
elt14lpo	0:c81dc1cb885e	118	double angValues = 270;
elt14lpo	0:c81dc1cb885e	119	int stepValues = 50000;
elt14lpo	0:c81dc1cb885e	120	string tiltNumber = " 18000"; //hårdkodat Camera Pan värde
elt14lpo	0:c81dc1cb885e	121
elt14lpo	0:c81dc1cb885e	122	double oneAng = stepValues/angValues;
elt14lpo	0:c81dc1cb885e	123	double cameraAngNumber = ang*oneAng;
elt14lpo	0:c81dc1cb885e	124	int panInt = (int)(cameraAngNumber); //double to int
elt14lpo	0:c81dc1cb885e	125	//int to string
elt14lpo	0:c81dc1cb885e	126	string panNumber;
elt14lpo	0:c81dc1cb885e	127	ostringstream convert;
elt14lpo	0:c81dc1cb885e	128	convert << panInt;
elt14lpo	0:c81dc1cb885e	129	panNumber = convert.str();
elt14lpo	0:c81dc1cb885e	130
elt14lpo	0:c81dc1cb885e	131	string send = panNumber + tiltNumber;
elt14lpo	0:c81dc1cb885e	132	return send;
elt14lpo	0:c81dc1cb885e	133	}
elt14lpo	0:c81dc1cb885e	134
elt14lpo	0:c81dc1cb885e	135
elt14lpo	0:c81dc1cb885e	136	//calc time delay by finding peak values in 2 vectors
elt14lpo	0:c81dc1cb885e	137	//channel = 1 or 2
elt14lpo	0:c81dc1cb885e	138	int FindPeak(int channel)
elt14lpo	0:c81dc1cb885e	139	{
elt14lpo	0:c81dc1cb885e	140	float *channel_curr; //temporary vector with channel voltage values
elt14lpo	0:c81dc1cb885e	141
elt14lpo	0:c81dc1cb885e	142	//if channel 1 then set current channel to channel 1
elt14lpo	0:c81dc1cb885e	143	if (channel == 1) {
elt14lpo	0:c81dc1cb885e	144	channel_curr = capture_1;
elt14lpo	0:c81dc1cb885e	145	} else channel_curr = capture_2;
elt14lpo	0:c81dc1cb885e	146
elt14lpo	0:c81dc1cb885e	147	//reset max value & sum value
elt14lpo	0:c81dc1cb885e	148	double valueMax = 0;
elt14lpo	0:c81dc1cb885e	149
elt14lpo	0:c81dc1cb885e	150	//reset array position
elt14lpo	0:c81dc1cb885e	151	int positionOfMaxVal = 0;
elt14lpo	0:c81dc1cb885e	152
elt14lpo	0:c81dc1cb885e	153	//find largest value & mark that position in vectors
elt14lpo	0:c81dc1cb885e	154	for (int position = 0; position < captureLength; position++) {
elt14lpo	0:c81dc1cb885e	155	double val = abs(channel_curr[position]);
elt14lpo	0:c81dc1cb885e	156	if (val > valueMax ) {
elt14lpo	0:c81dc1cb885e	157	valueMax = val;
elt14lpo	0:c81dc1cb885e	158	positionOfMaxVal = position;
elt14lpo	0:c81dc1cb885e	159	}
elt14lpo	0:c81dc1cb885e	160	}
elt14lpo	0:c81dc1cb885e	161	return positionOfMaxVal;
elt14lpo	0:c81dc1cb885e	162	}
elt14lpo	0:c81dc1cb885e	163
elt14lpo	0:c81dc1cb885e	164	double FindTimeDelay(int positionOfMaxVal_1, int positionOfMaxVal_2)
elt14lpo	0:c81dc1cb885e	165	{
elt14lpo	0:c81dc1cb885e	166	double timemax_1 = capturestamps_1[positionOfMaxVal_1];
elt14lpo	0:c81dc1cb885e	167	double timemax_2 = capturestamps_2[positionOfMaxVal_2];
elt14lpo	0:c81dc1cb885e	168	double delay = timemax_1 - timemax_2;
elt14lpo	0:c81dc1cb885e	169	return delay; //if negative near microphone 1, if positive near micropnone 2
elt14lpo	0:c81dc1cb885e	170	}
elt14lpo	0:c81dc1cb885e	171
elt14lpo	0:c81dc1cb885e	172
elt14lpo	0:c81dc1cb885e	173	//get voltage value which represents audio amplitude from microphone
elt14lpo	0:c81dc1cb885e	174	double getAudioValue(AnalogIn micX)
elt14lpo	0:c81dc1cb885e	175	{
elt14lpo	0:c81dc1cb885e	176	return 1000*micX.read();
elt14lpo	0:c81dc1cb885e	177	}
elt14lpo	0:c81dc1cb885e	178
elt14lpo	0:c81dc1cb885e	179
elt14lpo	0:c81dc1cb885e	180	bool overThreshold(double micValue_1, double micValue_2)
elt14lpo	0:c81dc1cb885e	181	{
elt14lpo	0:c81dc1cb885e	182	if ((micValue_1 > threshold_1) \|\| (micValue_2 > threshold_2)) {
elt14lpo	0:c81dc1cb885e	183	return true;
elt14lpo	0:c81dc1cb885e	184	} else return false;
elt14lpo	0:c81dc1cb885e	185	}
elt14lpo	0:c81dc1cb885e	186
elt14lpo	0:c81dc1cb885e	187	//true if voltage value in microphone is above the current threshold value
elt14lpo	0:c81dc1cb885e	188	bool calibrateThreshold(double micValue, double currentThreshold)
elt14lpo	0:c81dc1cb885e	189	{
elt14lpo	0:c81dc1cb885e	190	if ( micValue > currentThreshold ) {
elt14lpo	0:c81dc1cb885e	191	return true;
elt14lpo	0:c81dc1cb885e	192	} else return false;
elt14lpo	0:c81dc1cb885e	193	}
elt14lpo	0:c81dc1cb885e	194
elt14lpo	4:a89e836d9faf	195	void luke_correlate_f32(
elt14lpo	4:a89e836d9faf	196	float* pSrcA,
elt14lpo	4:a89e836d9faf	197	int srcALen,
elt14lpo	4:a89e836d9faf	198	float* pSrcB,
elt14lpo	4:a89e836d9faf	199	int srcBLen,
elt14lpo	4:a89e836d9faf	200	float* pDst)
elt14lpo	4:a89e836d9faf	201	{
elt14lpo	4:a89e836d9faf	202
elt14lpo	4:a89e836d9faf	203
elt14lpo	4:a89e836d9faf	204	#ifndef ARM_MATH_CM0_FAMILY
elt14lpo	4:a89e836d9faf	205
elt14lpo	4:a89e836d9faf	206	/* Run the below code for Cortex-M4 and Cortex-M3 */
elt14lpo	4:a89e836d9faf	207
elt14lpo	4:a89e836d9faf	208	float pIn1; / inputA pointer */
elt14lpo	4:a89e836d9faf	209	float pIn2; / inputB pointer */
elt14lpo	4:a89e836d9faf	210	float pOut = pDst; / output pointer */
elt14lpo	4:a89e836d9faf	211	float px; / Intermediate inputA pointer */
elt14lpo	4:a89e836d9faf	212	float py; / Intermediate inputB pointer */
elt14lpo	4:a89e836d9faf	213	float pSrc1; / Intermediate pointers */
elt14lpo	4:a89e836d9faf	214	float sum, acc0, acc1, acc2, acc3; /* Accumulators */
elt14lpo	4:a89e836d9faf	215	float x0, x1, x2, x3, c0; /* temporary variables for holding input and coefficient values */
elt14lpo	4:a89e836d9faf	216	int j, k = 0u, count, blkCnt, outBlockSize, blockSize1, blockSize2, blockSize3; /* loop counters */
elt14lpo	4:a89e836d9faf	217	int32_t inc = 1; /* Destination address modifier */
elt14lpo	4:a89e836d9faf	218
elt14lpo	4:a89e836d9faf	219
elt14lpo	4:a89e836d9faf	220	/* The algorithm implementation is based on the lengths of the inputs. */
elt14lpo	4:a89e836d9faf	221	/* srcB is always made to slide across srcA. */
elt14lpo	4:a89e836d9faf	222	/* So srcBLen is always considered as shorter or equal to srcALen */
elt14lpo	4:a89e836d9faf	223	/* But CORR(x, y) is reverse of CORR(y, x) */
elt14lpo	4:a89e836d9faf	224	/* So, when srcBLen > srcALen, output pointer is made to point to the end of the output buffer */
elt14lpo	4:a89e836d9faf	225	/* and the destination pointer modifier, inc is set to -1 */
elt14lpo	4:a89e836d9faf	226	/* If srcALen > srcBLen, zero pad has to be done to srcB to make the two inputs of same length */
elt14lpo	4:a89e836d9faf	227	/* But to improve the performance,
elt14lpo	4:a89e836d9faf	228	* we assume zeroes in the output instead of zero padding either of the the inputs*/
elt14lpo	4:a89e836d9faf	229	/* If srcALen > srcBLen,
elt14lpo	4:a89e836d9faf	230	* (srcALen - srcBLen) zeroes has to included in the starting of the output buffer */
elt14lpo	4:a89e836d9faf	231	/* If srcALen < srcBLen,
elt14lpo	4:a89e836d9faf	232	* (srcALen - srcBLen) zeroes has to included in the ending of the output buffer */
elt14lpo	4:a89e836d9faf	233	if(srcALen >= srcBLen)
elt14lpo	4:a89e836d9faf	234	{
elt14lpo	4:a89e836d9faf	235	/* Initialization of inputA pointer */
elt14lpo	4:a89e836d9faf	236	pIn1 = pSrcA;
elt14lpo	4:a89e836d9faf	237
elt14lpo	4:a89e836d9faf	238	/* Initialization of inputB pointer */
elt14lpo	4:a89e836d9faf	239	pIn2 = pSrcB;
elt14lpo	4:a89e836d9faf	240
elt14lpo	4:a89e836d9faf	241	/* Number of output samples is calculated */
elt14lpo	4:a89e836d9faf	242	outBlockSize = (2u * srcALen) - 1u;
elt14lpo	4:a89e836d9faf	243
elt14lpo	4:a89e836d9faf	244	/* When srcALen > srcBLen, zero padding has to be done to srcB
elt14lpo	4:a89e836d9faf	245	* to make their lengths equal.
elt14lpo	4:a89e836d9faf	246	* Instead, (outBlockSize - (srcALen + srcBLen - 1))
elt14lpo	4:a89e836d9faf	247	* number of output samples are made zero */
elt14lpo	4:a89e836d9faf	248	j = outBlockSize - (srcALen + (srcBLen - 1u));
elt14lpo	4:a89e836d9faf	249
elt14lpo	4:a89e836d9faf	250	/* Updating the pointer position to non zero value */
elt14lpo	4:a89e836d9faf	251	pOut += j;
elt14lpo	4:a89e836d9faf	252
elt14lpo	4:a89e836d9faf	253	//while(j > 0u)
elt14lpo	4:a89e836d9faf	254	//{
elt14lpo	4:a89e836d9faf	255	// /* Zero is stored in the destination buffer */
elt14lpo	4:a89e836d9faf	256	// *pOut++ = 0.0f;
elt14lpo	4:a89e836d9faf	257
elt14lpo	4:a89e836d9faf	258	// /* Decrement the loop counter */
elt14lpo	4:a89e836d9faf	259	// j--;
elt14lpo	4:a89e836d9faf	260	//}
elt14lpo	4:a89e836d9faf	261
elt14lpo	4:a89e836d9faf	262	}
elt14lpo	4:a89e836d9faf	263	else
elt14lpo	4:a89e836d9faf	264	{
elt14lpo	4:a89e836d9faf	265	/* Initialization of inputA pointer */
elt14lpo	4:a89e836d9faf	266	pIn1 = pSrcB;
elt14lpo	4:a89e836d9faf	267
elt14lpo	4:a89e836d9faf	268	/* Initialization of inputB pointer */
elt14lpo	4:a89e836d9faf	269	pIn2 = pSrcA;
elt14lpo	4:a89e836d9faf	270
elt14lpo	4:a89e836d9faf	271	/* srcBLen is always considered as shorter or equal to srcALen */
elt14lpo	4:a89e836d9faf	272	j = srcBLen;
elt14lpo	4:a89e836d9faf	273	srcBLen = srcALen;
elt14lpo	4:a89e836d9faf	274	srcALen = j;
elt14lpo	4:a89e836d9faf	275
elt14lpo	4:a89e836d9faf	276	/* CORR(x, y) = Reverse order(CORR(y, x)) */
elt14lpo	4:a89e836d9faf	277	/* Hence set the destination pointer to point to the last output sample */
elt14lpo	4:a89e836d9faf	278	pOut = pDst + ((srcALen + srcBLen) - 2u);
elt14lpo	4:a89e836d9faf	279
elt14lpo	4:a89e836d9faf	280	/* Destination address modifier is set to -1 */
elt14lpo	4:a89e836d9faf	281	inc = -1;
elt14lpo	4:a89e836d9faf	282
elt14lpo	4:a89e836d9faf	283	}
elt14lpo	4:a89e836d9faf	284
elt14lpo	4:a89e836d9faf	285	/* The function is internally
elt14lpo	4:a89e836d9faf	286	* divided into three parts according to the number of multiplications that has to be
elt14lpo	4:a89e836d9faf	287	* taken place between inputA samples and inputB samples. In the first part of the
elt14lpo	4:a89e836d9faf	288	* algorithm, the multiplications increase by one for every iteration.
elt14lpo	4:a89e836d9faf	289	* In the second part of the algorithm, srcBLen number of multiplications are done.
elt14lpo	4:a89e836d9faf	290	* In the third part of the algorithm, the multiplications decrease by one
elt14lpo	4:a89e836d9faf	291	* for every iteration.*/
elt14lpo	4:a89e836d9faf	292	/* The algorithm is implemented in three stages.
elt14lpo	4:a89e836d9faf	293	* The loop counters of each stage is initiated here. */
elt14lpo	4:a89e836d9faf	294	blockSize1 = srcBLen - 1u;
elt14lpo	4:a89e836d9faf	295	blockSize2 = srcALen - (srcBLen - 1u);
elt14lpo	4:a89e836d9faf	296	blockSize3 = blockSize1;
elt14lpo	4:a89e836d9faf	297
elt14lpo	4:a89e836d9faf	298	/* --------------------------
elt14lpo	4:a89e836d9faf	299	* Initializations of stage1
elt14lpo	4:a89e836d9faf	300	* -------------------------*/
elt14lpo	4:a89e836d9faf	301
elt14lpo	4:a89e836d9faf	302	/* sum = x[0] * y[srcBlen - 1]
elt14lpo	4:a89e836d9faf	303	* sum = x[0] * y[srcBlen-2] + x[1] * y[srcBlen - 1]
elt14lpo	4:a89e836d9faf	304	* ....
elt14lpo	4:a89e836d9faf	305	* sum = x[0] * y[0] + x[1] * y[1] +...+ x[srcBLen - 1] * y[srcBLen - 1]
elt14lpo	4:a89e836d9faf	306	*/
elt14lpo	4:a89e836d9faf	307
elt14lpo	4:a89e836d9faf	308	/* In this stage the MAC operations are increased by 1 for every iteration.
elt14lpo	4:a89e836d9faf	309	The count variable holds the number of MAC operations performed */
elt14lpo	4:a89e836d9faf	310	count = 1u;
elt14lpo	4:a89e836d9faf	311
elt14lpo	4:a89e836d9faf	312	/* Working pointer of inputA */
elt14lpo	4:a89e836d9faf	313	px = pIn1;
elt14lpo	4:a89e836d9faf	314
elt14lpo	4:a89e836d9faf	315	/* Working pointer of inputB */
elt14lpo	4:a89e836d9faf	316	pSrc1 = pIn2 + (srcBLen - 1u);
elt14lpo	4:a89e836d9faf	317	py = pSrc1;
elt14lpo	4:a89e836d9faf	318
elt14lpo	4:a89e836d9faf	319	/* ------------------------
elt14lpo	4:a89e836d9faf	320	* Stage1 process
elt14lpo	4:a89e836d9faf	321	* ----------------------*/
elt14lpo	4:a89e836d9faf	322
elt14lpo	4:a89e836d9faf	323	/* The first stage starts here */
elt14lpo	4:a89e836d9faf	324	while(blockSize1 > 0u)
elt14lpo	4:a89e836d9faf	325	{
elt14lpo	4:a89e836d9faf	326	/* Accumulator is made zero for every iteration */
elt14lpo	4:a89e836d9faf	327	sum = 0.0f;
elt14lpo	4:a89e836d9faf	328
elt14lpo	4:a89e836d9faf	329	/* Apply loop unrolling and compute 4 MACs simultaneously. */
elt14lpo	4:a89e836d9faf	330	k = count >> 2u;
elt14lpo	4:a89e836d9faf	331
elt14lpo	4:a89e836d9faf	332	/* First part of the processing with loop unrolling. Compute 4 MACs at a time.
elt14lpo	4:a89e836d9faf	333	** a second loop below computes MACs for the remaining 1 to 3 samples. */
elt14lpo	4:a89e836d9faf	334	while(k > 0u)
elt14lpo	4:a89e836d9faf	335	{
elt14lpo	4:a89e836d9faf	336	/* x[0] * y[srcBLen - 4] */
elt14lpo	4:a89e836d9faf	337	sum += px++ *py++;
elt14lpo	4:a89e836d9faf	338	/* x[1] * y[srcBLen - 3] */
elt14lpo	4:a89e836d9faf	339	sum += px++ *py++;
elt14lpo	4:a89e836d9faf	340	/* x[2] * y[srcBLen - 2] */
elt14lpo	4:a89e836d9faf	341	sum += px++ *py++;
elt14lpo	4:a89e836d9faf	342	/* x[3] * y[srcBLen - 1] */
elt14lpo	4:a89e836d9faf	343	sum += px++ *py++;
elt14lpo	4:a89e836d9faf	344
elt14lpo	4:a89e836d9faf	345	/* Decrement the loop counter */
elt14lpo	4:a89e836d9faf	346	k--;
elt14lpo	4:a89e836d9faf	347	}
elt14lpo	4:a89e836d9faf	348
elt14lpo	4:a89e836d9faf	349	/* If the count is not a multiple of 4, compute any remaining MACs here.
elt14lpo	4:a89e836d9faf	350	** No loop unrolling is used. */
elt14lpo	4:a89e836d9faf	351	k = count % 0x4u;
elt14lpo	4:a89e836d9faf	352
elt14lpo	4:a89e836d9faf	353	while(k > 0u)
elt14lpo	4:a89e836d9faf	354	{
elt14lpo	4:a89e836d9faf	355	/* Perform the multiply-accumulate */
elt14lpo	4:a89e836d9faf	356	/* x[0] * y[srcBLen - 1] */
elt14lpo	4:a89e836d9faf	357	sum += px++ *py++;
elt14lpo	4:a89e836d9faf	358
elt14lpo	4:a89e836d9faf	359	/* Decrement the loop counter */
elt14lpo	4:a89e836d9faf	360	k--;
elt14lpo	4:a89e836d9faf	361	}
elt14lpo	4:a89e836d9faf	362
elt14lpo	4:a89e836d9faf	363	/* Store the result in the accumulator in the destination buffer. */
elt14lpo	4:a89e836d9faf	364	*pOut = sum;
elt14lpo	4:a89e836d9faf	365	/* Destination pointer is updated according to the address modifier, inc */
elt14lpo	4:a89e836d9faf	366	pOut += inc;
elt14lpo	4:a89e836d9faf	367
elt14lpo	4:a89e836d9faf	368	/* Update the inputA and inputB pointers for next MAC calculation */
elt14lpo	4:a89e836d9faf	369	py = pSrc1 - count;
elt14lpo	4:a89e836d9faf	370	px = pIn1;
elt14lpo	4:a89e836d9faf	371
elt14lpo	4:a89e836d9faf	372	/* Increment the MAC count */
elt14lpo	4:a89e836d9faf	373	count++;
elt14lpo	4:a89e836d9faf	374
elt14lpo	4:a89e836d9faf	375	/* Decrement the loop counter */
elt14lpo	4:a89e836d9faf	376	blockSize1--;
elt14lpo	4:a89e836d9faf	377	}
elt14lpo	4:a89e836d9faf	378
elt14lpo	4:a89e836d9faf	379	/* --------------------------
elt14lpo	4:a89e836d9faf	380	* Initializations of stage2
elt14lpo	4:a89e836d9faf	381	* ------------------------*/
elt14lpo	4:a89e836d9faf	382
elt14lpo	4:a89e836d9faf	383	/* sum = x[0] * y[0] + x[1] * y[1] +...+ x[srcBLen-1] * y[srcBLen-1]
elt14lpo	4:a89e836d9faf	384	* sum = x[1] * y[0] + x[2] * y[1] +...+ x[srcBLen] * y[srcBLen-1]
elt14lpo	4:a89e836d9faf	385	* ....
elt14lpo	4:a89e836d9faf	386	* sum = x[srcALen-srcBLen-2] * y[0] + x[srcALen-srcBLen-1] * y[1] +...+ x[srcALen-1] * y[srcBLen-1]
elt14lpo	4:a89e836d9faf	387	*/
elt14lpo	4:a89e836d9faf	388
elt14lpo	4:a89e836d9faf	389	/* Working pointer of inputA */
elt14lpo	4:a89e836d9faf	390	px = pIn1;
elt14lpo	4:a89e836d9faf	391
elt14lpo	4:a89e836d9faf	392	/* Working pointer of inputB */
elt14lpo	4:a89e836d9faf	393	py = pIn2;
elt14lpo	4:a89e836d9faf	394
elt14lpo	4:a89e836d9faf	395	/* count is index by which the pointer pIn1 to be incremented */
elt14lpo	4:a89e836d9faf	396	count = 0u;
elt14lpo	4:a89e836d9faf	397
elt14lpo	4:a89e836d9faf	398	/* -------------------
elt14lpo	4:a89e836d9faf	399	* Stage2 process
elt14lpo	4:a89e836d9faf	400	* ------------------*/
elt14lpo	4:a89e836d9faf	401
elt14lpo	4:a89e836d9faf	402	/* Stage2 depends on srcBLen as in this stage srcBLen number of MACS are performed.
elt14lpo	4:a89e836d9faf	403	* So, to loop unroll over blockSize2,
elt14lpo	4:a89e836d9faf	404	* srcBLen should be greater than or equal to 4, to loop unroll the srcBLen loop */
elt14lpo	4:a89e836d9faf	405	if(srcBLen >= 4u)
elt14lpo	4:a89e836d9faf	406	{
elt14lpo	4:a89e836d9faf	407	/* Loop unroll over blockSize2, by 4 */
elt14lpo	4:a89e836d9faf	408	blkCnt = blockSize2 >> 2u;
elt14lpo	4:a89e836d9faf	409
elt14lpo	4:a89e836d9faf	410	while(blkCnt > 0u)
elt14lpo	4:a89e836d9faf	411	{
elt14lpo	4:a89e836d9faf	412	/* Set all accumulators to zero */
elt14lpo	4:a89e836d9faf	413	acc0 = 0.0f;
elt14lpo	4:a89e836d9faf	414	acc1 = 0.0f;
elt14lpo	4:a89e836d9faf	415	acc2 = 0.0f;
elt14lpo	4:a89e836d9faf	416	acc3 = 0.0f;
elt14lpo	4:a89e836d9faf	417
elt14lpo	4:a89e836d9faf	418	/* read x[0], x[1], x[2] samples */
elt14lpo	4:a89e836d9faf	419	x0 = *(px++);
elt14lpo	4:a89e836d9faf	420	x1 = *(px++);
elt14lpo	4:a89e836d9faf	421	x2 = *(px++);
elt14lpo	4:a89e836d9faf	422
elt14lpo	4:a89e836d9faf	423	/* Apply loop unrolling and compute 4 MACs simultaneously. */
elt14lpo	4:a89e836d9faf	424	k = srcBLen >> 2u;
elt14lpo	4:a89e836d9faf	425
elt14lpo	4:a89e836d9faf	426	/* First part of the processing with loop unrolling. Compute 4 MACs at a time.
elt14lpo	4:a89e836d9faf	427	** a second loop below computes MACs for the remaining 1 to 3 samples. */
elt14lpo	4:a89e836d9faf	428	do
elt14lpo	4:a89e836d9faf	429	{
elt14lpo	4:a89e836d9faf	430	/* Read y[0] sample */
elt14lpo	4:a89e836d9faf	431	c0 = *(py++);
elt14lpo	4:a89e836d9faf	432
elt14lpo	4:a89e836d9faf	433	/* Read x[3] sample */
elt14lpo	4:a89e836d9faf	434	x3 = *(px++);
elt14lpo	4:a89e836d9faf	435
elt14lpo	4:a89e836d9faf	436	/* Perform the multiply-accumulate */
elt14lpo	4:a89e836d9faf	437	/* acc0 += x[0] * y[0] */
elt14lpo	4:a89e836d9faf	438	acc0 += x0 * c0;
elt14lpo	4:a89e836d9faf	439	/* acc1 += x[1] * y[0] */
elt14lpo	4:a89e836d9faf	440	acc1 += x1 * c0;
elt14lpo	4:a89e836d9faf	441	/* acc2 += x[2] * y[0] */
elt14lpo	4:a89e836d9faf	442	acc2 += x2 * c0;
elt14lpo	4:a89e836d9faf	443	/* acc3 += x[3] * y[0] */
elt14lpo	4:a89e836d9faf	444	acc3 += x3 * c0;
elt14lpo	4:a89e836d9faf	445
elt14lpo	4:a89e836d9faf	446	/* Read y[1] sample */
elt14lpo	4:a89e836d9faf	447	c0 = *(py++);
elt14lpo	4:a89e836d9faf	448
elt14lpo	4:a89e836d9faf	449	/* Read x[4] sample */
elt14lpo	4:a89e836d9faf	450	x0 = *(px++);
elt14lpo	4:a89e836d9faf	451
elt14lpo	4:a89e836d9faf	452	/* Perform the multiply-accumulate */
elt14lpo	4:a89e836d9faf	453	/* acc0 += x[1] * y[1] */
elt14lpo	4:a89e836d9faf	454	acc0 += x1 * c0;
elt14lpo	4:a89e836d9faf	455	/* acc1 += x[2] * y[1] */
elt14lpo	4:a89e836d9faf	456	acc1 += x2 * c0;
elt14lpo	4:a89e836d9faf	457	/* acc2 += x[3] * y[1] */
elt14lpo	4:a89e836d9faf	458	acc2 += x3 * c0;
elt14lpo	4:a89e836d9faf	459	/* acc3 += x[4] * y[1] */
elt14lpo	4:a89e836d9faf	460	acc3 += x0 * c0;
elt14lpo	4:a89e836d9faf	461
elt14lpo	4:a89e836d9faf	462	/* Read y[2] sample */
elt14lpo	4:a89e836d9faf	463	c0 = *(py++);
elt14lpo	4:a89e836d9faf	464
elt14lpo	4:a89e836d9faf	465	/* Read x[5] sample */
elt14lpo	4:a89e836d9faf	466	x1 = *(px++);
elt14lpo	4:a89e836d9faf	467
elt14lpo	4:a89e836d9faf	468	/* Perform the multiply-accumulates */
elt14lpo	4:a89e836d9faf	469	/* acc0 += x[2] * y[2] */
elt14lpo	4:a89e836d9faf	470	acc0 += x2 * c0;
elt14lpo	4:a89e836d9faf	471	/* acc1 += x[3] * y[2] */
elt14lpo	4:a89e836d9faf	472	acc1 += x3 * c0;
elt14lpo	4:a89e836d9faf	473	/* acc2 += x[4] * y[2] */
elt14lpo	4:a89e836d9faf	474	acc2 += x0 * c0;
elt14lpo	4:a89e836d9faf	475	/* acc3 += x[5] * y[2] */
elt14lpo	4:a89e836d9faf	476	acc3 += x1 * c0;
elt14lpo	4:a89e836d9faf	477
elt14lpo	4:a89e836d9faf	478	/* Read y[3] sample */
elt14lpo	4:a89e836d9faf	479	c0 = *(py++);
elt14lpo	4:a89e836d9faf	480
elt14lpo	4:a89e836d9faf	481	/* Read x[6] sample */
elt14lpo	4:a89e836d9faf	482	x2 = *(px++);
elt14lpo	4:a89e836d9faf	483
elt14lpo	4:a89e836d9faf	484	/* Perform the multiply-accumulates */
elt14lpo	4:a89e836d9faf	485	/* acc0 += x[3] * y[3] */
elt14lpo	4:a89e836d9faf	486	acc0 += x3 * c0;
elt14lpo	4:a89e836d9faf	487	/* acc1 += x[4] * y[3] */
elt14lpo	4:a89e836d9faf	488	acc1 += x0 * c0;
elt14lpo	4:a89e836d9faf	489	/* acc2 += x[5] * y[3] */
elt14lpo	4:a89e836d9faf	490	acc2 += x1 * c0;
elt14lpo	4:a89e836d9faf	491	/* acc3 += x[6] * y[3] */
elt14lpo	4:a89e836d9faf	492	acc3 += x2 * c0;
elt14lpo	4:a89e836d9faf	493
elt14lpo	4:a89e836d9faf	494
elt14lpo	4:a89e836d9faf	495	} while(--k);
elt14lpo	4:a89e836d9faf	496
elt14lpo	4:a89e836d9faf	497	/* If the srcBLen is not a multiple of 4, compute any remaining MACs here.
elt14lpo	4:a89e836d9faf	498	** No loop unrolling is used. */
elt14lpo	4:a89e836d9faf	499	k = srcBLen % 0x4u;
elt14lpo	4:a89e836d9faf	500
elt14lpo	4:a89e836d9faf	501	while(k > 0u)
elt14lpo	4:a89e836d9faf	502	{
elt14lpo	4:a89e836d9faf	503	/* Read y[4] sample */
elt14lpo	4:a89e836d9faf	504	c0 = *(py++);
elt14lpo	4:a89e836d9faf	505
elt14lpo	4:a89e836d9faf	506	/* Read x[7] sample */
elt14lpo	4:a89e836d9faf	507	x3 = *(px++);
elt14lpo	4:a89e836d9faf	508
elt14lpo	4:a89e836d9faf	509	/* Perform the multiply-accumulates */
elt14lpo	4:a89e836d9faf	510	/* acc0 += x[4] * y[4] */
elt14lpo	4:a89e836d9faf	511	acc0 += x0 * c0;
elt14lpo	4:a89e836d9faf	512	/* acc1 += x[5] * y[4] */
elt14lpo	4:a89e836d9faf	513	acc1 += x1 * c0;
elt14lpo	4:a89e836d9faf	514	/* acc2 += x[6] * y[4] */
elt14lpo	4:a89e836d9faf	515	acc2 += x2 * c0;
elt14lpo	4:a89e836d9faf	516	/* acc3 += x[7] * y[4] */
elt14lpo	4:a89e836d9faf	517	acc3 += x3 * c0;
elt14lpo	4:a89e836d9faf	518
elt14lpo	4:a89e836d9faf	519	/* Reuse the present samples for the next MAC */
elt14lpo	4:a89e836d9faf	520	x0 = x1;
elt14lpo	4:a89e836d9faf	521	x1 = x2;
elt14lpo	4:a89e836d9faf	522	x2 = x3;
elt14lpo	4:a89e836d9faf	523
elt14lpo	4:a89e836d9faf	524	/* Decrement the loop counter */
elt14lpo	4:a89e836d9faf	525	k--;
elt14lpo	4:a89e836d9faf	526	}
elt14lpo	4:a89e836d9faf	527
elt14lpo	4:a89e836d9faf	528	/* Store the result in the accumulator in the destination buffer. */
elt14lpo	4:a89e836d9faf	529	*pOut = acc0;
elt14lpo	4:a89e836d9faf	530	/* Destination pointer is updated according to the address modifier, inc */
elt14lpo	4:a89e836d9faf	531	pOut += inc;
elt14lpo	4:a89e836d9faf	532
elt14lpo	4:a89e836d9faf	533	*pOut = acc1;
elt14lpo	4:a89e836d9faf	534	pOut += inc;
elt14lpo	4:a89e836d9faf	535
elt14lpo	4:a89e836d9faf	536	*pOut = acc2;
elt14lpo	4:a89e836d9faf	537	pOut += inc;
elt14lpo	4:a89e836d9faf	538
elt14lpo	4:a89e836d9faf	539	*pOut = acc3;
elt14lpo	4:a89e836d9faf	540	pOut += inc;
elt14lpo	4:a89e836d9faf	541
elt14lpo	4:a89e836d9faf	542	/* Increment the pointer pIn1 index, count by 4 */
elt14lpo	4:a89e836d9faf	543	count += 4u;
elt14lpo	4:a89e836d9faf	544
elt14lpo	4:a89e836d9faf	545	/* Update the inputA and inputB pointers for next MAC calculation */
elt14lpo	4:a89e836d9faf	546	px = pIn1 + count;
elt14lpo	4:a89e836d9faf	547	py = pIn2;
elt14lpo	4:a89e836d9faf	548
elt14lpo	4:a89e836d9faf	549	/* Decrement the loop counter */
elt14lpo	4:a89e836d9faf	550	blkCnt--;
elt14lpo	4:a89e836d9faf	551	}
elt14lpo	4:a89e836d9faf	552
elt14lpo	4:a89e836d9faf	553	/* If the blockSize2 is not a multiple of 4, compute any remaining output samples here.
elt14lpo	4:a89e836d9faf	554	** No loop unrolling is used. */
elt14lpo	4:a89e836d9faf	555	blkCnt = blockSize2 % 0x4u;
elt14lpo	4:a89e836d9faf	556
elt14lpo	4:a89e836d9faf	557	while(blkCnt > 0u)
elt14lpo	4:a89e836d9faf	558	{
elt14lpo	4:a89e836d9faf	559	/* Accumulator is made zero for every iteration */
elt14lpo	4:a89e836d9faf	560	sum = 0.0f;
elt14lpo	4:a89e836d9faf	561
elt14lpo	4:a89e836d9faf	562	/* Apply loop unrolling and compute 4 MACs simultaneously. */
elt14lpo	4:a89e836d9faf	563	k = srcBLen >> 2u;
elt14lpo	4:a89e836d9faf	564
elt14lpo	4:a89e836d9faf	565	/* First part of the processing with loop unrolling. Compute 4 MACs at a time.
elt14lpo	4:a89e836d9faf	566	** a second loop below computes MACs for the remaining 1 to 3 samples. */
elt14lpo	4:a89e836d9faf	567	while(k > 0u)
elt14lpo	4:a89e836d9faf	568	{
elt14lpo	4:a89e836d9faf	569	/* Perform the multiply-accumulates */
elt14lpo	4:a89e836d9faf	570	sum += px++ *py++;
elt14lpo	4:a89e836d9faf	571	sum += px++ *py++;
elt14lpo	4:a89e836d9faf	572	sum += px++ *py++;
elt14lpo	4:a89e836d9faf	573	sum += px++ *py++;
elt14lpo	4:a89e836d9faf	574
elt14lpo	4:a89e836d9faf	575	/* Decrement the loop counter */
elt14lpo	4:a89e836d9faf	576	k--;
elt14lpo	4:a89e836d9faf	577	}
elt14lpo	4:a89e836d9faf	578
elt14lpo	4:a89e836d9faf	579	/* If the srcBLen is not a multiple of 4, compute any remaining MACs here.
elt14lpo	4:a89e836d9faf	580	** No loop unrolling is used. */
elt14lpo	4:a89e836d9faf	581	k = srcBLen % 0x4u;
elt14lpo	4:a89e836d9faf	582
elt14lpo	4:a89e836d9faf	583	while(k > 0u)
elt14lpo	4:a89e836d9faf	584	{
elt14lpo	4:a89e836d9faf	585	/* Perform the multiply-accumulate */
elt14lpo	4:a89e836d9faf	586	sum += px++ *py++;
elt14lpo	4:a89e836d9faf	587
elt14lpo	4:a89e836d9faf	588	/* Decrement the loop counter */
elt14lpo	4:a89e836d9faf	589	k--;
elt14lpo	4:a89e836d9faf	590	}
elt14lpo	4:a89e836d9faf	591
elt14lpo	4:a89e836d9faf	592	/* Store the result in the accumulator in the destination buffer. */
elt14lpo	4:a89e836d9faf	593	*pOut = sum;
elt14lpo	4:a89e836d9faf	594	/* Destination pointer is updated according to the address modifier, inc */
elt14lpo	4:a89e836d9faf	595	pOut += inc;
elt14lpo	4:a89e836d9faf	596
elt14lpo	4:a89e836d9faf	597	/* Increment the pointer pIn1 index, count by 1 */
elt14lpo	4:a89e836d9faf	598	count++;
elt14lpo	4:a89e836d9faf	599
elt14lpo	4:a89e836d9faf	600	/* Update the inputA and inputB pointers for next MAC calculation */
elt14lpo	4:a89e836d9faf	601	px = pIn1 + count;
elt14lpo	4:a89e836d9faf	602	py = pIn2;
elt14lpo	4:a89e836d9faf	603
elt14lpo	4:a89e836d9faf	604	/* Decrement the loop counter */
elt14lpo	4:a89e836d9faf	605	blkCnt--;
elt14lpo	4:a89e836d9faf	606	}
elt14lpo	4:a89e836d9faf	607	}
elt14lpo	4:a89e836d9faf	608	else
elt14lpo	4:a89e836d9faf	609	{
elt14lpo	4:a89e836d9faf	610	/* If the srcBLen is not a multiple of 4,
elt14lpo	4:a89e836d9faf	611	* the blockSize2 loop cannot be unrolled by 4 */
elt14lpo	4:a89e836d9faf	612	blkCnt = blockSize2;
elt14lpo	4:a89e836d9faf	613
elt14lpo	4:a89e836d9faf	614	while(blkCnt > 0u)
elt14lpo	4:a89e836d9faf	615	{
elt14lpo	4:a89e836d9faf	616	/* Accumulator is made zero for every iteration */
elt14lpo	4:a89e836d9faf	617	sum = 0.0f;
elt14lpo	4:a89e836d9faf	618
elt14lpo	4:a89e836d9faf	619	/* Loop over srcBLen */
elt14lpo	4:a89e836d9faf	620	k = srcBLen;
elt14lpo	4:a89e836d9faf	621
elt14lpo	4:a89e836d9faf	622	while(k > 0u)
elt14lpo	4:a89e836d9faf	623	{
elt14lpo	4:a89e836d9faf	624	/* Perform the multiply-accumulate */
elt14lpo	4:a89e836d9faf	625	sum += px++ *py++;
elt14lpo	4:a89e836d9faf	626
elt14lpo	4:a89e836d9faf	627	/* Decrement the loop counter */
elt14lpo	4:a89e836d9faf	628	k--;
elt14lpo	4:a89e836d9faf	629	}
elt14lpo	4:a89e836d9faf	630
elt14lpo	4:a89e836d9faf	631	/* Store the result in the accumulator in the destination buffer. */
elt14lpo	4:a89e836d9faf	632	*pOut = sum;
elt14lpo	4:a89e836d9faf	633	/* Destination pointer is updated according to the address modifier, inc */
elt14lpo	4:a89e836d9faf	634	pOut += inc;
elt14lpo	4:a89e836d9faf	635
elt14lpo	4:a89e836d9faf	636	/* Increment the pointer pIn1 index, count by 1 */
elt14lpo	4:a89e836d9faf	637	count++;
elt14lpo	4:a89e836d9faf	638
elt14lpo	4:a89e836d9faf	639	/* Update the inputA and inputB pointers for next MAC calculation */
elt14lpo	4:a89e836d9faf	640	px = pIn1 + count;
elt14lpo	4:a89e836d9faf	641	py = pIn2;
elt14lpo	4:a89e836d9faf	642
elt14lpo	4:a89e836d9faf	643	/* Decrement the loop counter */
elt14lpo	4:a89e836d9faf	644	blkCnt--;
elt14lpo	4:a89e836d9faf	645	}
elt14lpo	4:a89e836d9faf	646	}
elt14lpo	4:a89e836d9faf	647
elt14lpo	4:a89e836d9faf	648	/* --------------------------
elt14lpo	4:a89e836d9faf	649	* Initializations of stage3
elt14lpo	4:a89e836d9faf	650	* -------------------------*/
elt14lpo	4:a89e836d9faf	651
elt14lpo	4:a89e836d9faf	652	/* sum += x[srcALen-srcBLen+1] * y[0] + x[srcALen-srcBLen+2] * y[1] +...+ x[srcALen-1] * y[srcBLen-1]
elt14lpo	4:a89e836d9faf	653	* sum += x[srcALen-srcBLen+2] * y[0] + x[srcALen-srcBLen+3] * y[1] +...+ x[srcALen-1] * y[srcBLen-1]
elt14lpo	4:a89e836d9faf	654	* ....
elt14lpo	4:a89e836d9faf	655	* sum += x[srcALen-2] * y[0] + x[srcALen-1] * y[1]
elt14lpo	4:a89e836d9faf	656	* sum += x[srcALen-1] * y[0]
elt14lpo	4:a89e836d9faf	657	*/
elt14lpo	4:a89e836d9faf	658
elt14lpo	4:a89e836d9faf	659	/* In this stage the MAC operations are decreased by 1 for every iteration.
elt14lpo	4:a89e836d9faf	660	The count variable holds the number of MAC operations performed */
elt14lpo	4:a89e836d9faf	661	count = srcBLen - 1u;
elt14lpo	4:a89e836d9faf	662
elt14lpo	4:a89e836d9faf	663	/* Working pointer of inputA */
elt14lpo	4:a89e836d9faf	664	pSrc1 = pIn1 + (srcALen - (srcBLen - 1u));
elt14lpo	4:a89e836d9faf	665	px = pSrc1;
elt14lpo	4:a89e836d9faf	666
elt14lpo	4:a89e836d9faf	667	/* Working pointer of inputB */
elt14lpo	4:a89e836d9faf	668	py = pIn2;
elt14lpo	4:a89e836d9faf	669
elt14lpo	4:a89e836d9faf	670	/* -------------------
elt14lpo	4:a89e836d9faf	671	* Stage3 process
elt14lpo	4:a89e836d9faf	672	* ------------------*/
elt14lpo	4:a89e836d9faf	673
elt14lpo	4:a89e836d9faf	674	while(blockSize3 > 0u)
elt14lpo	4:a89e836d9faf	675	{
elt14lpo	4:a89e836d9faf	676	/* Accumulator is made zero for every iteration */
elt14lpo	4:a89e836d9faf	677	sum = 0.0f;
elt14lpo	4:a89e836d9faf	678
elt14lpo	4:a89e836d9faf	679	/* Apply loop unrolling and compute 4 MACs simultaneously. */
elt14lpo	4:a89e836d9faf	680	k = count >> 2u;
elt14lpo	4:a89e836d9faf	681
elt14lpo	4:a89e836d9faf	682	/* First part of the processing with loop unrolling. Compute 4 MACs at a time.
elt14lpo	4:a89e836d9faf	683	** a second loop below computes MACs for the remaining 1 to 3 samples. */
elt14lpo	4:a89e836d9faf	684	while(k > 0u)
elt14lpo	4:a89e836d9faf	685	{
elt14lpo	4:a89e836d9faf	686	/* Perform the multiply-accumulates */
elt14lpo	4:a89e836d9faf	687	/* sum += x[srcALen - srcBLen + 4] * y[3] */
elt14lpo	4:a89e836d9faf	688	sum += px++ *py++;
elt14lpo	4:a89e836d9faf	689	/* sum += x[srcALen - srcBLen + 3] * y[2] */
elt14lpo	4:a89e836d9faf	690	sum += px++ *py++;
elt14lpo	4:a89e836d9faf	691	/* sum += x[srcALen - srcBLen + 2] * y[1] */
elt14lpo	4:a89e836d9faf	692	sum += px++ *py++;
elt14lpo	4:a89e836d9faf	693	/* sum += x[srcALen - srcBLen + 1] * y[0] */
elt14lpo	4:a89e836d9faf	694	sum += px++ *py++;
elt14lpo	4:a89e836d9faf	695
elt14lpo	4:a89e836d9faf	696	/* Decrement the loop counter */
elt14lpo	4:a89e836d9faf	697	k--;
elt14lpo	4:a89e836d9faf	698	}
elt14lpo	4:a89e836d9faf	699
elt14lpo	4:a89e836d9faf	700	/* If the count is not a multiple of 4, compute any remaining MACs here.
elt14lpo	4:a89e836d9faf	701	** No loop unrolling is used. */
elt14lpo	4:a89e836d9faf	702	k = count % 0x4u;
elt14lpo	4:a89e836d9faf	703
elt14lpo	4:a89e836d9faf	704	while(k > 0u)
elt14lpo	4:a89e836d9faf	705	{
elt14lpo	4:a89e836d9faf	706	/* Perform the multiply-accumulates */
elt14lpo	4:a89e836d9faf	707	sum += px++ *py++;
elt14lpo	4:a89e836d9faf	708
elt14lpo	4:a89e836d9faf	709	/* Decrement the loop counter */
elt14lpo	4:a89e836d9faf	710	k--;
elt14lpo	4:a89e836d9faf	711	}
elt14lpo	4:a89e836d9faf	712
elt14lpo	4:a89e836d9faf	713	/* Store the result in the accumulator in the destination buffer. */
elt14lpo	4:a89e836d9faf	714	*pOut = sum;
elt14lpo	4:a89e836d9faf	715	/* Destination pointer is updated according to the address modifier, inc */
elt14lpo	4:a89e836d9faf	716	pOut += inc;
elt14lpo	4:a89e836d9faf	717
elt14lpo	4:a89e836d9faf	718	/* Update the inputA and inputB pointers for next MAC calculation */
elt14lpo	4:a89e836d9faf	719	px = ++pSrc1;
elt14lpo	4:a89e836d9faf	720	py = pIn2;
elt14lpo	4:a89e836d9faf	721
elt14lpo	4:a89e836d9faf	722	/* Decrement the MAC count */
elt14lpo	4:a89e836d9faf	723	count--;
elt14lpo	4:a89e836d9faf	724
elt14lpo	4:a89e836d9faf	725	/* Decrement the loop counter */
elt14lpo	4:a89e836d9faf	726	blockSize3--;
elt14lpo	4:a89e836d9faf	727	}
elt14lpo	4:a89e836d9faf	728
elt14lpo	4:a89e836d9faf	729	#else
elt14lpo	4:a89e836d9faf	730
elt14lpo	4:a89e836d9faf	731	/* Run the below code for Cortex-M0 */
elt14lpo	4:a89e836d9faf	732
elt14lpo	4:a89e836d9faf	733	float pIn1 = pSrcA; / inputA pointer */
elt14lpo	4:a89e836d9faf	734	float pIn2 = pSrcB + (srcBLen - 1u); / inputB pointer */
elt14lpo	4:a89e836d9faf	735	float sum; /* Accumulator */
elt14lpo	4:a89e836d9faf	736	int i = 0u, j; /* loop counters */
elt14lpo	4:a89e836d9faf	737	int inv = 0u; /* Reverse order flag */
elt14lpo	4:a89e836d9faf	738	int tot = 0u; /* Length */
elt14lpo	4:a89e836d9faf	739
elt14lpo	4:a89e836d9faf	740	/* The algorithm implementation is based on the lengths of the inputs. */
elt14lpo	4:a89e836d9faf	741	/* srcB is always made to slide across srcA. */
elt14lpo	4:a89e836d9faf	742	/* So srcBLen is always considered as shorter or equal to srcALen */
elt14lpo	4:a89e836d9faf	743	/* But CORR(x, y) is reverse of CORR(y, x) */
elt14lpo	4:a89e836d9faf	744	/* So, when srcBLen > srcALen, output pointer is made to point to the end of the output buffer */
elt14lpo	4:a89e836d9faf	745	/* and a varaible, inv is set to 1 */
elt14lpo	4:a89e836d9faf	746	/* If lengths are not equal then zero pad has to be done to make the two
elt14lpo	4:a89e836d9faf	747	* inputs of same length. But to improve the performance, we assume zeroes
elt14lpo	4:a89e836d9faf	748	* in the output instead of zero padding either of the the inputs*/
elt14lpo	4:a89e836d9faf	749	/* If srcALen > srcBLen, (srcALen - srcBLen) zeroes has to included in the
elt14lpo	4:a89e836d9faf	750	* starting of the output buffer */
elt14lpo	4:a89e836d9faf	751	/* If srcALen < srcBLen, (srcALen - srcBLen) zeroes has to included in the
elt14lpo	4:a89e836d9faf	752	* ending of the output buffer */
elt14lpo	4:a89e836d9faf	753	/* Once the zero padding is done the remaining of the output is calcualted
elt14lpo	4:a89e836d9faf	754	* using convolution but with the shorter signal time shifted. */
elt14lpo	4:a89e836d9faf	755
elt14lpo	4:a89e836d9faf	756	/* Calculate the length of the remaining sequence */
elt14lpo	4:a89e836d9faf	757	tot = ((srcALen + srcBLen) - 2u);
elt14lpo	4:a89e836d9faf	758
elt14lpo	4:a89e836d9faf	759	if(srcALen > srcBLen)
elt14lpo	4:a89e836d9faf	760	{
elt14lpo	4:a89e836d9faf	761	/* Calculating the number of zeros to be padded to the output */
elt14lpo	4:a89e836d9faf	762	j = srcALen - srcBLen;
elt14lpo	4:a89e836d9faf	763
elt14lpo	4:a89e836d9faf	764	/* Initialise the pointer after zero padding */
elt14lpo	4:a89e836d9faf	765	pDst += j;
elt14lpo	4:a89e836d9faf	766	}
elt14lpo	4:a89e836d9faf	767
elt14lpo	4:a89e836d9faf	768	else if(srcALen < srcBLen)
elt14lpo	4:a89e836d9faf	769	{
elt14lpo	4:a89e836d9faf	770	/* Initialization to inputB pointer */
elt14lpo	4:a89e836d9faf	771	pIn1 = pSrcB;
elt14lpo	4:a89e836d9faf	772
elt14lpo	4:a89e836d9faf	773	/* Initialization to the end of inputA pointer */
elt14lpo	4:a89e836d9faf	774	pIn2 = pSrcA + (srcALen - 1u);
elt14lpo	4:a89e836d9faf	775
elt14lpo	4:a89e836d9faf	776	/* Initialisation of the pointer after zero padding */
elt14lpo	4:a89e836d9faf	777	pDst = pDst + tot;
elt14lpo	4:a89e836d9faf	778
elt14lpo	4:a89e836d9faf	779	/* Swapping the lengths */
elt14lpo	4:a89e836d9faf	780	j = srcALen;
elt14lpo	4:a89e836d9faf	781	srcALen = srcBLen;
elt14lpo	4:a89e836d9faf	782	srcBLen = j;
elt14lpo	4:a89e836d9faf	783
elt14lpo	4:a89e836d9faf	784	/* Setting the reverse flag */
elt14lpo	4:a89e836d9faf	785	inv = 1;
elt14lpo	4:a89e836d9faf	786
elt14lpo	4:a89e836d9faf	787	}
elt14lpo	4:a89e836d9faf	788
elt14lpo	4:a89e836d9faf	789	/* Loop to calculate convolution for output length number of times */
elt14lpo	4:a89e836d9faf	790	for (i = 0u; i <= tot; i++)
elt14lpo	4:a89e836d9faf	791	{
elt14lpo	4:a89e836d9faf	792	/* Initialize sum with zero to carry on MAC operations */
elt14lpo	4:a89e836d9faf	793	sum = 0.0f;
elt14lpo	4:a89e836d9faf	794
elt14lpo	4:a89e836d9faf	795	/* Loop to perform MAC operations according to convolution equation */
elt14lpo	4:a89e836d9faf	796	for (j = 0u; j <= i; j++)
elt14lpo	4:a89e836d9faf	797	{
elt14lpo	4:a89e836d9faf	798	/* Check the array limitations */
elt14lpo	4:a89e836d9faf	799	if((((i - j) < srcBLen) && (j < srcALen)))
elt14lpo	4:a89e836d9faf	800	{
elt14lpo	4:a89e836d9faf	801	/* z[i] += x[i-j] * y[j] */
elt14lpo	4:a89e836d9faf	802	sum += pIn1[j] * pIn2[-((int32_t) i - j)];
elt14lpo	4:a89e836d9faf	803	}
elt14lpo	4:a89e836d9faf	804	}
elt14lpo	4:a89e836d9faf	805	/* Store the output in the destination buffer */
elt14lpo	4:a89e836d9faf	806	if(inv == 1)
elt14lpo	4:a89e836d9faf	807	*pDst-- = sum;
elt14lpo	4:a89e836d9faf	808	else
elt14lpo	4:a89e836d9faf	809	*pDst++ = sum;
elt14lpo	4:a89e836d9faf	810	}
elt14lpo	4:a89e836d9faf	811
elt14lpo	4:a89e836d9faf	812	#endif /* #ifndef ARM_MATH_CM0_FAMILY */
elt14lpo	4:a89e836d9faf	813
elt14lpo	4:a89e836d9faf	814	}
elt14lpo	4:a89e836d9faf	815
elt14lpo	4:a89e836d9faf	816	/**
elt14lpo	4:a89e836d9faf	817	* @} end of Corr group
elt14lpo	4:a89e836d9faf	818	*/
elt14lpo	0:c81dc1cb885e	819
elt14lpo	0:c81dc1cb885e	820	// main() runs in its own thread in the OS
elt14lpo	0:c81dc1cb885e	821	int main()
elt14lpo	0:c81dc1cb885e	822	{
elt14lpo	4:a89e836d9faf	823
elt14lpo	0:c81dc1cb885e	824	for(int i = 0; i < test; i++) {
elt14lpo	0:c81dc1cb885e	825	delaytest[i] = -420 + i*105;
elt14lpo	0:c81dc1cb885e	826	}
elt14lpo	0:c81dc1cb885e	827	t.start(); // start timer
elt14lpo	0:c81dc1cb885e	828
elt14lpo	0:c81dc1cb885e	829	//while (true) {
elt14lpo	0:c81dc1cb885e	830	led1 = !led1;
elt14lpo	0:c81dc1cb885e	831	wait(0.5);
elt14lpo	0:c81dc1cb885e	832
elt14lpo	0:c81dc1cb885e	833
elt14lpo	0:c81dc1cb885e	834	//STATE MACHINE
elt14lpo	0:c81dc1cb885e	835	STATE = IDLE;
elt14lpo	0:c81dc1cb885e	836	//int counter = 0;
elt14lpo	0:c81dc1cb885e	837	while (true) {
elt14lpo	0:c81dc1cb885e	838	switch (STATE) {
elt14lpo	0:c81dc1cb885e	839	case IDLE: //always start here
elt14lpo	0:c81dc1cb885e	840	DebugPrintState( std::cout << "Nucleo state is IDLE: " << std::endl; );
elt14lpo	0:c81dc1cb885e	841	Debug( wait(0.5); );
elt14lpo	0:c81dc1cb885e	842	if (!calibratedStatus) STATE = CALIBRATE;
elt14lpo	0:c81dc1cb885e	843	else STATE = TESTNEW;
elt14lpo	0:c81dc1cb885e	844	break;
elt14lpo	0:c81dc1cb885e	845
elt14lpo	0:c81dc1cb885e	846	case CALIBRATE:
elt14lpo	0:c81dc1cb885e	847	DebugPrintState( std::cout << "Nucleo state is CALIBRATE: " << std::endl; );
elt14lpo	0:c81dc1cb885e	848	Debug( wait(1); );
elt14lpo	0:c81dc1cb885e	849	//listen for X seconds to background noise, to set accurate threshold value
elt14lpo	0:c81dc1cb885e	850	// This should be done only once when rebooting Nucleo
elt14lpo	0:c81dc1cb885e	851	int startTime = t.read_us();
elt14lpo	0:c81dc1cb885e	852	int offsetTime = 3000; //microseconds
elt14lpo	0:c81dc1cb885e	853	int blinkTime = 500; //microseconds
elt14lpo	0:c81dc1cb885e	854	while (t.read_us() < (startTime + offsetTime) ) {
elt14lpo	0:c81dc1cb885e	855	double micValue_1 = getAudioValue(mic1);
elt14lpo	0:c81dc1cb885e	856	if ( calibrateThreshold(micValue_1, threshold_1) ) {
elt14lpo	0:c81dc1cb885e	857	threshold_1 = micValue_1; //threshold value updated
elt14lpo	0:c81dc1cb885e	858	}
elt14lpo	0:c81dc1cb885e	859	double micValue_2 = getAudioValue(mic2);
elt14lpo	0:c81dc1cb885e	860	if ( calibrateThreshold(micValue_2, threshold_2) ) {
elt14lpo	0:c81dc1cb885e	861	threshold_2 = micValue_2; //threshold value updated
elt14lpo	0:c81dc1cb885e	862	}
elt14lpo	0:c81dc1cb885e	863	//make LED blink every 500 ms
elt14lpo	0:c81dc1cb885e	864	if ( t.read_us() > (startTime + blinkTime) ) {
elt14lpo	0:c81dc1cb885e	865	led1 = !led1;
elt14lpo	0:c81dc1cb885e	866	blinkTime = blinkTime + 500;
elt14lpo	0:c81dc1cb885e	867	}
elt14lpo	0:c81dc1cb885e	868	}
elt14lpo	0:c81dc1cb885e	869	threshold_1 = threshold_2 + threshold_adjust;
elt14lpo	0:c81dc1cb885e	870	threshold_2 = threshold_2 + threshold_adjust;
elt14lpo	0:c81dc1cb885e	871
elt14lpo	0:c81dc1cb885e	872	//Calibrate temp and hum
elt14lpo	0:c81dc1cb885e	873	if(checkTemp){
elt14lpo	0:c81dc1cb885e	874	bool done = false;
elt14lpo	0:c81dc1cb885e	875	while(!done) {
elt14lpo	0:c81dc1cb885e	876	if(sensor.readData() == 0) {
elt14lpo	0:c81dc1cb885e	877	temp = sensor.ReadTemperature(CELCIUS);
elt14lpo	0:c81dc1cb885e	878	hum = sensor.ReadHumidity();
elt14lpo	0:c81dc1cb885e	879	DebugPrintState(std::cout << "Temp: " << temp << "Degrees Celcius" <<std::endl; );
elt14lpo	0:c81dc1cb885e	880	DebugPrintState(std::cout << "Hum: " << temp << "%" <<std::endl; );
elt14lpo	0:c81dc1cb885e	881	done = true;
elt14lpo	0:c81dc1cb885e	882	}
elt14lpo	0:c81dc1cb885e	883	}
elt14lpo	0:c81dc1cb885e	884	}
elt14lpo	0:c81dc1cb885e	885
elt14lpo	0:c81dc1cb885e	886	calibratedStatus = true;
elt14lpo	0:c81dc1cb885e	887	STATE = TESTNEW; //next state
elt14lpo	0:c81dc1cb885e	888	break;
elt14lpo	0:c81dc1cb885e	889
elt14lpo	0:c81dc1cb885e	890	case TESTNEW:
elt14lpo	0:c81dc1cb885e	891	DebugPrintState( std::cout << "Nucleo state is TESTNEW: " << std::endl; );
elt14lpo	0:c81dc1cb885e	892	int i = 0;
elt14lpo	0:c81dc1cb885e	893	bool quit = false;
elt14lpo	0:c81dc1cb885e	894	while(!quit) {
elt14lpo	0:c81dc1cb885e	895	channel_1[i] = getAudioValue(mic1);
elt14lpo	0:c81dc1cb885e	896	timestamps_1[i] = t.read_us();
elt14lpo	0:c81dc1cb885e	897	channel_2[i] = getAudioValue(mic2);
elt14lpo	0:c81dc1cb885e	898	timestamps_2[i] = t.read_us();
elt14lpo	0:c81dc1cb885e	899	if(overThreshold(channel_1[i], channel_2[i]) == true) {
elt14lpo	0:c81dc1cb885e	900	capture_1[0] = channel_1[i];
elt14lpo	0:c81dc1cb885e	901	capturestamps_1[0] = timestamps_1[i];
elt14lpo	0:c81dc1cb885e	902	capture_2[0] = channel_2[i];
elt14lpo	0:c81dc1cb885e	903	capturestamps_2[0] = timestamps_2[i];
elt14lpo	0:c81dc1cb885e	904	for(int i = 1; i < captureLength; i++) {
elt14lpo	0:c81dc1cb885e	905	capture_1[i] = getAudioValue(mic1);
elt14lpo	0:c81dc1cb885e	906	capturestamps_1[i] = t.read_us();
elt14lpo	0:c81dc1cb885e	907	capture_2[i] = getAudioValue(mic2);
elt14lpo	0:c81dc1cb885e	908	capturestamps_2[i] = t.read_us();
elt14lpo	0:c81dc1cb885e	909	}
elt14lpo	0:c81dc1cb885e	910	quit = true;
elt14lpo	0:c81dc1cb885e	911	}
elt14lpo	0:c81dc1cb885e	912	if(i < dataLength) {
elt14lpo	0:c81dc1cb885e	913	i++;
elt14lpo	0:c81dc1cb885e	914	} else {
elt14lpo	0:c81dc1cb885e	915	i = 0;
elt14lpo	0:c81dc1cb885e	916	}
elt14lpo	0:c81dc1cb885e	917	}
elt14lpo	0:c81dc1cb885e	918	STATE = CALC;
elt14lpo	0:c81dc1cb885e	919	break;
elt14lpo	0:c81dc1cb885e	920
elt14lpo	0:c81dc1cb885e	921
elt14lpo	0:c81dc1cb885e	922	case CALC:
elt14lpo	0:c81dc1cb885e	923	DebugPrintState( std::cout << "Nucleo state is CALC: " << std::endl; );
elt14lpo	0:c81dc1cb885e	924	//Debug( wait(0.5); );
elt14lpo	4:a89e836d9faf	925	luke_correlate_f32(p1, cL, p2, cL, p_res);
elt14lpo	4:a89e836d9faf	926	Debug(
elt14lpo	4:a89e836d9faf	927	for(int i = 0; i < dataLength; i++){
elt14lpo	4:a89e836d9faf	928	std::cout << dsp_res[i] << " ";
elt14lpo	4:a89e836d9faf	929	});
elt14lpo	4:a89e836d9faf	930
elt14lpo	0:c81dc1cb885e	931	int positionOfMaxVal_1 = FindPeak(1);
elt14lpo	0:c81dc1cb885e	932	int positionOfMaxVal_2 = FindPeak(2);
elt14lpo	0:c81dc1cb885e	933	//run functions
elt14lpo	0:c81dc1cb885e	934	double timedelay = FindTimeDelay(positionOfMaxVal_1, positionOfMaxVal_2); //microseceonds
elt14lpo	0:c81dc1cb885e	935	if(abs(timedelay) > micDist/calcSoundSpeed(temp, hum)){
elt14lpo	0:c81dc1cb885e	936	STATE = CALC_ERROR;
elt14lpo	0:c81dc1cb885e	937	break;
elt14lpo	0:c81dc1cb885e	938	}
elt14lpo	0:c81dc1cb885e	939	double speed = calcSoundSpeed(temp, hum); //meters per second
elt14lpo	0:c81dc1cb885e	940	double distance = calcDist(timedelay/1000000, speed); //input converted to meters
elt14lpo	0:c81dc1cb885e	941	double angle = calcAng((double)distance, micDist); //0,15m = 15cm = 150mm, double type cast because of asin function in angle calculation
elt14lpo	0:c81dc1cb885e	942	//go to state SEND if no calc_error
elt14lpo	0:c81dc1cb885e	943
elt14lpo	0:c81dc1cb885e	944	Debug(
elt14lpo	0:c81dc1cb885e	945	std::cout << "max position for channel 1: " << positionOfMaxVal_1+1 << std::endl;
elt14lpo	0:c81dc1cb885e	946	std::cout << "max position for channel 2: " << positionOfMaxVal_2+1 << std::endl;
elt14lpo	0:c81dc1cb885e	947	std::cout << "run FindPeak, delay is: " << timedelay << "microseconds" << std::endl;
elt14lpo	0:c81dc1cb885e	948	std::cout << "run calcDist, delta s is: " << distance << " millimeters" << std::endl;
elt14lpo	0:c81dc1cb885e	949	std::cout << "run calcAngle, angle is: " << angle << " radians" << std::endl;
elt14lpo	0:c81dc1cb885e	950	std::cout << "run calcAngle, angle is: " << angle*(180 / PI) << " degrees" << std::endl;
elt14lpo	0:c81dc1cb885e	951	std::cout << "run convertAngToCamNbr, coordinates: "<< convertAngToCamNbr(angle)<<std::endl; //return "panNumber tiltNumber";
elt14lpo	0:c81dc1cb885e	952	);
elt14lpo	0:c81dc1cb885e	953	if (angle > (3 * PI )/2 \|\| angle < 0 ) { //vinkel larger than 270 eller minde än noll
elt14lpo	0:c81dc1cb885e	954	STATE = CALC_ERROR;
elt14lpo	0:c81dc1cb885e	955	} else {
elt14lpo	0:c81dc1cb885e	956	STATE = SEND;
elt14lpo	0:c81dc1cb885e	957	}
elt14lpo	0:c81dc1cb885e	958	break;
elt14lpo	0:c81dc1cb885e	959
elt14lpo	0:c81dc1cb885e	960	case CALC_ERROR:
elt14lpo	0:c81dc1cb885e	961	DebugPrintState( std::cout << "Nucleo state is CALC_ERROR: " << std::endl; );
elt14lpo	0:c81dc1cb885e	962	Debug( wait(0.5); );
elt14lpo	0:c81dc1cb885e	963	//error message
elt14lpo	0:c81dc1cb885e	964	std::cout << "Error. angle not within limits 0 -270 degrees" << std::endl;
elt14lpo	0:c81dc1cb885e	965	//nollställ vektorer, , stoppa klockan , osv
elt14lpo	0:c81dc1cb885e	966	STATE = TESTNEW;
elt14lpo	0:c81dc1cb885e	967	break;
elt14lpo	0:c81dc1cb885e	968
elt14lpo	0:c81dc1cb885e	969	case SEND:
elt14lpo	0:c81dc1cb885e	970	DebugPrintState( std::cout << "Nucleo state is SEND: " << std::endl; );
elt14lpo	0:c81dc1cb885e	971	Debug( wait(0.5); );
elt14lpo	0:c81dc1cb885e	972	// send coordinates to serial port to camera
elt14lpo	0:c81dc1cb885e	973	std::cout<<convertAngToCamNbr(angle)<<std::endl; //return "panNumber tiltNumber";
elt14lpo	0:c81dc1cb885e	974	Debug( wait(0.5); );
elt14lpo	0:c81dc1cb885e	975	STATE = IDLE;
elt14lpo	0:c81dc1cb885e	976	wait(5);
elt14lpo	0:c81dc1cb885e	977	break;
elt14lpo	0:c81dc1cb885e	978	}
elt14lpo	0:c81dc1cb885e	979	}
elt14lpo	0:c81dc1cb885e	980	}
elt14lpo	0:c81dc1cb885e	981

Repository toolbox

Export to desktop IDE

Build repository

Repository details

Type:	Program
Mbed OS support:	Mbed 2 deprecated
Created:	12 May 2017
Imports:	4
Forks:	0
Commits:	5
Dependents:	0
Dependencies:	2
Followers:	6

dsp_test.cpp@4:a89e836d9faf, 2017-05-12 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning