Rev 12 | Rev 17 | Go to most recent revision | Details | Compare with Previous | Last modification | View Log | RSS feed
Rev | Author | Line No. | Line |
---|---|---|---|
5 | hbuss | 1 | /*####################################################################################### |
7 | hbuss | 2 | MK3Mag 3D-Magnet sensor |
12 | hbuss | 3 | !!! THIS IS NOT FREE SOFTWARE !!! |
5 | hbuss | 4 | #######################################################################################*/ |
5 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
||
7 | hbuss | 6 | // + Copyright (c) 05.2008 Holger Buss |
8 | hbuss | 7 | // + Thanks to Ilja Fähnrich (P_Latzhalter) |
5 | hbuss | 8 | // + Nur für den privaten Gebrauch |
9 | // + www.MikroKopter.com |
||
10 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
||
12 | hbuss | 11 | // + Die Portierung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur |
12 | // + mit unserer Zustimmung zulässig |
||
13 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
||
5 | hbuss | 14 | // + Es gilt für das gesamte Projekt (Hardware, Software, Binärfiles, Sourcecode und Dokumentation), |
15 | // + dass eine Nutzung (auch auszugsweise) nur für den privaten (nicht-kommerziellen) Gebrauch zulässig ist. |
||
12 | hbuss | 16 | // + AUSNAHME: Ein bei www.mikrokopter.de erworbener vorbestückter MK3Mag darf als Baugruppe auch in kommerziellen Systemen verbaut werden |
17 | // + Im Zweifelsfall bitte anfragen bei: info@mikrokopter.de |
||
5 | hbuss | 18 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
19 | // + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder veröffentlicht, |
||
20 | // + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright müssen dann beiliegen |
||
21 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
||
22 | // + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts |
||
23 | // + auf anderen Webseiten oder sonstigen Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de" |
||
24 | // + eindeutig als Ursprung verlinkt werden |
||
25 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
||
26 | // + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion |
||
27 | // + Benutzung auf eigene Gefahr |
||
28 | // + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden |
||
29 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
||
30 | // + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen |
||
31 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
||
32 | // + Redistributions of source code (with or without modifications) must retain the above copyright notice, |
||
33 | // + this list of conditions and the following disclaimer. |
||
12 | hbuss | 34 | // + * PORTING this software (or parts of it) to systems (other than hardware from www.mikrokopter.de) is NOT allowed |
5 | hbuss | 35 | // + * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived |
36 | // + from this software without specific prior written permission. |
||
37 | // + * The use of this project (hardware, software, binary files, sources and documentation) is only permittet |
||
38 | // + for non-commercial use (directly or indirectly) |
||
39 | // + Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted |
||
40 | // + with our written permission |
||
12 | hbuss | 41 | // + Exception: A preassembled MK3Mag, purchased from www.mikrokopter.de may be used as a part of commercial systems |
42 | // + In case of doubt please contact: info@MikroKopter.de |
||
7 | hbuss | 43 | // + * If sources or documentations are redistributet on other webpages, our webpage (http://www.MikroKopter.de) must be |
44 | // + clearly linked as origin |
||
5 | hbuss | 45 | // + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" |
46 | // + AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE |
||
47 | // + IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE |
||
48 | // + ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE |
||
49 | // + LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR |
||
50 | // + CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF |
||
51 | // + SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS |
||
12 | hbuss | 52 | // + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN |
53 | // + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) |
||
5 | hbuss | 54 | // + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
55 | // + POSSIBILITY OF SUCH DAMAGE. |
||
56 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
||
7 | hbuss | 57 | signed int OffsetN, OffsetR, OffsetZ; |
1 | ingob | 58 | |
7 | hbuss | 59 | signed int RawMagnet1a,RawMagnet1b; // raw AD-Data |
1 | ingob | 60 | signed int RawMagnet2a,RawMagnet2b; |
61 | signed int RawMagnet3a,RawMagnet3b; |
||
16 | holgerb | 62 | signed int RawAccN = 0, RawAccR = 0, RawAccZ = 0; |
63 | signed int AccN = 0, AccR = 0, AccZ = 0; |
||
64 | signed int OffsetAccN = 0, OffsetAccR = 0, OffsetAccZ = 0; |
||
7 | hbuss | 65 | signed int Xmin = 0, Xmax = 0; Ymin = 0, Ymax = 0; Zmin = 0, Zmax = 0; |
1 | ingob | 66 | signed int UncalMagnetN,UncalMagnetR,UncalMagnetZ; // Messwert-Delta ohne Offset- und Verstärker korrektur |
7 | hbuss | 67 | signed int MagnetN,MagnetR,MagnetZ; |
1 | ingob | 68 | unsigned int PwmHeading = 0; |
7 | hbuss | 69 | unsigned int PC_Connected = 0; |
70 | unsigned int Heading; |
||
16 | holgerb | 71 | unsigned char ExternCalState = 0; |
72 | unsigned char AngleSource = 0; |
||
73 | unsigned char AccPresent = 0; |
||
1 | ingob | 74 | #include "main.h" |
75 | |||
7 | hbuss | 76 | uint16_t eeXmin EEMEM = 0; |
77 | uint16_t eeXmax EEMEM = 0; |
||
78 | uint16_t eeYmin EEMEM = 0; |
||
79 | uint16_t eeYmax EEMEM = 0; |
||
80 | uint16_t eeZmin EEMEM = 0; |
||
81 | uint16_t eeZmax EEMEM = 0; |
||
16 | holgerb | 82 | uint16_t eeNoffset EEMEM = 0; |
83 | uint16_t eeRoffset EEMEM = 0; |
||
84 | uint16_t eeZoffset EEMEM = 0; |
||
1 | ingob | 85 | |
86 | //############################################################################ |
||
87 | // |
||
88 | void Wait(unsigned char dauer) |
||
89 | //############################################################################ |
||
90 | { |
||
91 | dauer = (unsigned char)TCNT0 + dauer; |
||
92 | while((TCNT0 - dauer) & 0x80); |
||
93 | } |
||
94 | |||
95 | void CalcFields(void) |
||
7 | hbuss | 96 | { |
97 | UncalMagnetN = (1 * UncalMagnetN + (RawMagnet1a - RawMagnet1b)) / 2; |
||
98 | UncalMagnetR = (1 * UncalMagnetR + (RawMagnet3a - RawMagnet3b)) / 2; |
||
99 | UncalMagnetZ = (1 * UncalMagnetZ + (RawMagnet2a - RawMagnet2b)) / 2; |
||
100 | |||
101 | OffsetN = (Xmin + Xmax) / 2; |
||
102 | OffsetR = (Ymin + Ymax) / 2; |
||
103 | OffsetZ = (Zmin + Zmax) / 2; |
||
104 | |||
105 | MagnetN = (1024L * (long)(UncalMagnetN - OffsetN)) / (Xmax - Xmin); |
||
106 | MagnetR = (1024L * (long)(UncalMagnetR - OffsetR)) / (Ymax - Ymin); |
||
107 | MagnetZ = (1024L * (long)(UncalMagnetZ - OffsetZ)) / (Zmax - Zmin); |
||
1 | ingob | 108 | } |
109 | |||
110 | void CalcHeading(void) |
||
111 | { |
||
112 | double nick_rad, roll_rad, Hx, Hy, Cx, Cy, Cz; |
||
7 | hbuss | 113 | int heading; |
16 | holgerb | 114 | if(!AngleSource) |
115 | { |
||
116 | if(AccN > 125) nick_rad = M_PI / 2; |
||
117 | else |
||
118 | if(AccN < -125) nick_rad = -M_PI / 2; |
||
119 | else |
||
120 | { |
||
121 | nick_rad = asin((double) AccN / 125.0); |
||
122 | } |
||
123 | if(AccR > 125) roll_rad = M_PI / 2; |
||
124 | else |
||
125 | if(AccR < -125) roll_rad = -M_PI / 2; |
||
126 | else |
||
127 | { |
||
128 | roll_rad = asin((double) AccR / 125.0); |
||
129 | } |
||
130 | ExternData.Winkel[0] = (nick_rad * 1800.0) / M_PI; |
||
131 | ExternData.Winkel[1] = (roll_rad * 1800.0) / M_PI; |
||
132 | ExternData.Orientation = 1; |
||
133 | } |
||
134 | else |
||
135 | { |
||
136 | nick_rad = ((double)ExternData.Winkel[0]) * M_PI / (double)(1800); |
||
137 | roll_rad = ((double)ExternData.Winkel[1]) * M_PI / (double)(1800); |
||
138 | } |
||
1 | ingob | 139 | |
140 | Cx = MagnetN; |
||
141 | Cy = MagnetR; |
||
7 | hbuss | 142 | Cz = MagnetZ; |
143 | |||
144 | if(ExternData.Orientation == 1) |
||
145 | { |
||
146 | Cx = MagnetR; |
||
147 | Cy = -MagnetN; |
||
148 | Cz = MagnetZ; |
||
149 | } |
||
150 | |||
1 | ingob | 151 | Hx = Cx * (double)cos(nick_rad) + |
152 | Cy * (double)sin(nick_rad) * (double)sin(roll_rad) - |
||
153 | Cz * (double)sin(nick_rad) * (double)cos(roll_rad); |
||
154 | |||
155 | Hy = Cy * (double)cos(roll_rad) + |
||
156 | Cz * (double)sin(roll_rad); |
||
157 | |||
158 | |||
159 | if(Hx == 0 && Hy < 0) heading = 90; |
||
160 | else if(Hx == 0 && Hy > 0) heading = 270; |
||
7 | hbuss | 161 | else if(Hx < 0) heading = 180 - (atan(Hy / Hx) * 180.0) / M_PI; |
162 | else if(Hx > 0 && Hy < 0) heading = - (atan(Hy / Hx) * 180.0) / M_PI; |
||
163 | else if(Hx > 0 && Hy > 0) heading = 360 - (atan(Hy / Hx) * 180.0) / M_PI; |
||
1 | ingob | 164 | |
7 | hbuss | 165 | if(abs(heading) < 361) Heading = heading; |
166 | PwmHeading = Heading + 10; |
||
1 | ingob | 167 | } |
168 | |||
169 | |||
7 | hbuss | 170 | void Calibrate(void) |
171 | { |
||
172 | unsigned char cal; |
||
173 | if(I2C_WriteCal.CalByte) cal = I2C_WriteCal.CalByte; |
||
16 | holgerb | 174 | else |
175 | if(ExternCalState) cal = ExternCalState; |
||
7 | hbuss | 176 | else cal = ExternData.CalState; |
16 | holgerb | 177 | |
7 | hbuss | 178 | switch(cal) |
179 | { |
||
180 | case 0: |
||
181 | LED_ON; |
||
182 | break; |
||
183 | case 1: |
||
184 | Xmin = 10000; |
||
185 | Xmax = -10000; |
||
186 | Ymin = 10000; |
||
187 | Ymax = -10000; |
||
188 | Zmin = 10000; |
||
189 | Zmax = -10000; |
||
16 | holgerb | 190 | OffsetAccN = RawAccN; |
191 | OffsetAccR = RawAccR; |
||
192 | OffsetAccZ = RawAccZ; |
||
7 | hbuss | 193 | LED_OFF; |
194 | break; |
||
195 | case 2: |
||
196 | LED_ON; // find Min and Max of the X- and Y-Sensors |
||
197 | if(UncalMagnetN < Xmin) Xmin = UncalMagnetN; |
||
198 | if(UncalMagnetN > Xmax) Xmax = UncalMagnetN; |
||
199 | if(UncalMagnetR < Ymin) Ymin = UncalMagnetR; |
||
200 | if(UncalMagnetR > Ymax) Ymax = UncalMagnetR; |
||
201 | break; |
||
202 | case 3: |
||
203 | LED_OFF; |
||
204 | break; |
||
205 | case 4: |
||
206 | LED_ON; // find Min and Max of the Z-Sensor |
||
207 | if(UncalMagnetZ < Zmin) Zmin = UncalMagnetZ; |
||
208 | if(UncalMagnetZ > Zmax) Zmax = UncalMagnetZ; |
||
209 | break; |
||
210 | case 5: |
||
211 | LED_OFF; // Save values |
||
212 | if((Xmax - Xmin) > 150 && (Ymax - Ymin) > 150 && (Zmax - Zmin) > 150) |
||
213 | { |
||
214 | eeprom_write_word(&eeXmin, Xmin); |
||
215 | eeprom_write_word(&eeXmax, Xmax); |
||
216 | eeprom_write_word(&eeYmin, Ymin); |
||
217 | eeprom_write_word(&eeYmax, Ymax); |
||
218 | eeprom_write_word(&eeZmin, Zmin); |
||
219 | eeprom_write_word(&eeZmax, Zmax); |
||
16 | holgerb | 220 | eeprom_write_word(&eeNoffset, OffsetAccN); |
221 | eeprom_write_word(&eeRoffset, OffsetAccR); |
||
222 | eeprom_write_word(&eeZoffset, OffsetAccZ); |
||
7 | hbuss | 223 | Delay_ms(2000); |
224 | } |
||
16 | holgerb | 225 | ExternCalState = 0; |
7 | hbuss | 226 | LED_ON; |
227 | break; |
||
228 | } |
||
229 | } |
||
1 | ingob | 230 | |
7 | hbuss | 231 | |
232 | void SetDebugValues(void) |
||
233 | { |
||
234 | DebugOut.Analog[0] = MagnetN; |
||
235 | DebugOut.Analog[1] = MagnetR; |
||
236 | DebugOut.Analog[2] = MagnetZ; |
||
237 | DebugOut.Analog[3] = UncalMagnetN; |
||
238 | DebugOut.Analog[4] = UncalMagnetR; |
||
239 | DebugOut.Analog[5] = UncalMagnetZ; |
||
240 | DebugOut.Analog[6] = ExternData.Winkel[0]; |
||
241 | DebugOut.Analog[7] = ExternData.Winkel[1]; |
||
242 | DebugOut.Analog[8] = Xmin; |
||
243 | DebugOut.Analog[9] = Xmax; |
||
244 | DebugOut.Analog[10] = Ymin; |
||
245 | DebugOut.Analog[11] = Ymax; |
||
246 | DebugOut.Analog[12] = Zmin; |
||
247 | DebugOut.Analog[13] = Zmax; |
||
16 | holgerb | 248 | DebugOut.Analog[14] = Heading; |
249 | if(!ExternCalState) DebugOut.Analog[15] = ExternData.CalState; |
||
250 | else DebugOut.Analog[15] = ExternCalState; |
||
7 | hbuss | 251 | DebugOut.Analog[16] = ExternData.UserParameter[0]; |
252 | DebugOut.Analog[17] = ExternData.UserParameter[1]; |
||
16 | holgerb | 253 | DebugOut.Analog[18] = AccN; |
254 | DebugOut.Analog[19] = AccR; |
||
255 | DebugOut.Analog[20] = AccZ; |
||
256 | DebugOut.Analog[21] = RawAccN; |
||
257 | DebugOut.Analog[22] = RawAccR; |
||
258 | DebugOut.Analog[23] = RawAccZ; |
||
259 | DebugOut.Analog[24] = OffsetAccN; |
||
260 | DebugOut.Analog[25] = OffsetAccR; |
||
261 | DebugOut.Analog[26] = OffsetAccZ; |
||
7 | hbuss | 262 | } |
263 | |||
16 | holgerb | 264 | void AccMeasurement(void) |
265 | { |
||
266 | if(AccPresent) |
||
267 | { |
||
268 | RawAccN = (RawAccN + (signed int)MessAD(2))/2; |
||
269 | RawAccR = (RawAccR + (signed int)MessAD(3))/2; |
||
270 | RawAccZ = (RawAccZ + (signed int)MessAD(6))/2; |
||
271 | } |
||
272 | else |
||
273 | { |
||
274 | RawAccN = 0; |
||
275 | RawAccR = 0; |
||
276 | RawAccZ = 0; |
||
277 | } |
||
278 | AccR = (((signed int) RawAccN - OffsetAccN) + AccR * 7) / 8; |
||
279 | AccN = (((signed int) RawAccR - OffsetAccR) + AccN * 7) / 8; |
||
280 | AccZ = ((OffsetAccZ - (signed int) RawAccZ) + AccZ * 7) / 8; |
||
281 | } |
||
7 | hbuss | 282 | |
1 | ingob | 283 | //############################################################################ |
284 | //Hauptprogramm |
||
285 | int main (void) |
||
286 | //############################################################################ |
||
287 | { |
||
16 | holgerb | 288 | char j3_state = 0, debounce = 0; |
289 | DDRC = 0x00; |
||
290 | PORTC = 0x0c; |
||
291 | DDRD = 0xfe; |
||
292 | PORTD = 0xA8; |
||
1 | ingob | 293 | DDRB = 0x04; |
294 | PORTB = 0x35; |
||
16 | holgerb | 295 | |
1 | ingob | 296 | LED_ON; |
297 | |||
298 | UART_Init(); |
||
299 | Timer0_Init(); |
||
300 | ADC_Init(); |
||
3 | ingob | 301 | InitIC2_Slave(); |
1 | ingob | 302 | sei();//Globale Interrupts Einschalten |
303 | Debug_Timer = SetDelay(100); // Sendeintervall |
||
3 | ingob | 304 | |
7 | hbuss | 305 | Xmin = eeprom_read_word(&eeXmin); |
306 | Xmax = eeprom_read_word(&eeXmax); |
||
307 | Ymin = eeprom_read_word(&eeYmin); |
||
308 | Ymax = eeprom_read_word(&eeYmax); |
||
309 | Zmin = eeprom_read_word(&eeZmin); |
||
310 | Zmax = eeprom_read_word(&eeZmax); |
||
16 | holgerb | 311 | OffsetAccN = eeprom_read_word(&eeNoffset); |
312 | OffsetAccR = eeprom_read_word(&eeRoffset); |
||
313 | OffsetAccZ = eeprom_read_word(&eeZoffset); |
||
3 | ingob | 314 | VersionInfo.Hauptversion = VERSION_HAUPTVERSION; |
315 | VersionInfo.Nebenversion = VERSION_NEBENVERSION; |
||
1 | ingob | 316 | VersionInfo.PCKompatibel = 7; |
317 | |||
7 | hbuss | 318 | ExternData.Orientation = 0; |
319 | ExternData.CalState = 0; |
||
320 | I2C_WriteCal.CalByte = 0; |
||
16 | holgerb | 321 | ExternCalState = 0; |
322 | Delay_ms(10); |
||
323 | RawAccR = MessAD(2); |
||
324 | RawAccN = MessAD(3); |
||
325 | if(RawAccR > 1000 && RawAccN > 1000) AccPresent = 0; else AccPresent = 1; |
||
326 | if(AccPresent) |
||
327 | { |
||
328 | uart_putchar ('A');uart_putchar ('C');uart_putchar ('C');uart_putchar ('\n'); |
||
329 | } |
||
330 | PORTC &= ~0x0c; |
||
331 | Delay_ms(100); |
||
332 | if(!PIN_J3) { ExternCalState++; j3_state = 1;} |
||
333 | RawAccR = MessAD(2); |
||
334 | RawAccN = MessAD(3); |
||
335 | RawAccZ = MessAD(6); |
||
1 | ingob | 336 | while (1) |
337 | { |
||
338 | FLIP_LOW; |
||
339 | Delay_ms(2); |
||
16 | holgerb | 340 | RawMagnet1b = MessAD(0); |
341 | RawMagnet2b = -MessAD(1); |
||
342 | RawMagnet3b = MessAD(7); |
||
343 | AccMeasurement(); |
||
1 | ingob | 344 | Delay_ms(1); |
7 | hbuss | 345 | |
1 | ingob | 346 | FLIP_HIGH; |
347 | Delay_ms(2); |
||
348 | RawMagnet1b = MessAD(0); |
||
7 | hbuss | 349 | RawMagnet2b = -MessAD(1); |
1 | ingob | 350 | RawMagnet3b = MessAD(7); |
16 | holgerb | 351 | AccMeasurement(); |
1 | ingob | 352 | Delay_ms(1); |
353 | |||
354 | CalcFields(); |
||
16 | holgerb | 355 | if(ExternData.CalState || I2C_WriteCal.CalByte || ExternCalState) Calibrate(); |
7 | hbuss | 356 | else CalcHeading(); |
357 | BearbeiteRxDaten(); |
||
1 | ingob | 358 | |
16 | holgerb | 359 | if(ExternCalState) |
360 | if(!debounce--) |
||
361 | { |
||
362 | if(!PIN_J3) { if(!j3_state) ExternCalState++; j3_state = 1;} else j3_state = 0; |
||
363 | debounce = 20; |
||
364 | } |
||
365 | |||
1 | ingob | 366 | if(PC_Connected) |
367 | { |
||
7 | hbuss | 368 | DDRD |= 0x02; // TXD-Portpin |
369 | UCR |= (1 << TXEN); |
||
370 | DatenUebertragung(); |
||
1 | ingob | 371 | PC_Connected--; |
372 | } |
||
373 | else |
||
374 | { |
||
7 | hbuss | 375 | UCR &= ~(1 << TXEN); |
376 | DDRD &= ~0x02; // TXD-Portpin |
||
1 | ingob | 377 | } |
7 | hbuss | 378 | } // while(1) |
1 | ingob | 379 | } |
380 |