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?691HoldyourheadingwithHMC5843Magnetometer

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 0360

133 
if (heading < 0) absoluteHeading = 360 + heading; 
134 
else absoluteHeading = heading;

135 
} 
136 
}; 