Rev 373 | 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; |