Rev 373 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 373 | Rev 378 | ||
---|---|---|---|
Line 13... | Line 13... | ||
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 | */ |
Line 15... | Line 15... | ||
15 | 15 | ||
Line 16... | Line 16... | ||
16 | #include "main.h" |
16 | #include "main.h" |
Line 17... | Line 17... | ||
17 | 17 | ||
18 | struct MM3_calib_struct ee_calib EEMEM; |
18 | struct MM3_calib_struct ee_calib EEMEM; // Reservierung im EEPROM |
Line 19... | Line 19... | ||
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 |
Line 26... | Line 26... | ||
26 | void init_MM3(void) |
26 | void init_MM3(void) |
Line 27... | Line 27... | ||
27 | //############################################################################ |
27 | //############################################################################ |
Line 58... | Line 58... | ||
58 | 58 | ||
59 | if (MM3.AXIS == MM3_X) SPDR = 0x31; // Schreiben ins SPDR löst automatisch Übertragung (MOSI und MISO) aus |
59 | if (MM3.AXIS == MM3_X) SPDR = 0x31; // Schreiben ins SPDR löst automatisch Übertragung (MOSI und MISO) aus |
60 | else if (MM3.AXIS == MM3_Y) SPDR = 0x32; // Micromag Period Select ist auf 256 (0x30) |
60 | else if (MM3.AXIS == MM3_Y) SPDR = 0x32; // Micromag Period Select ist auf 256 (0x30) |
Line 61... | Line 61... | ||
61 | else if (MM3.AXIS == MM3_Z) SPDR = 0x33; // 1: x-Achse, 2: Y-Achse, 3: Z-Achse |
61 | else if (MM3.AXIS == MM3_Z) SPDR = 0x33; // 1: x-Achse, 2: Y-Achse, 3: Z-Achse |
62 | 62 | ||
63 | MM3.DRDY = SetDelay(8); // Laut Datenblatt max. Zeit bis Messung fertig (bei PS 256 eigentlich 4 ms) |
63 | MM3.DRDY = SetDelay(5); // Laut Datenblatt max. Zeit bis Messung fertig (bei PS 256 eigentlich 4 ms) |
Line 64... | Line 64... | ||
64 | MM3.STATE = MM3_WAIT_DRDY; |
64 | MM3.STATE = MM3_WAIT_DRDY; |
65 | return; |
65 | return; |
66 | 66 | ||
67 | case MM3_WAIT_DRDY: |
- | |
68 | if (CheckDelay(MM3.DRDY)) {SPDR = 0x00;MM3.STATE = MM3_DRDY;} // Irgendwas ins SPDR, damit Übertragung ausgelöst wird, wenn Wartezeit vorbei |
- | |
69 | return; // Jetzt gehts weiter in SIGNAL (SIG_SPI) |
- | |
70 | /* |
- | |
71 | case MM3_TILT: // Zeitnahe Speicherung der aktuellen Neigung in ° |
- | |
72 | MM3.NickGrad = IntegralNick/(EE_Parameter.UserParam1*8); |
- | |
73 | MM3.RollGrad = IntegralRoll/(EE_Parameter.UserParam2*8); |
- | |
74 | |
- | |
75 | MM3.AXIS = MM3_X; |
- | |
76 | MM3.STATE = MM3_RESET; |
67 | case MM3_WAIT_DRDY: |
77 | return; |
68 | if (CheckDelay(MM3.DRDY)) {SPDR = 0x00;MM3.STATE = MM3_DRDY;} // Irgendwas ins SPDR, damit Übertragung ausgelöst wird, wenn Wartezeit vorbei |
Line 78... | Line 69... | ||
78 | */ |
69 | return; // Jetzt gehts weiter in SIGNAL (SIG_SPI) |
Line 166... | Line 157... | ||
166 | 157 | ||
167 | if (!beeper) |
158 | if (!beeper) |
168 | { |
159 | { |
169 | ROT_FLASH; |
160 | ROT_FLASH; |
170 | GRN_FLASH; |
161 | GRN_FLASH; |
171 | //beeptime = 50; |
162 | beeptime = 50; |
172 | beeper = 50; |
163 | beeper = 50; |
173 | } |
164 | } |
Line 174... | Line 165... | ||
174 | beeper--; |
165 | beeper--; |
Line 179... | Line 170... | ||
179 | 170 | ||
180 | // Wenn Gas zurück genommen wird, Kalibrierung mit 1/2 Sekunde Verzögerung beenden |
171 | // Wenn Gas zurück genommen wird, Kalibrierung mit 1/2 Sekunde Verzögerung beenden |
181 | if (PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] < 100) measurement--; |
172 | if (PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] < 100) measurement--; |
Line 182... | Line 173... | ||
182 | } |
173 | } |
183 | 174 | ||
184 | // Offset der Achsen berechnen |
175 | // Wertebereich der Achsen |
185 | MM3_calib.X_off = (x_max + x_min) / 2; |
176 | MM3_calib.X_range = (x_max - x_min); |
- | 177 | MM3_calib.Y_range = (y_max - y_min); |
|
- | 178 | MM3_calib.Z_range = (z_max - z_min); |
|
- | 179 | ||
- | 180 | // Offset der Achsen |
|
- | 181 | MM3_calib.X_off = (MM3_calib.X_range / 2) - x_max; |
|
186 | MM3_calib.Y_off = (y_max + y_min) / 2; |
182 | MM3_calib.Y_off = (MM3_calib.Y_range / 2) - y_max; |
187 | MM3_calib.Z_off = (z_max + z_min) / 2; |
183 | MM3_calib.Z_off = (MM3_calib.Z_range / 2) - z_max; |
188 | 184 | ||
189 | // und im EEProm abspeichern |
- | |
190 | eeprom_write_block(&MM3_calib,&ee_calib,sizeof(struct MM3_calib_struct)); |
185 | // und im EEProm abspeichern |
Line 191... | Line 186... | ||
191 | 186 | eeprom_write_block(&MM3_calib,&ee_calib,sizeof(struct MM3_calib_struct)); |
|
192 | } |
187 | } |
193 | 188 | ||
194 | 189 | ||
195 | //############################################################################ |
190 | //############################################################################ |
196 | // Neigungskompensierung und Berechnung der Ausrichtung |
191 | // Neigungskompensierung und Berechnung der Ausrichtung |
197 | signed int heading_MM3(void) |
192 | signed int heading_MM3(void) |
198 | //############################################################################ |
193 | //############################################################################ |
- | 194 | { |
|
199 | { |
195 | float sin_nick, cos_nick, sin_roll, cos_roll; |
200 | float sin_nick, cos_nick, sin_roll, cos_roll; |
196 | float x_corr, y_corr; |
201 | signed int x_corr, y_corr, heading; |
- | |
Line 202... | Line 197... | ||
202 | signed int x_axis,y_axis,z_axis; |
197 | signed int x_axis,y_axis,z_axis, heading; |
- | 198 | unsigned int div_faktor; |
|
203 | 199 | ||
204 | MM3.NickGrad = -(IntegralNick/(EE_Parameter.UserParam1*8)); |
200 | div_faktor = (uint16_t)EE_Parameter.UserParam1 * 8; |
- | 201 | ||
- | 202 | // Berechung von sinus und cosinus |
|
205 | MM3.RollGrad = -(IntegralRoll/(EE_Parameter.UserParam2*8)); |
203 | MM3.NickGrad = -(IntegralNick/div_faktor); |
206 | 204 | sin_nick = sin_f(MM3.NickGrad); |
|
- | 205 | cos_nick = cos_f(MM3.NickGrad); |
|
- | 206 | ||
- | 207 | MM3.RollGrad = -(IntegralRoll/div_faktor); |
|
- | 208 | sin_roll = sin_f(MM3.RollGrad); |
|
- | 209 | cos_roll = cos_f(MM3.RollGrad); |
|
Line -... | Line 210... | ||
- | 210 | ||
- | 211 | // Offset |
|
- | 212 | x_axis = (MM3.x_axis + MM3_calib.X_off); |
|
- | 213 | y_axis = (MM3.y_axis + MM3_calib.Y_off); |
|
207 | // Berechung von sinus und cosinus |
214 | z_axis = (MM3.z_axis + MM3_calib.Z_off); |
- | 215 | ||
- | 216 | // Normierung Wertebereich |
|
- | 217 | if ((MM3_calib.X_range > MM3_calib.Y_range) && (MM3_calib.X_range > MM3_calib.Z_range)) |
|
208 | sin_nick = sin_f(MM3.NickGrad); |
218 | { |
209 | cos_nick = cos_f(MM3.NickGrad); |
219 | y_axis = ((long)y_axis * MM3_calib.X_range) / MM3_calib.Y_range; |
- | 220 | z_axis = ((long)z_axis * MM3_calib.X_range) / MM3_calib.Z_range; |
|
- | 221 | } |
|
- | 222 | else if ((MM3_calib.Y_range > MM3_calib.X_range) && (MM3_calib.Y_range > MM3_calib.Z_range)) |
|
- | 223 | { |
|
210 | sin_roll = sin_f(MM3.RollGrad); |
224 | x_axis = ((long)x_axis * MM3_calib.Y_range) / MM3_calib.X_range; |
- | 225 | z_axis = ((long)z_axis * MM3_calib.Y_range) / MM3_calib.Z_range; |
|
- | 226 | } |
|
- | 227 | else //if ((MM3_calib.Z_range > MM3_calib.X_range) && (MM3_calib.Z_range > MM3_calib.Y_range)) |
|
- | 228 | { |
|
- | 229 | x_axis = ((long)x_axis * MM3_calib.Z_range) / MM3_calib.X_range; |
|
Line 211... | Line 230... | ||
211 | cos_roll = cos_f(MM3.RollGrad); |
230 | y_axis = ((long)y_axis * MM3_calib.Z_range) / MM3_calib.Y_range; |
- | 231 | } |
|
212 | 232 | ||
213 | // Offset der Achsen nur bei Bedarf (also hier) berücksichtigen |
233 | DebugOut.Analog[9] = x_axis; |
- | 234 | DebugOut.Analog[10] = y_axis; |
|
- | 235 | DebugOut.Analog[11] = z_axis; |
|
- | 236 | ||
Line 214... | Line 237... | ||
214 | x_axis = -(MM3.x_axis - MM3_calib.X_off); |
237 | // Neigungskompensation |
215 | y_axis = -(MM3.y_axis - MM3_calib.Y_off); |
238 | x_corr = x_axis * cos_nick; |
Line 216... | Line 239... | ||
216 | z_axis = -(MM3.z_axis - MM3_calib.Z_off); |
239 | x_corr += y_axis * sin_roll * sin_nick; |