| User | Revision | Line number | New contents of line |
|---|
| elt14lpo |
0:c81dc1cb885e
|
1
|
#include "mbed.h"
|
| elt14lpo |
0:c81dc1cb885e
|
2
|
#include <iostream> /* cout */
|
| elt14lpo |
0:c81dc1cb885e
|
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 |
0:c81dc1cb885e
|
12
|
#include "arm_correlate_f32.h"
|
| elt14lpo |
0:c81dc1cb885e
|
13
|
//#include "arm_correlate_fast_q15.h"
|
| elt14lpo |
0:c81dc1cb885e
|
14
|
|
| elt14lpo |
0:c81dc1cb885e
|
15
|
//using namespace std;
|
| elt14lpo |
0:c81dc1cb885e
|
16
|
|
| elt14lpo |
0:c81dc1cb885e
|
17
|
/* DEBUG FUNCTION
|
| elt14lpo |
0:c81dc1cb885e
|
18
|
// ersätter Debug(xyz) med xyz , där xyz är din kod
|
| elt14lpo |
0:c81dc1cb885e
|
19
|
//För att aktivera:
|
| elt14lpo |
0:c81dc1cb885e
|
20
|
#define Debug(xyz) xyz
|
| elt14lpo |
0:c81dc1cb885e
|
21
|
|
| elt14lpo |
0:c81dc1cb885e
|
22
|
//För att "stänga av":
|
| elt14lpo |
0:c81dc1cb885e
|
23
|
#define Debug(xyz)
|
| elt14lpo |
0:c81dc1cb885e
|
24
|
|
| elt14lpo |
0:c81dc1cb885e
|
25
|
//I din kod, skriv din debug kod liknande så här:
|
| elt14lpo |
0:c81dc1cb885e
|
26
|
Debug( std::cout << "My text: " << myVariable << std::endl; );
|
| elt14lpo |
0:c81dc1cb885e
|
27
|
|
| elt14lpo |
0:c81dc1cb885e
|
28
|
*/
|
| elt14lpo |
0:c81dc1cb885e
|
29
|
|
| elt14lpo |
0:c81dc1cb885e
|
30
|
#define Debug(x) x
|
| elt14lpo |
0:c81dc1cb885e
|
31
|
#define DebugPrintState(y) y
|
| elt14lpo |
0:c81dc1cb885e
|
32
|
#define DebugArcSin(z) z
|
| elt14lpo |
0:c81dc1cb885e
|
33
|
|
| elt14lpo |
0:c81dc1cb885e
|
34
|
|
| elt14lpo |
0:c81dc1cb885e
|
35
|
//----------VARIABLES HERE
|
| elt14lpo |
2:1161fea84522
|
36
|
int dataLength = 1000;
|
| elt14lpo |
2:1161fea84522
|
37
|
int captureLength = 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.6016*temp + 0.0126*hum;
|
| 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 |
0:c81dc1cb885e
|
195
|
|
| elt14lpo |
0:c81dc1cb885e
|
196
|
// main() runs in its own thread in the OS
|
| elt14lpo |
0:c81dc1cb885e
|
197
|
int main()
|
| elt14lpo |
0:c81dc1cb885e
|
198
|
{
|
| elt14lpo |
0:c81dc1cb885e
|
199
|
for(int i = 0; i < test; i++) {
|
| elt14lpo |
0:c81dc1cb885e
|
200
|
delaytest[i] = -420 + i*105;
|
| elt14lpo |
0:c81dc1cb885e
|
201
|
}
|
| elt14lpo |
0:c81dc1cb885e
|
202
|
t.start(); // start timer
|
| elt14lpo |
0:c81dc1cb885e
|
203
|
|
| elt14lpo |
0:c81dc1cb885e
|
204
|
//while (true) {
|
| elt14lpo |
0:c81dc1cb885e
|
205
|
led1 = !led1;
|
| elt14lpo |
0:c81dc1cb885e
|
206
|
wait(0.5);
|
| elt14lpo |
0:c81dc1cb885e
|
207
|
|
| elt14lpo |
0:c81dc1cb885e
|
208
|
|
| elt14lpo |
0:c81dc1cb885e
|
209
|
//STATE MACHINE
|
| elt14lpo |
0:c81dc1cb885e
|
210
|
STATE = IDLE;
|
| elt14lpo |
0:c81dc1cb885e
|
211
|
//int counter = 0;
|
| elt14lpo |
0:c81dc1cb885e
|
212
|
while (true) {
|
| elt14lpo |
0:c81dc1cb885e
|
213
|
switch (STATE) {
|
| elt14lpo |
0:c81dc1cb885e
|
214
|
case IDLE: //always start here
|
| elt14lpo |
0:c81dc1cb885e
|
215
|
DebugPrintState( std::cout << "Nucleo state is IDLE: " << std::endl; );
|
| elt14lpo |
0:c81dc1cb885e
|
216
|
Debug( wait(0.5); );
|
| elt14lpo |
0:c81dc1cb885e
|
217
|
if (!calibratedStatus) STATE = CALIBRATE;
|
| elt14lpo |
0:c81dc1cb885e
|
218
|
else STATE = TESTNEW;
|
| elt14lpo |
0:c81dc1cb885e
|
219
|
break;
|
| elt14lpo |
0:c81dc1cb885e
|
220
|
|
| elt14lpo |
0:c81dc1cb885e
|
221
|
case CALIBRATE:
|
| elt14lpo |
0:c81dc1cb885e
|
222
|
DebugPrintState( std::cout << "Nucleo state is CALIBRATE: " << std::endl; );
|
| elt14lpo |
0:c81dc1cb885e
|
223
|
Debug( wait(1); );
|
| elt14lpo |
0:c81dc1cb885e
|
224
|
//listen for X seconds to background noise, to set accurate threshold value
|
| elt14lpo |
0:c81dc1cb885e
|
225
|
// This should be done only once when rebooting Nucleo
|
| elt14lpo |
0:c81dc1cb885e
|
226
|
int startTime = t.read_us();
|
| elt14lpo |
0:c81dc1cb885e
|
227
|
int offsetTime = 3000; //microseconds
|
| elt14lpo |
0:c81dc1cb885e
|
228
|
int blinkTime = 500; //microseconds
|
| elt14lpo |
0:c81dc1cb885e
|
229
|
while (t.read_us() < (startTime + offsetTime) ) {
|
| elt14lpo |
0:c81dc1cb885e
|
230
|
double micValue_1 = getAudioValue(mic1);
|
| elt14lpo |
0:c81dc1cb885e
|
231
|
if ( calibrateThreshold(micValue_1, threshold_1) ) {
|
| elt14lpo |
0:c81dc1cb885e
|
232
|
threshold_1 = micValue_1; //threshold value updated
|
| elt14lpo |
0:c81dc1cb885e
|
233
|
}
|
| elt14lpo |
0:c81dc1cb885e
|
234
|
double micValue_2 = getAudioValue(mic2);
|
| elt14lpo |
0:c81dc1cb885e
|
235
|
if ( calibrateThreshold(micValue_2, threshold_2) ) {
|
| elt14lpo |
0:c81dc1cb885e
|
236
|
threshold_2 = micValue_2; //threshold value updated
|
| elt14lpo |
0:c81dc1cb885e
|
237
|
}
|
| elt14lpo |
0:c81dc1cb885e
|
238
|
//make LED blink every 500 ms
|
| elt14lpo |
0:c81dc1cb885e
|
239
|
if ( t.read_us() > (startTime + blinkTime) ) {
|
| elt14lpo |
0:c81dc1cb885e
|
240
|
led1 = !led1;
|
| elt14lpo |
0:c81dc1cb885e
|
241
|
blinkTime = blinkTime + 500;
|
| elt14lpo |
0:c81dc1cb885e
|
242
|
}
|
| elt14lpo |
0:c81dc1cb885e
|
243
|
}
|
| elt14lpo |
0:c81dc1cb885e
|
244
|
threshold_1 = threshold_2 + threshold_adjust;
|
| elt14lpo |
0:c81dc1cb885e
|
245
|
threshold_2 = threshold_2 + threshold_adjust;
|
| elt14lpo |
0:c81dc1cb885e
|
246
|
|
| elt14lpo |
0:c81dc1cb885e
|
247
|
//Calibrate temp and hum
|
| elt14lpo |
0:c81dc1cb885e
|
248
|
if(checkTemp){
|
| elt14lpo |
0:c81dc1cb885e
|
249
|
bool done = false;
|
| elt14lpo |
0:c81dc1cb885e
|
250
|
while(!done) {
|
| elt14lpo |
0:c81dc1cb885e
|
251
|
if(sensor.readData() == 0) {
|
| elt14lpo |
0:c81dc1cb885e
|
252
|
temp = sensor.ReadTemperature(CELCIUS);
|
| elt14lpo |
0:c81dc1cb885e
|
253
|
hum = sensor.ReadHumidity();
|
| elt14lpo |
0:c81dc1cb885e
|
254
|
DebugPrintState(std::cout << "Temp: " << temp << "Degrees Celcius" <<std::endl; );
|
| elt14lpo |
0:c81dc1cb885e
|
255
|
DebugPrintState(std::cout << "Hum: " << temp << "%" <<std::endl; );
|
| elt14lpo |
0:c81dc1cb885e
|
256
|
done = true;
|
| elt14lpo |
0:c81dc1cb885e
|
257
|
}
|
| elt14lpo |
0:c81dc1cb885e
|
258
|
}
|
| elt14lpo |
0:c81dc1cb885e
|
259
|
}
|
| elt14lpo |
0:c81dc1cb885e
|
260
|
|
| elt14lpo |
0:c81dc1cb885e
|
261
|
calibratedStatus = true;
|
| elt14lpo |
0:c81dc1cb885e
|
262
|
STATE = TESTNEW; //next state
|
| elt14lpo |
0:c81dc1cb885e
|
263
|
break;
|
| elt14lpo |
0:c81dc1cb885e
|
264
|
|
| elt14lpo |
0:c81dc1cb885e
|
265
|
case TESTNEW:
|
| elt14lpo |
0:c81dc1cb885e
|
266
|
DebugPrintState( std::cout << "Nucleo state is TESTNEW: " << std::endl; );
|
| elt14lpo |
0:c81dc1cb885e
|
267
|
int i = 0;
|
| elt14lpo |
0:c81dc1cb885e
|
268
|
bool quit = false;
|
| elt14lpo |
0:c81dc1cb885e
|
269
|
while(!quit) {
|
| elt14lpo |
0:c81dc1cb885e
|
270
|
channel_1[i] = getAudioValue(mic1);
|
| elt14lpo |
0:c81dc1cb885e
|
271
|
timestamps_1[i] = t.read_us();
|
| elt14lpo |
0:c81dc1cb885e
|
272
|
channel_2[i] = getAudioValue(mic2);
|
| elt14lpo |
0:c81dc1cb885e
|
273
|
timestamps_2[i] = t.read_us();
|
| elt14lpo |
0:c81dc1cb885e
|
274
|
if(overThreshold(channel_1[i], channel_2[i]) == true) {
|
| elt14lpo |
0:c81dc1cb885e
|
275
|
capture_1[0] = channel_1[i];
|
| elt14lpo |
0:c81dc1cb885e
|
276
|
capturestamps_1[0] = timestamps_1[i];
|
| elt14lpo |
0:c81dc1cb885e
|
277
|
capture_2[0] = channel_2[i];
|
| elt14lpo |
0:c81dc1cb885e
|
278
|
capturestamps_2[0] = timestamps_2[i];
|
| elt14lpo |
0:c81dc1cb885e
|
279
|
for(int i = 1; i < captureLength; i++) {
|
| elt14lpo |
0:c81dc1cb885e
|
280
|
capture_1[i] = getAudioValue(mic1);
|
| elt14lpo |
0:c81dc1cb885e
|
281
|
capturestamps_1[i] = t.read_us();
|
| elt14lpo |
0:c81dc1cb885e
|
282
|
capture_2[i] = getAudioValue(mic2);
|
| elt14lpo |
0:c81dc1cb885e
|
283
|
capturestamps_2[i] = t.read_us();
|
| elt14lpo |
0:c81dc1cb885e
|
284
|
}
|
| elt14lpo |
0:c81dc1cb885e
|
285
|
quit = true;
|
| elt14lpo |
0:c81dc1cb885e
|
286
|
}
|
| elt14lpo |
0:c81dc1cb885e
|
287
|
if(i < dataLength) {
|
| elt14lpo |
0:c81dc1cb885e
|
288
|
i++;
|
| elt14lpo |
0:c81dc1cb885e
|
289
|
} else {
|
| elt14lpo |
0:c81dc1cb885e
|
290
|
i = 0;
|
| elt14lpo |
0:c81dc1cb885e
|
291
|
}
|
| elt14lpo |
0:c81dc1cb885e
|
292
|
}
|
| elt14lpo |
0:c81dc1cb885e
|
293
|
STATE = CALC;
|
| elt14lpo |
0:c81dc1cb885e
|
294
|
break;
|
| elt14lpo |
0:c81dc1cb885e
|
295
|
|
| elt14lpo |
0:c81dc1cb885e
|
296
|
|
| elt14lpo |
0:c81dc1cb885e
|
297
|
case CALC:
|
| elt14lpo |
0:c81dc1cb885e
|
298
|
DebugPrintState( std::cout << "Nucleo state is CALC: " << std::endl; );
|
| elt14lpo |
0:c81dc1cb885e
|
299
|
//Debug( wait(0.5); );
|
| elt14lpo |
0:c81dc1cb885e
|
300
|
arm_correlate_f32(p1, captureLength, p2, captureLength, p_res);
|
| elt14lpo |
0:c81dc1cb885e
|
301
|
|
| elt14lpo |
0:c81dc1cb885e
|
302
|
int positionOfMaxVal_1 = FindPeak(1);
|
| elt14lpo |
0:c81dc1cb885e
|
303
|
int positionOfMaxVal_2 = FindPeak(2);
|
| elt14lpo |
0:c81dc1cb885e
|
304
|
//run functions
|
| elt14lpo |
0:c81dc1cb885e
|
305
|
double timedelay = FindTimeDelay(positionOfMaxVal_1, positionOfMaxVal_2); //microseceonds
|
| elt14lpo |
0:c81dc1cb885e
|
306
|
if(abs(timedelay) > micDist/calcSoundSpeed(temp, hum)){
|
| elt14lpo |
0:c81dc1cb885e
|
307
|
STATE = CALC_ERROR;
|
| elt14lpo |
0:c81dc1cb885e
|
308
|
break;
|
| elt14lpo |
0:c81dc1cb885e
|
309
|
}
|
| elt14lpo |
0:c81dc1cb885e
|
310
|
double speed = calcSoundSpeed(temp, hum); //meters per second
|
| elt14lpo |
0:c81dc1cb885e
|
311
|
double distance = calcDist(timedelay/1000000, speed); //input converted to meters
|
| elt14lpo |
0:c81dc1cb885e
|
312
|
double angle = calcAng((double)distance, micDist); //0,15m = 15cm = 150mm, double type cast because of asin function in angle calculation
|
| elt14lpo |
0:c81dc1cb885e
|
313
|
//go to state SEND if no calc_error
|
| elt14lpo |
0:c81dc1cb885e
|
314
|
|
| elt14lpo |
0:c81dc1cb885e
|
315
|
Debug(
|
| elt14lpo |
0:c81dc1cb885e
|
316
|
std::cout << "max position for channel 1: " << positionOfMaxVal_1+1 << std::endl;
|
| elt14lpo |
0:c81dc1cb885e
|
317
|
std::cout << "max position for channel 2: " << positionOfMaxVal_2+1 << std::endl;
|
| elt14lpo |
0:c81dc1cb885e
|
318
|
std::cout << "run FindPeak, delay is: " << timedelay << "microseconds" << std::endl;
|
| elt14lpo |
0:c81dc1cb885e
|
319
|
std::cout << "run calcDist, delta s is: " << distance << " millimeters" << std::endl;
|
| elt14lpo |
0:c81dc1cb885e
|
320
|
std::cout << "run calcAngle, angle is: " << angle << " radians" << std::endl;
|
| elt14lpo |
0:c81dc1cb885e
|
321
|
std::cout << "run calcAngle, angle is: " << angle*(180 / PI) << " degrees" << std::endl;
|
| elt14lpo |
0:c81dc1cb885e
|
322
|
std::cout << "run convertAngToCamNbr, coordinates: "<< convertAngToCamNbr(angle)<<std::endl; //return "panNumber tiltNumber";
|
| elt14lpo |
0:c81dc1cb885e
|
323
|
);
|
| elt14lpo |
0:c81dc1cb885e
|
324
|
if (angle > (3 * PI )/2 || angle < 0 ) { //vinkel larger than 270 eller minde än noll
|
| elt14lpo |
0:c81dc1cb885e
|
325
|
STATE = CALC_ERROR;
|
| elt14lpo |
0:c81dc1cb885e
|
326
|
} else {
|
| elt14lpo |
0:c81dc1cb885e
|
327
|
STATE = SEND;
|
| elt14lpo |
0:c81dc1cb885e
|
328
|
}
|
| elt14lpo |
0:c81dc1cb885e
|
329
|
break;
|
| elt14lpo |
0:c81dc1cb885e
|
330
|
|
| elt14lpo |
0:c81dc1cb885e
|
331
|
case CALC_ERROR:
|
| elt14lpo |
0:c81dc1cb885e
|
332
|
DebugPrintState( std::cout << "Nucleo state is CALC_ERROR: " << std::endl; );
|
| elt14lpo |
0:c81dc1cb885e
|
333
|
Debug( wait(0.5); );
|
| elt14lpo |
0:c81dc1cb885e
|
334
|
//error message
|
| elt14lpo |
0:c81dc1cb885e
|
335
|
std::cout << "Error. angle not within limits 0 -270 degrees" << std::endl;
|
| elt14lpo |
0:c81dc1cb885e
|
336
|
//nollställ vektorer, , stoppa klockan , osv
|
| elt14lpo |
0:c81dc1cb885e
|
337
|
STATE = TESTNEW;
|
| elt14lpo |
0:c81dc1cb885e
|
338
|
break;
|
| elt14lpo |
0:c81dc1cb885e
|
339
|
|
| elt14lpo |
0:c81dc1cb885e
|
340
|
case SEND:
|
| elt14lpo |
0:c81dc1cb885e
|
341
|
DebugPrintState( std::cout << "Nucleo state is SEND: " << std::endl; );
|
| elt14lpo |
0:c81dc1cb885e
|
342
|
Debug( wait(0.5); );
|
| elt14lpo |
0:c81dc1cb885e
|
343
|
// send coordinates to serial port to camera
|
| elt14lpo |
0:c81dc1cb885e
|
344
|
std::cout<<convertAngToCamNbr(angle)<<std::endl; //return "panNumber tiltNumber";
|
| elt14lpo |
0:c81dc1cb885e
|
345
|
Debug( wait(0.5); );
|
| elt14lpo |
0:c81dc1cb885e
|
346
|
STATE = IDLE;
|
| elt14lpo |
0:c81dc1cb885e
|
347
|
wait(5);
|
| elt14lpo |
0:c81dc1cb885e
|
348
|
break;
|
| elt14lpo |
0:c81dc1cb885e
|
349
|
}
|
| elt14lpo |
0:c81dc1cb885e
|
350
|
}
|
| elt14lpo |
0:c81dc1cb885e
|
351
|
}
|
| elt14lpo |
0:c81dc1cb885e
|
352
|
|
Elektronikprojekt Grupp 13