Rev 851 | Go to most recent revision | Only display areas with differences | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 851 | Rev 857 | ||
---|---|---|---|
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 | struct MM3_calib_struct ee_calib EEMEM; // Reservierung im EEPROM |
18 | struct MM3_calib_struct ee_calib EEMEM; // Reservierung im EEPROM |
19 | 19 | ||
20 | struct MM3_working_struct MM3; |
20 | struct MM3_working_struct MM3; |
21 | struct MM3_calib_struct MM3_calib; |
21 | struct MM3_calib_struct MM3_calib; |
22 | 22 | ||
23 | 23 | ||
24 | //############################################################################ |
24 | //############################################################################ |
25 | // Initialisierung |
25 | // Initialisierung |
26 | void init_MM3(void) |
26 | void init_MM3(void) |
27 | //############################################################################ |
27 | //############################################################################ |
28 | { |
28 | { |
29 | // SPI-Schnittstelle initialisieren |
29 | // SPI-Schnittstelle initialisieren |
30 | SPCR = (1<<SPIE)|(1<<SPE)|(1<<MSTR)|(1<<SPR1)|(1<<SPR0); // Interrupt an, Master, 156 kHz Oszillator |
30 | SPCR = (1<<SPIE)|(1<<SPE)|(1<<MSTR)|(1<<SPR1)|(1<<SPR0); // Interrupt an, Master, 156 kHz Oszillator |
31 | 31 | ||
32 | DDRB |= (1<<PB7)|(1<<PB5); // MOSI, SCK Ausgang |
32 | DDRB |= (1<<PB7)|(1<<PB5); // MOSI, SCK Ausgang |
33 | DDRC |= (1<<PC4)|(1<<PC5); // PC5 (RESET) und PC4 (SSNOT) als Ausgang |
33 | DDRC |= (1<<PC4)|(1<<PC5); // PC5 (RESET) und PC4 (SSNOT) als Ausgang |
34 | PORTC |= (1<<PC4); // PC4 (SSNOT) auf High -> MM3 passiv |
34 | PORTC |= (1<<PC4); // PC4 (SSNOT) auf High -> MM3 passiv |
35 | PORTC &= ~(1<<PC5); // PC5 (RESET) auf Low |
35 | PORTC &= ~(1<<PC5); // PC5 (RESET) auf Low |
36 | 36 | ||
37 | // Init Statemachine |
37 | // Init Statemachine |
38 | MM3.AXIS = MM3_X; |
38 | MM3.AXIS = MM3_X; |
39 | MM3.STATE = MM3_RESET; |
39 | MM3.STATE = MM3_RESET; |
40 | 40 | ||
41 | // Kalibrierung aus dem EEprom lesen |
41 | // Kalibrierung aus dem EEprom lesen |
42 | eeprom_read_block(&MM3_calib,&ee_calib,sizeof(struct MM3_calib_struct)); |
42 | eeprom_read_block(&MM3_calib,&ee_calib,sizeof(struct MM3_calib_struct)); |
43 | } |
43 | } |
44 | 44 | ||
45 | 45 | ||
46 | //############################################################################ |
46 | //############################################################################ |
47 | // Wird in der SIGNAL (SIG_OVERFLOW0) aufgerufen |
47 | // Wird in der SIGNAL (SIG_OVERFLOW0) aufgerufen |
48 | void timer0_MM3(void) |
48 | void timer0_MM3(void) |
49 | //############################################################################ |
49 | //############################################################################ |
50 | { |
50 | { |
51 | switch (MM3.STATE) |
51 | switch (MM3.STATE) |
52 | { |
52 | { |
53 | case MM3_RESET: |
53 | case MM3_RESET: |
54 | PORTC &= ~(1<<PC4); // MM3 aktiv |
54 | PORTC &= ~(1<<PC4); // MM3 aktiv |
55 | PORTC |= (1<<PC5); // MM3 Reset |
55 | PORTC |= (1<<PC5); // MM3 Reset |
56 | MM3.STATE = MM3_START_TRANSFER; |
56 | MM3.STATE = MM3_START_TRANSFER; |
57 | return; |
57 | return; |
58 | 58 | ||
59 | case MM3_START_TRANSFER: |
59 | case MM3_START_TRANSFER: |
60 | PORTC &= ~(1<<PC5); // PC5 auf Low (war ~125 µs auf High) |
60 | PORTC &= ~(1<<PC5); // PC5 auf Low (war ~125 µs auf High) |
61 | 61 | ||
62 | if (MM3.AXIS == MM3_X) SPDR = MM3_PERIOD_512 + MM3_X_AXIS; // Schreiben ins SPDR löst automatisch SPI-Übertragung (MOSI und MISO) aus |
62 | if (MM3.AXIS == MM3_X) SPDR = MM3_PERIOD_512 + MM3_X_AXIS; // Schreiben ins SPDR löst automatisch SPI-Übertragung (MOSI und MISO) aus |
63 | else if (MM3.AXIS == MM3_Y) SPDR = MM3_PERIOD_512 + MM3_Y_AXIS; // Micromag Period Select ist 256 (0x30) |
63 | else if (MM3.AXIS == MM3_Y) SPDR = MM3_PERIOD_512 + MM3_Y_AXIS; // Micromag Period Select ist 256 (0x30) |
64 | else SPDR = MM3_PERIOD_512 + MM3_Z_AXIS; //if (MM3.AXIS == MM3_Z) |
64 | else SPDR = MM3_PERIOD_512 + MM3_Z_AXIS; //if (MM3.AXIS == MM3_Z) |
65 | 65 | ||
66 | MM3.DRDY = SetDelay(10); // Laut Datenblatt max. Zeit bis Messung fertig (bei PS 512 eigentlich 8 ms) |
66 | MM3.DRDY = SetDelay(10); // Laut Datenblatt max. Zeit bis Messung fertig (bei PS 512 eigentlich 8 ms) |
67 | MM3.STATE = MM3_WAIT_DRDY; |
67 | MM3.STATE = MM3_WAIT_DRDY; |
68 | return; |
68 | return; |
69 | 69 | ||
70 | case MM3_WAIT_DRDY: |
70 | case MM3_WAIT_DRDY: |
71 | if (CheckDelay(MM3.DRDY)) {SPDR = 0x00;MM3.STATE = MM3_DRDY;} // Irgendwas ins SPDR, damit Übertragung ausgelöst wird, wenn Wartezeit vorbei |
71 | if (CheckDelay(MM3.DRDY)) {SPDR = 0x00;MM3.STATE = MM3_DRDY;} // Irgendwas ins SPDR, damit Übertragung ausgelöst wird, wenn Wartezeit vorbei |
72 | return; // Jetzt gehts weiter in SIGNAL (SIG_SPI) |
72 | return; // Jetzt gehts weiter in SIGNAL (SIG_SPI) |
73 | } |
73 | } |
74 | } |
74 | } |
75 | 75 | ||
76 | 76 | ||
77 | //############################################################################ |
77 | //############################################################################ |
78 | // SPI byte ready |
78 | // SPI byte ready |
79 | SIGNAL (SIG_SPI) |
79 | SIGNAL (SIG_SPI) |
80 | //############################################################################ |
80 | //############################################################################ |
81 | { |
81 | { |
82 | static char tmp; |
82 | static char tmp; |
83 | int value; |
83 | int value; |
84 | 84 | ||
85 | switch (MM3.STATE) |
85 | switch (MM3.STATE) |
86 | { |
86 | { |
87 | case MM3_DRDY: // 1. Byte ist da, zwischenspeichern |
87 | case MM3_DRDY: // 1. Byte ist da, zwischenspeichern |
88 | tmp = SPDR; |
88 | tmp = SPDR; |
89 | SPDR = 0x00; // Übertragung von 2. Byte auslösen |
89 | SPDR = 0x00; // Übertragung von 2. Byte auslösen |
90 | MM3.STATE = MM3_BYTE2; |
90 | MM3.STATE = MM3_BYTE2; |
91 | return; |
91 | return; |
92 | 92 | ||
93 | case MM3_BYTE2: // 2. Byte der entsprechenden Achse ist da |
93 | case MM3_BYTE2: // 2. Byte der entsprechenden Achse ist da |
94 | value = tmp; |
94 | value = tmp; |
95 | value <<= 8; // 1. Byte an MSB-Stelle rücken |
95 | value <<= 8; // 1. Byte an MSB-Stelle rücken |
96 | value |= SPDR; // 2. Byte dranpappen |
96 | value |= SPDR; // 2. Byte dranpappen |
97 | 97 | ||
98 | if(abs(value) < Max_Axis_Value) // Spikes filtern. Zuweisung nur, wenn Max-Wert nicht überschritten |
98 | if(abs(value) < Max_Axis_Value) // Spikes filtern. Zuweisung nur, wenn Max-Wert nicht überschritten |
99 | switch (MM3.AXIS) |
99 | switch (MM3.AXIS) |
100 | { |
100 | { |
101 | case MM3_X: |
101 | case MM3_X: |
102 | MM3.x_axis = value; |
102 | MM3.x_axis = value; |
103 | MM3.AXIS = MM3_Y; |
103 | MM3.AXIS = MM3_Y; |
104 | break; |
104 | break; |
105 | case MM3_Y: |
105 | case MM3_Y: |
106 | MM3.y_axis = value; |
106 | MM3.y_axis = value; |
107 | MM3.AXIS = MM3_Z; |
107 | MM3.AXIS = MM3_Z; |
108 | break; |
108 | break; |
109 | default: //case MM3_Z: |
109 | default: //case MM3_Z: |
110 | MM3.z_axis = value; |
110 | MM3.z_axis = value; |
111 | MM3.AXIS = MM3_X; |
111 | MM3.AXIS = MM3_X; |
112 | } |
112 | } |
113 | PORTC |= (1<<PC4); // MM3 passiv |
113 | PORTC |= (1<<PC4); // MM3 passiv |
114 | MM3.STATE = MM3_RESET; |
114 | MM3.STATE = MM3_RESET; |
115 | } |
115 | } |
116 | } |
116 | } |
117 | 117 | ||
118 | //############################################################################ |
118 | //############################################################################ |
119 | // Kompass kalibrieren |
119 | // Kompass kalibrieren |
120 | void calib_MM3(void) |
120 | void calib_MM3(void) |
121 | //############################################################################ |
121 | //############################################################################ |
122 | { |
122 | { |
123 | int16_t x_min=0,x_max=0,y_min=0,y_max=0,z_min=0,z_max=0; |
123 | int16_t x_min=0,x_max=0,y_min=0,y_max=0,z_min=0,z_max=0; |
124 | int16_t x_axis, y_axis, z_axis; |
124 | int16_t x_axis, y_axis, z_axis; |
125 | uint8_t measurement=50,beeper=0; |
125 | uint8_t measurement=50,beeper=0; |
126 | 126 | ||
127 | GRN_ON; |
127 | GRN_ON; |
128 | ROT_OFF; |
128 | ROT_OFF; |
129 | 129 | ||
130 | while (measurement) |
130 | while (measurement) |
131 | { |
131 | { |
132 | uint8_t tmp_sreg = SREG; |
132 | uint8_t tmp_sreg = SREG; |
133 | cli(); |
133 | cli(); |
134 | x_axis = MM3.x_axis; |
134 | x_axis = MM3.x_axis; |
135 | y_axis = MM3.y_axis; |
135 | y_axis = MM3.y_axis; |
136 | z_axis = MM3.z_axis; |
136 | z_axis = MM3.z_axis; |
137 | SREG = tmp_sreg; |
137 | SREG = tmp_sreg; |
138 | 138 | ||
139 | if (x_axis > x_max) x_max = x_axis; |
139 | if (x_axis > x_max) x_max = x_axis; |
140 | else if (x_axis < x_min) x_min = x_axis; |
140 | else if (x_axis < x_min) x_min = x_axis; |
141 | 141 | ||
142 | if (y_axis > y_max) y_max = y_axis; |
142 | if (y_axis > y_max) y_max = y_axis; |
143 | else if (y_axis < y_min) y_min = y_axis; |
143 | else if (y_axis < y_min) y_min = y_axis; |
144 | 144 | ||
145 | if (z_axis > z_max) z_max = z_axis; |
145 | if (z_axis > z_max) z_max = z_axis; |
146 | else if (z_axis < z_min) z_min = z_axis; |
146 | else if (z_axis < z_min) z_min = z_axis; |
147 | 147 | ||
148 | if (!beeper) |
148 | if (!beeper) |
149 | { |
149 | { |
150 | ROT_FLASH; |
150 | ROT_FLASH; |
151 | GRN_FLASH; |
151 | GRN_FLASH; |
152 | beeptime = 50; |
152 | beeptime = 50; |
153 | beeper = 50; |
153 | beeper = 50; |
154 | } |
154 | } |
155 | beeper--; |
155 | beeper--; |
156 | 156 | ||
157 | // Schleife mit 100 Hz |
157 | // Schleife mit 100 Hz |
158 | Delay_ms(10); |
158 | Delay_ms(10); |
159 | 159 | ||
160 | // Wenn Gas zurück genommen wird, Kalibrierung mit 1/2 Sekunde Verzögerung beenden |
160 | // Wenn Gas zurück genommen wird, Kalibrierung mit 1/2 Sekunde Verzögerung beenden |
161 | if (PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] < 100) measurement--; |
161 | if (PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] < 100) measurement--; |
162 | } |
162 | } |
163 | 163 | ||
164 | // Wertebereich der Achsen |
164 | // Wertebereich der Achsen |
165 | MM3_calib.X_range = (x_max - x_min); |
165 | MM3_calib.X_range = (x_max - x_min); |
166 | MM3_calib.Y_range = (y_max - y_min); |
166 | MM3_calib.Y_range = (y_max - y_min); |
167 | MM3_calib.Z_range = (z_max - z_min); |
167 | MM3_calib.Z_range = (z_max - z_min); |
168 | 168 | ||
169 | // Offset der Achsen |
169 | // Offset der Achsen |
170 | MM3_calib.X_off = (x_max + x_min) /2; |
170 | MM3_calib.X_off = (x_max + x_min) /2; |
171 | MM3_calib.Y_off = (y_max + y_min) /2; |
171 | MM3_calib.Y_off = (y_max + y_min) /2; |
172 | MM3_calib.Z_off = (z_max + z_min) /2; |
172 | MM3_calib.Z_off = (z_max + z_min) /2; |
173 | 173 | ||
174 | // und im EEProm abspeichern |
174 | // und im EEProm abspeichern |
175 | eeprom_write_block(&MM3_calib,&ee_calib,sizeof(struct MM3_calib_struct)); |
175 | eeprom_write_block(&MM3_calib,&ee_calib,sizeof(struct MM3_calib_struct)); |
176 | } |
176 | } |
177 | 177 | ||
178 | 178 | ||
179 | //############################################################################ |
179 | //############################################################################ |
180 | // Neigungskompensierung und Berechnung der Ausrichtung |
180 | // Neigungskompensierung und Berechnung der Ausrichtung |
181 | int heading_MM3(void) |
181 | int heading_MM3(void) |
182 | //############################################################################ |
182 | //############################################################################ |
183 | { |
183 | { |
184 | int16_t sin_nick, cos_nick, sin_roll, cos_roll; |
184 | int16_t sin_nick, cos_nick, sin_roll, cos_roll; |
185 | int16_t mm3_x_axis, mm3_y_axis, mm3_z_axis; |
185 | int16_t mm3_x_axis, mm3_y_axis, mm3_z_axis; |
186 | int32_t Hx, Hy, Hz, x_corr, y_corr; |
186 | int32_t Hx, Hy, Hz, x_corr, y_corr; |
187 | int16_t heading; |
187 | int16_t heading; |
188 | int8_t tilt; |
188 | int8_t tilt; |
189 | 189 | ||
190 | // 16bit-Werte lesen |
190 | // 16bit-Werte lesen |
191 | uint8_t tmp_sreg = SREG; |
191 | uint8_t tmp_sreg = SREG; |
192 | cli(); |
192 | cli(); |
193 | mm3_x_axis = MM3.x_axis; |
193 | mm3_x_axis = MM3.x_axis; |
194 | mm3_y_axis = MM3.y_axis; |
194 | mm3_y_axis = MM3.y_axis; |
195 | mm3_z_axis = MM3.z_axis; |
195 | mm3_z_axis = MM3.z_axis; |
196 | SREG = tmp_sreg; |
196 | SREG = tmp_sreg; |
197 | /* |
197 | |
198 | int temp = Aktuell_az-550; |
198 | int temp = Aktuell_az-550; |
199 | DebugOut.Analog[2] = temp; |
- | |
200 | // Lage-Berechnung mittels Acc-Messwerte |
199 | // Lage-Berechnung mittels Acc-Messwerte |
201 | tilt = atan2_i(temp,AdWertAccNick*64); |
200 | tilt = atan2_i(temp,AdWertAccNick*64); |
202 | DebugOut.Analog[0] = tilt; |
- | |
203 | sin_nick = sin_i(tilt); |
201 | sin_nick = sin_i(tilt); |
204 | cos_nick = cos_i(tilt); |
202 | cos_nick = cos_i(tilt); |
205 | |
203 | |
206 | tilt = atan2_i(temp,AdWertAccRoll*64); |
204 | tilt = atan2_i(temp,AdWertAccRoll*64); |
207 | DebugOut.Analog[1] = tilt; |
- | |
208 | sin_roll = sin_i(tilt); |
205 | sin_roll = sin_i(tilt); |
209 | cos_roll = cos_i(tilt); |
206 | cos_roll = cos_i(tilt); |
- | 207 | ||
210 | */ |
208 | /* |
211 | // Lage-Berechnung mittels Gyro-Integral |
209 | // Lage-Berechnung mittels Gyro-Integral |
212 | uint16_t div_faktor; |
210 | uint16_t div_faktor; |
213 | div_faktor = (uint16_t)EE_Parameter.UserParam3 *8; |
211 | div_faktor = (uint16_t)EE_Parameter.UserParam3 *8; |
214 | 212 | ||
215 | tilt = (IntegralNick /div_faktor); |
213 | tilt = (IntegralNick /div_faktor); |
216 | sin_nick = sin_i(tilt); |
214 | sin_nick = sin_i(tilt); |
217 | cos_nick = cos_i(tilt); |
215 | cos_nick = cos_i(tilt); |
218 | 216 | ||
219 | tilt = (IntegralRoll /div_faktor); |
217 | tilt = (IntegralRoll /div_faktor); |
220 | sin_roll = sin_i(tilt); |
218 | sin_roll = sin_i(tilt); |
221 | cos_roll = cos_i(tilt); |
219 | cos_roll = cos_i(tilt); |
222 | 220 | */ |
|
223 | // Offset und Normalisierung |
221 | // Offset und Normalisierung |
224 | Hx = (((int32_t)(mm3_x_axis - MM3_calib.X_off)) *512) /MM3_calib.X_range; |
222 | Hx = (((int32_t)(mm3_x_axis - MM3_calib.X_off)) *512) /MM3_calib.X_range; |
225 | Hy = (((int32_t)(mm3_y_axis - MM3_calib.Y_off)) *512) /MM3_calib.Y_range; |
223 | Hy = (((int32_t)(mm3_y_axis - MM3_calib.Y_off)) *512) /MM3_calib.Y_range; |
226 | Hz = (((int32_t)(mm3_z_axis - MM3_calib.Z_off)) *512) /MM3_calib.Z_range; |
224 | Hz = (((int32_t)(mm3_z_axis - MM3_calib.Z_off)) *512) /MM3_calib.Z_range; |
227 | 225 | ||
228 | // Neigungskompensierung |
226 | // Neigungskompensierung |
229 | x_corr = Hx * cos_nick; |
227 | x_corr = Hx * cos_nick; |
230 | x_corr -= Hz * sin_nick; |
228 | x_corr -= Hz * sin_nick; |
231 | x_corr /= 1024; |
229 | x_corr /= 1024; |
232 | 230 | ||
233 | y_corr = Hy * cos_roll; |
231 | y_corr = Hy * cos_roll; |
234 | y_corr += Hz * sin_roll; |
232 | y_corr += Hz * sin_roll; |
235 | y_corr /= 16; // atan2_i erwartet y_corr *64. Deshalb /16 und nicht /1024 |
233 | y_corr /= 16; // atan2_i erwartet y_corr *64. Deshalb /16 und nicht /1024 |
236 | 234 | ||
237 | // Winkelberechnung |
235 | // Winkelberechnung |
238 | heading = atan2_i(x_corr, y_corr); |
236 | heading = atan2_i(x_corr, y_corr); |
239 | 237 | ||
240 | // Skalieren von +-180° auf 0-360° |
238 | // Skalieren von +-180° auf 0-360° |
241 | if (heading < 0) heading = -heading; |
239 | if (heading < 0) heading = -heading; |
242 | else heading = 360 - heading; |
240 | else heading = 360 - heading; |
243 | 241 | ||
244 | return (heading); |
242 | return (heading); |
245 | } |
243 | } |
246 | 244 |