Statistics
| Branch: | Revision:

root / quad1 / AeroQuad / Compass.h @ 9240aaa3

History | View | Annotate | Download (5.16 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
// Class to define sensors that can determine absolute heading
22

    
23
// ***********************************************************************
24
// ************************** Compass Class ******************************
25
// ***********************************************************************
26
class Compass {
27
public: 
28
  int compassAddress;
29
  float heading, absoluteHeading, gyroStartHeading;
30
  float compass;
31
  
32
  Compass(void) { }
33

    
34
  // **********************************************************************
35
  // The following function calls must be defined inside any new subclasses
36
  // **********************************************************************
37
  virtual void initialize(void); 
38
  virtual void measure(void); 
39
  
40
  // *********************************************************
41
  // The following functions are common between all subclasses
42
  // *********************************************************
43
  const float getData(void) {
44
    return compass;
45
  }
46
  
47
  const float getHeading(void) {
48
    return heading;
49
  }
50
  
51
  const float getAbsoluteHeading(void) {
52
    return absoluteHeading;
53
  }
54
};
55

    
56
// ***********************************************************************
57
// ************************ HMC5843 Subclass *****************************
58
// ***********************************************************************
59
class Compass_AeroQuad_v2 : public Compass {
60
// This sets up the HMC5843 from Sparkfun
61
private:
62
  float cosRoll;
63
  float sinRoll;
64
  float cosPitch;
65
  float sinPitch;
66
  float magX;
67
  float magY;
68
  int measuredMagX;
69
  int measuredMagY;
70
  int measuredMagZ;
71
  float smoothFactor; // time constant for complementary filter
72
  float filter1, filter2; // coefficients for complementary filter
73
  float adjustedGyroHeading, previousHead;
74
  int gyroZero;
75
  
76
public: 
77
  Compass_AeroQuad_v2() : Compass(){
78
    compassAddress = 0x1E;
79
    // smoothFactor means time in seconds less than smoothFactor, depend on gyro more
80
    // time greater than smoothFactor depend on magnetometer more (mags are very noisy)
81
    smoothFactor = 1.0; 
82
    filter1 = smoothFactor / (smoothFactor + G_Dt);
83
    filter2 = 1 - filter1;
84
    gyroZero = gyro.getZero(YAW);
85
  }
86

    
87
  // ***********************************************************
88
  // Define all the virtual functions declared in the main class
89
  // ***********************************************************
90
  void initialize(void) {
91
    // Should do a WhoAmI to know if mag is present
92
    updateRegisterI2C(compassAddress, 0x01, 0x20);
93
    updateRegisterI2C(compassAddress, 0x02, 0x00); // continuous 10Hz mode
94
    measure();
95
    gyroStartHeading = getData();
96
    if (gyroStartHeading < 0) gyroStartHeading += 360;
97
    gyro.setStartHeading(gyroStartHeading);
98
  }
99
  
100
  void measure(void) {
101
    sendByteI2C(compassAddress, 0x03);
102
    Wire.requestFrom(compassAddress, 6);
103
    measuredMagX = (Wire.receive() << 8) | Wire.receive();
104
    measuredMagY = (Wire.receive() << 8) | Wire.receive();
105
    measuredMagZ = (Wire.receive() << 8) | Wire.receive();
106
    Wire.endTransmission();
107
    // Heading calculation based on code written by FabQuad
108
    // http://aeroquad.com/showthread.php?691-Hold-your-heading-with-HMC5843-Magnetometer
109
    cosRoll = cos(radians(flightAngle.getData(ROLL)));
110
    sinRoll = sin(radians(flightAngle.getData(ROLL)));
111
    cosPitch = cos(radians(flightAngle.getData(PITCH)));
112
    sinPitch = sin(radians(flightAngle.getData(PITCH)));
113
    magX = measuredMagX * cosPitch + measuredMagY * sinRoll * sinPitch + measuredMagZ * cosRoll * sinPitch;
114
    magY = measuredMagY * cosRoll - measuredMagZ * sinRoll;
115
    compass = -degrees(atan2(-magY, magX));
116
    
117
    // Check if gyroZero adjusted, if it is, reset gyroHeading to compass value
118
    if (gyroZero != gyro.getZero(YAW)) {
119
      gyro.setStartHeading(heading);
120
      gyroZero = gyro.getZero(YAW);
121
    }
122
    
123
    adjustedGyroHeading = gyro.getHeading();
124
    // if compass is positive while gyro is negative force gyro positive past 180
125
    if ((compass > 90) && adjustedGyroHeading < -90) adjustedGyroHeading += 360;
126
    // if compass is negative whie gyro is positive force gyro negative past -180
127
    if ((compass < -90) && adjustedGyroHeading > 90) adjustedGyroHeading -= 360;
128
    
129
    // Complementry filter from http://chiefdelphi.com/media/papers/2010
130
    heading = (filter1 * adjustedGyroHeading) + (filter2 * compass);
131
    
132
    // Change from +/-180 to 0-360
133
    if (heading < 0) absoluteHeading = 360 + heading;
134
    else absoluteHeading = heading;
135
  }
136
};