So now that I have delved into the depths of 2’s compliment and also made sure I was using my incoming data in the correct order for the BitConverter, the Complementary filter from NuclearProjects.com, my IMU is working perfectly.
In the video I am using the Altitude indicator that Jesse wrote to test it with. (He even made the full source code available on CodeProject).
He has even implemented some calculations that now allows TriRot to track the z-Axis as well for Yaw control. Although this implementation is only based on Gyro’s (ITG-3200) and Accelerometers (ADXL345) ,which could drift over time, it is holding rock solid in tests so far.
The C# code that makes the above possible:
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.
Next steps would be to buy the Transmitter and Receiver pair and a PPM Encoder for interfacing into TriRot's Interface board.
|
Saturday 14 June 2014
IMU Test - Video
Subscribe to:
Post Comments (Atom)
No comments:
Post a Comment