Rev 308 | Only display areas with differences | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 308 | Rev 309 | ||
---|---|---|---|
1 | /* |
1 | /* |
2 | 2 | ||
3 | Copyright 2007, Niklas Nold |
3 | Copyright 2007, Niklas Nold |
4 | 4 | ||
5 | This program (files compass.c and compass.h) is free software; you can redistribute it and/or modify |
5 | This program (files compass.c and compass.h) is free software; you can redistribute it and/or modify |
6 | it under the terms of the GNU Lesser General Public License as published by the Free Software Foundation; |
6 | it under the terms of the GNU Lesser General Public License as published by the Free Software Foundation; |
7 | either version 3 of the License, or (at your option) any later version. |
7 | either version 3 of the License, or (at your option) any later version. |
8 | This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; |
8 | This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; |
9 | without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
9 | without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
10 | GNU Lesser General Public License for more details. You should have received a copy of the GNU Lesser General Public License |
10 | GNU Lesser General Public License for more details. You should have received a copy of the GNU Lesser General Public License |
11 | along with this program. If not, see <http://www.gnu.org/licenses/>. |
11 | along with this program. If not, see <http://www.gnu.org/licenses/>. |
12 | 12 | ||
13 | Please note: All the other files for the project "Mikrokopter" by H. Buss are under the license (license_buss.txt) published by www.mikrokopter.de |
13 | Please note: All the other files for the project "Mikrokopter" by H. Buss are under the license (license_buss.txt) published by www.mikrokopter.de |
14 | */ |
14 | */ |
15 | 15 | ||
16 | #include "main.h" |
16 | #include "main.h" |
17 | 17 | ||
18 | MM3_struct MM3; |
18 | MM3_struct MM3; |
19 | 19 | ||
20 | 20 | ||
21 | //############################################################################ |
21 | //############################################################################ |
22 | //Initialisierung der SPI-Schnittstelle |
22 | //Initialisierung der SPI-Schnittstelle |
23 | void init_spi(void) |
23 | void init_spi(void) |
24 | //############################################################################ |
24 | //############################################################################ |
25 | { |
25 | { |
26 | SPCR = (1<<SPIE)|(1<<SPE)|(1<<MSTR)|(1<<SPR1)|(1<<SPR0); //Interrupt an, Master, 156 kHz Oszillator |
26 | SPCR = (1<<SPIE)|(1<<SPE)|(1<<MSTR)|(1<<SPR1)|(1<<SPR0); //Interrupt an, Master, 156 kHz Oszillator |
27 | //SPSR = (1<<SPI2X); |
27 | //SPSR = (1<<SPI2X); |
28 | 28 | ||
29 | DDRB |= (1<<PB7)|(1<<PB5)|(1<<PB2); // J8, MOSI, SCK Ausgang |
29 | DDRB |= (1<<PB7)|(1<<PB5)|(1<<PB2); // J8, MOSI, SCK Ausgang |
30 | 30 | ||
31 | PORTD &= ~(1<<PD3); // J5 auf Low |
31 | PORTD &= ~(1<<PD3); // J5 auf Low |
32 | 32 | ||
33 | MM3.AXIS = MM3_X; |
33 | MM3.AXIS = MM3_X; |
34 | MM3.STATE = MM3_RESET; |
34 | MM3.STATE = MM3_RESET; |
35 | } |
35 | } |
36 | 36 | ||
37 | 37 | ||
38 | //############################################################################ |
38 | //############################################################################ |
39 | //Wird in der SIGNAL (SIG_OVERFLOW0) aufgerufen |
39 | //Wird in der SIGNAL (SIG_OVERFLOW0) aufgerufen |
40 | void MM3_timer0(void) |
40 | void MM3_timer0(void) |
41 | //############################################################################ |
41 | //############################################################################ |
42 | { |
42 | { |
43 | switch (MM3.STATE) |
43 | switch (MM3.STATE) |
44 | { |
44 | { |
45 | case MM3_RESET: |
45 | case MM3_RESET: |
46 | PORTB |= (1<<PB2); // J8 auf High, MM3 Reset |
46 | PORTB |= (1<<PB2); // J8 auf High, MM3 Reset |
47 | MM3.STATE = MM3_START_TRANSFER; |
47 | MM3.STATE = MM3_START_TRANSFER; |
48 | return; |
48 | return; |
49 | 49 | ||
50 | case MM3_START_TRANSFER: |
50 | case MM3_START_TRANSFER: |
51 | PORTB &= ~(1<<PB2); // J8 auf Low (war ~125 µs auf High) |
51 | PORTB &= ~(1<<PB2); // J8 auf Low (war ~125 µs auf High) |
52 | 52 | ||
53 | if (MM3.AXIS == MM3_X) SPDR = 0x51; // Schreiben ins SPDR löst automatisch Übertragung (MOSI und MISO) aus |
53 | if (MM3.AXIS == MM3_X) SPDR = 0x51; // Schreiben ins SPDR löst automatisch Übertragung (MOSI und MISO) aus |
54 | else if (MM3.AXIS == MM3_Y) SPDR = 0x52; // Micromag Period Select ist auf 1024 (0x50) |
54 | else if (MM3.AXIS == MM3_Y) SPDR = 0x52; // Micromag Period Select ist auf 1024 (0x50) |
55 | else if (MM3.AXIS == MM3_Z) SPDR = 0x53; // 1: x-Achse, 2: Y-Achse, 3: Z-Achse |
55 | else if (MM3.AXIS == MM3_Z) SPDR = 0x53; // 1: x-Achse, 2: Y-Achse, 3: Z-Achse |
56 | 56 | ||
57 | MM3.DRDY = SetDelay(15); // Laut Datenblatt max. Zeit bis Messung fertig (bei PS 1024) |
57 | MM3.DRDY = SetDelay(15); // Laut Datenblatt max. Zeit bis Messung fertig (bei PS 1024) |
58 | MM3.STATE = MM3_WAIT_DRDY; |
58 | MM3.STATE = MM3_WAIT_DRDY; |
59 | return; |
59 | return; |
60 | 60 | ||
61 | case MM3_WAIT_DRDY: |
61 | case MM3_WAIT_DRDY: |
62 | if (CheckDelay(MM3.DRDY)) {SPDR = 0x00;MM3.STATE = MM3_DRDY;} // Irgendwas ins SPDR, damit Übertragung ausgelöst wird, wenn Wartezeit vorbei |
62 | if (CheckDelay(MM3.DRDY)) {SPDR = 0x00;MM3.STATE = MM3_DRDY;} // Irgendwas ins SPDR, damit Übertragung ausgelöst wird, wenn Wartezeit vorbei |
63 | return; // Jetzt gehts weiter in SIGNAL (SIG_SPI) |
63 | return; // Jetzt gehts weiter in SIGNAL (SIG_SPI) |
64 | 64 | ||
65 | case MM3_TILT: // Zeitnahe Speicherung der aktuellen Neigung in ° |
65 | case MM3_TILT: // Zeitnahe Speicherung der aktuellen Neigung in ° |
66 | MM3.NickGrad = asin_i((float)Aktuell_ax/220*200); |
66 | MM3.NickGrad = asin_i((float)Aktuell_ax/220*200); |
67 | MM3.RollGrad = asin_i((float)Aktuell_ay/220*200); |
67 | MM3.RollGrad = asin_i((float)Aktuell_ay/220*200); |
68 | 68 | ||
69 | MM3.AXIS = MM3_X; |
69 | MM3.AXIS = MM3_X; |
70 | MM3.STATE = MM3_RESET; |
70 | MM3.STATE = MM3_RESET; |
71 | return; |
71 | return; |
72 | } |
72 | } |
73 | } |
73 | } |
74 | 74 | ||
75 | 75 | ||
76 | //############################################################################ |
76 | //############################################################################ |
77 | //SPI byte ready |
77 | //SPI byte ready |
78 | SIGNAL (SIG_SPI) |
78 | SIGNAL (SIG_SPI) |
79 | //############################################################################ |
79 | //############################################################################ |
80 | { |
80 | { |
81 | switch (MM3.STATE) |
81 | switch (MM3.STATE) |
82 | { |
82 | { |
83 | case MM3_DRDY: // 1. Byte ist da, abspeichern, an die MSB-Stelle rücken |
83 | case MM3_DRDY: // 1. Byte ist da, abspeichern, an die MSB-Stelle rücken |
84 | if (MM3.AXIS == MM3_X) |
84 | if (MM3.AXIS == MM3_X) |
85 | { |
85 | { |
86 | MM3.x_axis = SPDR; |
86 | MM3.x_axis = SPDR; |
87 | MM3.x_axis <<= 8; |
87 | MM3.x_axis <<= 8; |
88 | } |
88 | } |
89 | else if (MM3.AXIS == MM3_Y) |
89 | else if (MM3.AXIS == MM3_Y) |
90 | { |
90 | { |
91 | MM3.y_axis = SPDR; |
91 | MM3.y_axis = SPDR; |
92 | MM3.y_axis <<= 8; |
92 | MM3.y_axis <<= 8; |
93 | } |
93 | } |
94 | else // if (MM3.AXIS == MM3_Z) |
94 | else // if (MM3.AXIS == MM3_Z) |
95 | { |
95 | { |
96 | MM3.z_axis = SPDR; |
96 | MM3.z_axis = SPDR; |
97 | MM3.z_axis <<= 8; |
97 | MM3.z_axis <<= 8; |
98 | } |
98 | } |
99 | 99 | ||
100 | SPDR=0x00; // Übertragung von 2. Byte auslösen |
100 | SPDR=0x00; // Übertragung von 2. Byte auslösen |
101 | MM3.STATE=MM3_BYTE2; |
101 | MM3.STATE=MM3_BYTE2; |
102 | return; |
102 | return; |
103 | 103 | ||
104 | case MM3_BYTE2: // 2. Byte der entsprechenden Achse ist da |
104 | case MM3_BYTE2: // 2. Byte der entsprechenden Achse ist da |
105 | if (MM3.AXIS == MM3_X) |
105 | if (MM3.AXIS == MM3_X) |
106 | { |
106 | { |
107 | MM3.x_axis |= SPDR; |
107 | MM3.x_axis |= SPDR; |
108 | MM3.x_axis -= OFF_X; // Sofort Offset aus der Kalibrierung berücksichtigen |
108 | MM3.x_axis -= OFF_X; // Sofort Offset aus der Kalibrierung berücksichtigen |
109 | MM3.AXIS = MM3_Y; |
109 | MM3.AXIS = MM3_Y; |
110 | MM3.STATE = MM3_RESET; |
110 | MM3.STATE = MM3_RESET; |
111 | } |
111 | } |
112 | else if (MM3.AXIS == MM3_Y) |
112 | else if (MM3.AXIS == MM3_Y) |
113 | { |
113 | { |
114 | MM3.y_axis |= SPDR; |
114 | MM3.y_axis |= SPDR; |
115 | MM3.y_axis -= OFF_Y; |
115 | MM3.y_axis -= OFF_Y; |
116 | MM3.AXIS = MM3_Z; |
116 | MM3.AXIS = MM3_Z; |
117 | MM3.STATE = MM3_RESET; |
117 | MM3.STATE = MM3_RESET; |
118 | } |
118 | } |
119 | else // if (MM3.AXIS == MM3_Z) |
119 | else // if (MM3.AXIS == MM3_Z) |
120 | { |
120 | { |
121 | MM3.z_axis |= SPDR; |
121 | MM3.z_axis |= SPDR; |
122 | MM3.z_axis -= OFF_Z; |
122 | MM3.z_axis -= OFF_Z; |
123 | MM3.STATE = MM3_TILT; |
123 | MM3.STATE = MM3_TILT; |
124 | } |
124 | } |
125 | 125 | ||
126 | return; |
126 | return; |
127 | } |
127 | } |
128 | } |
128 | } |
129 | 129 | ||
130 | signed int MM3_heading(void) |
130 | signed int MM3_heading(void) |
131 | { |
131 | { |
132 | float sin_nick, cos_nick, sin_roll, cos_roll; |
132 | float sin_nick, cos_nick, sin_roll, cos_roll; |
133 | signed int x_corr, y_corr; |
133 | signed int x_corr, y_corr; |
134 | signed int heading; |
134 | signed int heading; |
135 | 135 | ||
136 | // Berechung von sinus und cosinus |
136 | // Berechung von sinus und cosinus |
137 | sin_nick = sin_f(MM3.NickGrad); |
137 | sin_nick = sin_f(MM3.NickGrad); |
138 | cos_nick = cos_f(MM3.NickGrad); |
138 | cos_nick = cos_f(MM3.NickGrad); |
139 | sin_roll = sin_f(MM3.RollGrad); |
139 | sin_roll = sin_f(MM3.RollGrad); |
140 | cos_roll = cos_f(MM3.RollGrad); |
140 | cos_roll = cos_f(MM3.RollGrad); |
141 | 141 | ||
142 | // Neigungskompensation |
142 | // Neigungskompensation |
143 | x_corr = (cos_nick * MM3.x_axis) + (((sin_roll * MM3.y_axis) - (cos_roll * MM3.z_axis)) * sin_nick); |
143 | x_corr = (cos_nick * MM3.x_axis) - (((sin_roll * MM3.y_axis) - (cos_roll * MM3.z_axis)) * sin_nick); |
144 | y_corr = ((cos_roll * MM3.y_axis) + (sin_roll * MM3.z_axis)); |
144 | y_corr = ((cos_roll * MM3.y_axis) + (sin_roll * MM3.z_axis)); |
145 | 145 | ||
146 | // Winkelberechnung |
146 | // Winkelberechnung |
147 | heading = atan2_i(x_corr, y_corr); |
147 | heading = atan2_i(x_corr, y_corr); |
148 | 148 | ||
149 | return (heading); |
149 | return (heading); |
150 | } |
150 | } |
151 | 151 |