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)+1e-10; // 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<(arraySize-1); 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 |
} |