Project

General

Profile

Statistics
| Branch: | Revision:

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
}