root / quad1 / AeroQuad / Sensors.pde @ 9240aaa3
History  View  Annotate  Download (3.3 KB)
1 
/* 

2 
AeroQuad v2.1  October 2010 
3 
www.AeroQuad.com 
4 
Copyright (c) 2010 Ted Carancho. All rights reserved. 
5 
An Open Source Arduino based multicopter. 
6 

7 
This program is free software: you can redistribute it and/or modify 
8 
it under the terms of the GNU General Public License as published by 
9 
the Free Software Foundation, either version 3 of the License, or 
10 
(at your option) any later version. 
11  
12 
This program is distributed in the hope that it will be useful, 
13 
but WITHOUT ANY WARRANTY; without even the implied warranty of 
14 
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 
15 
GNU General Public License for more details. 
16  
17 
You should have received a copy of the GNU General Public License 
18 
along with this program. If not, see <http://www.gnu.org/licenses/>. 
19 
*/ 
20  
21 
// Sensors.pde is responsible for taking on board sensor measuremens of the AeroQuad 
22  
23 
void readSensors(void) { 
24 
// *********************** Read Critical Sensors ********************** 
25 
// Apply low pass filter to sensor values and center around zero 
26 
gyro.measure(); // defined in Gyro.h 
27 
accel.measure(); // defined in Accel.h 
28 

29 
// ********************* Read Slower Sensors ******************* 
30 
#if defined(HeadingMagHold) 
31 
if (currentTime > (compassTime + COMPASSLOOPTIME)) { // 10Hz 
32 
compass.measure(); // defined in compass.h 
33 
compassTime = currentTime; 
34 
} 
35 
#endif 
36 
#if defined(AltitudeHold) 
37 
if (currentTime > (altitudeTime + ALTITUDELOOPTIME)) { // 37Hz 
38 
altitude.measure(); // defined in altitude.h 
39 
altitudeTime = currentTime; 
40 
} 
41 
#endif 
42 

43 
#if defined(SonarHold) 
44 
if (currentTime > (sonarTime + SONARLOOPTIME)) { // ~14Hz 
45 
sonar.measure(); // defined in sonar.h 
46 
sonarTime = currentTime; 
47 
} 
48 
#endif 
49 

50 
// ****************** Calculate Absolute Angle ***************** 
51 
flightAngle.calculate(); // defined in FlightAngle.h 
52 
} 
53  
54 
// Alternate method to calculate arctangent from: http://www.dspguru.com/comp.dsp/tricks/alg/fxdatan2.htm 
55 
float arctan2(float y, float x) { 
56 
float coeff_1 = PI/4; 
57 
float coeff_2 = 3*coeff_1; 
58 
float abs_y = abs(y)+1e10; // kludge to prevent 0/0 condition 
59 
float r, angle; 
60 

61 
if (x >= 0) { 
62 
r = (x  abs_y) / (x + abs_y); 
63 
angle = coeff_1  coeff_1 * r; 
64 
} 
65 
else { 
66 
r = (x + abs_y) / (abs_y  x); 
67 
angle = coeff_2  coeff_1 * r; 
68 
} 
69 
if (y < 0) 
70 
return(angle); // negate if in quad III or IV 
71 
else 
72 
return(angle); 
73 
} 
74  
75 
// Used for sensor calibration 
76 
// The mode of a set of numbers returns the value that occurs most frequently 
77 
int findMode(int *data, int arraySize) { 
78 
boolean done = 0; 
79 
byte i; 
80 
int temp, maxData, frequency, maxFrequency; 
81 

82 
// Sorts numbers from lowest to highest 
83 
while (done != 1) { 
84 
done = 1; 
85 
for (i=0; i<(arraySize1); i++) { 
86 
if (data[i] > data[i+1]) { // numbers are out of order  swap 
87 
temp = data[i+1]; 
88 
data[i+1] = data[i]; 
89 
data[i] = temp; 
90 
done = 0; 
91 
} 
92 
} 
93 
} 
94 

95 
temp = 32768; 
96 
frequency = 0; 
97 
maxFrequency = 0; 
98 

99 
// Count number of times a value occurs in sorted array 
100 
for (i=0; i<arraySize; i++) { 
101 
if (data[i] > temp) { 
102 
frequency = 0; 
103 
temp = data[i]; 
104 
frequency++; 
105 
} else if (data[i] == temp) frequency++; 
106 
if (frequency > maxFrequency) { 
107 
maxFrequency = frequency; 
108 
maxData = data[i]; 
109 
} 
110 
} 
111 
return maxData; 
112 
} 