1: using System;
2: using Microsoft.SPOT;
3: using Tri_Rot_Version_1;
4: using System.Threading;
5: using GHIElectronics.NETMF.System;
6:
7: namespace IMUComplementaryFilter
8: {
9: public class Program
10: {
11:
12: private static Int32 gXZeroValue = 0;
13: private static Int32 gYZeroValue = 0;
14: private static Int32 gZZeroValue = 0;
15:
16: private static Int32 aXZeroValue = 0;
17: private static Int32 aYZeroValue = 0;
18: private static Int32 aZZeroValue = 0;
19: private const Double Rad2Deg = 57.2957795; // 1 radian = 57.2957795 degrees
20: private const Double Deg2Rad = 0.0174532925; // 0.0174532925 rads = 1 deg
21:
22:
23: public static void Main()
24: {
25: Debug.EnableGCMessages(false);
26: int i = 0;
27: Int16[] g;
28: Int16[] a;
29:
30: using (Gyro = new ITG3200I2C())
31: {
32: Gyro.StartUp();
33: }
34:
35: using (Acc = new ADXL345I2C())
36: {
37: Acc.StartUp();
38: }
39: Thread.Sleep(100);
40:
41: Int16 gXTemp = 0;
42: Int16 gYTemp = 0;
43: Int16 gZTemp = 0;
44:
45: Int16 aXTemp = 0;
46: Int16 aYTemp = 0;
47: Int16 aZTemp = 0;
48:
49: while (i < 100)
50: {
51: g = getGyroData();
52: a = getAccData();
53: Thread.Sleep(20);
54: i++;
55: }
56: i = 0;
57: while(i < 100)
58: {
59: g = getGyroData();
60: a = getAccData();
71:
72: gXTemp += g[0];
73: gYTemp += g[1];
74: gZTemp += g[2];
75:
76: aXTemp += a[0];
77: aYTemp += a[1];
78: aZTemp += a[2];
79:
80: Thread.Sleep(20);
81: i++;
82: }
83: gXZeroValue = (Int16)(gXTemp / 100F);
84: gYZeroValue = (Int16)(gYTemp / 100F);
85: gZZeroValue = (Int16)(gZTemp / 100F);
86:
87: aXZeroValue = (Int16)(aXTemp / 100F);
88: aYZeroValue = (Int16)(aYTemp / 100F);
89: aZZeroValue = (Int16)(aZTemp / 100F);
90:
91: Double GyroRateX = 0.0;
92: Double GyroRateY = 0.0;
93: Double GyroAngleX = 0.0;
94: Double GyroAngleY = 0.0;
95: Double GyroAngleZ = 0.0;
96:
97: Double AccelX = 0.0;
98: Double AccelY = 0.0;
99: Double AccelZ = 0.0;
100: Double AccelAngleX = 0.0;
101: Double AccelAngleY = 0.0;
102: Double AccelAngleZ = 0.0;
103: Double Roll = 0.0;
104: Double Pitch = 0.0;
105: Double Yaw = 0.0;
106:
107: Int16 minValx = -4095;
108: Int16 maxValx = 4095;
109: Int16 minValy = -4095;
110: Int16 maxValy = 4095;
111: Int16 minValz = -4095;
112: Int16 maxValz = 4095;
113:
114: long currentTime;
115: Single interval;
116: long lastTime = DateTime.Now.Ticks;
117:
118: long TicksPerMillisecond = TimeSpan.TicksPerMillisecond;
119: Single GyroAngleZ_dt = 0;
120:
121: Telemetry telemetry = new Telemetry();
122:
123:
124: while (true)
125: {
126: currentTime = DateTime.Now.Ticks;
127: interval = ((currentTime - lastTime) / TicksPerMillisecond)/1000F; //in milliseconds
128: lastTime = currentTime;
129:
130: g = getGyroData();
131: a = getAccData();
132:
133: GyroRateX = -1.0 * interval * (g[0] - gXZeroValue) / 14.375F;
134: GyroRateY = interval * (g[1] - gYZeroValue) / 14.375F;
135:
136: GyroAngleZ_dt = interval * (g[2] - gZZeroValue) / 14.375F;
137:
138: GyroAngleZ += -1.0 * GyroAngleZ_dt * (1 / (MathEx.Cos(Deg2Rad * Roll))); // convert Roll angle to Rads, find sin to use as scaler for Yaw
139:
140: if (GyroAngleZ < 0) GyroAngleZ += 360; // Keep within range of 0- 360 deg
141: if (GyroAngleZ >= 360) GyroAngleZ -= 360;
142: //----------------------------------------------------------------
143:
144: //convert read values to degrees -90 to 90 - Needed for atan2
145: Single xAng = map(a[0], minValx, maxValx, -90, 90);
146: Single yAng = map(a[1], minValy, maxValy, -90, 90);
147: Single zAng = map(a[2], minValz, maxValz, -90, 90);
148:
149: //Caculate 360deg values like so: atan2(-yAng, -zAng)
150: //atan2 outputs the value of -π to π (radians)
151: //We are then converting the radians to degrees
152: AccelAngleX = Rad2Deg * (MathEx.Atan2(-xAng, -zAng) + MathEx.PI);
153: AccelAngleY = Rad2Deg * (MathEx.Atan2(-yAng, -zAng) + MathEx.PI);
154:
155: // Keep angles between +-180deg
156: if (AccelAngleX > 180) AccelAngleX = AccelAngleX - 360;
157:
158: if (AccelAngleY <= 180) AccelAngleY = -1.0 * AccelAngleY;
159: if (AccelAngleY > 180) AccelAngleY = 360 - AccelAngleY;
160:
161: // Final values...
162: Roll = (0.98) * (Roll + GyroRateX) + (0.02) * (AccelAngleX);
163: Pitch = (0.98) * (Pitch + GyroRateY) + (0.02) * (AccelAngleY);
164: Yaw = GyroAngleZ;
165:
166: //Debug.Print(Roll.ToString() + "," + Pitch.ToString() + "," + Yaw.ToString());
167: telemetry.SendData(new TelemetryData(0, 0, 0, 0, (Single)Roll, (Single)Pitch, (Single)Yaw));
168: Thread.Sleep(50);
169: }
170: }
171:
172: private static Single map(Single x, Single in_min, Single in_max, Single out_min, Single out_max)
173: {
174: return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
175: }
176:
177: private static ITG3200I2C Gyro;
178: private static ADXL345I2C Acc;
179:
180: private static Int16[] getGyroData()
181: {
182: Int16[] data;
183: using (Gyro = new ITG3200I2C())
184: {
185: data = Gyro.AdcR();
186: }
187: return data;
188: }
189:
190: private static Int16[] getAccData()
191: {
192: Int16[] data;
193: using (Acc = new ADXL345I2C())
194: {
195: data = Acc.AdcR();
196: }
197: return data;
198: }
199:
200: }
201: }
I have now also finally updated my PartsList after noticing that I had never added the Gyro or the Accelerometer to the it.
