root / quad1 / AeroQuad / Sensors.pde @ 9240aaa3
History | View | Annotate | Download (3.3 KB)
1 | 9240aaa3 | Alex | /* |
---|---|---|---|
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 | } |