Rev 308 | Only display areas with differences | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 308 | Rev 309 | ||
---|---|---|---|
1 | /* |
1 | /* |
2 | This program (files spi.c and spi.h) is free software; you can redistribute it and/or modify |
2 | This program (files spi.c and spi.h) is free software; you can redistribute it and/or modify |
3 | it under the terms of the GNU General Public License as published by the Free Software Foundation; |
3 | it under the terms of the GNU General Public License as published by the Free Software Foundation; |
4 | either version 3 of the License, or (at your option) any later version. |
4 | either version 3 of the License, or (at your option) any later version. |
5 | This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; |
5 | This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; |
6 | without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
6 | without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
7 | GNU General Public License for more details. You should have received a copy of the GNU General Public License |
7 | GNU General Public License for more details. You should have received a copy of the GNU General Public License |
8 | along with this program. If not, see <http://www.gnu.org/licenses/>. |
8 | along with this program. If not, see <http://www.gnu.org/licenses/>. |
9 | 9 | ||
10 | 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 |
10 | 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 |
11 | */ |
11 | */ |
12 | /*++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
12 | /*++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
13 | Patrick Kögel |
13 | Patrick Kögel |
14 | Micromag3 Auswertung beta |
14 | Micromag3 Auswertung beta |
15 | ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/ |
15 | ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/ |
16 | //***************************************************************************** |
16 | //***************************************************************************** |
17 | // |
17 | // |
18 | // File Name : 'spi.c' |
18 | // File Name : 'spi.c' |
19 | // Title : SPI interface driver |
19 | // Title : SPI interface driver |
20 | // Author : Pascal Stang - Copyright (C) 2000-2002 |
20 | // Author : Pascal Stang - Copyright (C) 2000-2002 |
21 | // Created : 11/22/2000 |
21 | // Created : 11/22/2000 |
22 | // Revised : 06/06/2002 |
22 | // Revised : 06/06/2002 |
23 | // Version : 0.6 |
23 | // Version : 0.6 |
24 | // Target MCU : Atmel AVR series |
24 | // Target MCU : Atmel AVR series |
25 | // Editor Tabs : 4 |
25 | // Editor Tabs : 4 |
26 | // |
26 | // |
27 | // NOTE: This code is currently below version 1.0, and therefore is considered |
27 | // NOTE: This code is currently below version 1.0, and therefore is considered |
28 | // to be lacking in some functionality or documentation, or may not be fully |
28 | // to be lacking in some functionality or documentation, or may not be fully |
29 | // tested. Nonetheless, you can expect most functions to work. |
29 | // tested. Nonetheless, you can expect most functions to work. |
30 | // |
30 | // |
31 | // This code is distributed under the GNU Public License |
31 | // This code is distributed under the GNU Public License |
32 | // which can be found at http://www.gnu.org/licenses/gpl.txt |
32 | // which can be found at http://www.gnu.org/licenses/gpl.txt |
33 | // |
33 | // |
34 | //***************************************************************************** |
34 | //***************************************************************************** |
35 | 35 | ||
36 | #include "main.h" |
36 | #include "main.h" |
37 | 37 | ||
38 | 38 | ||
39 | 39 | ||
40 | // Define the SPI_USEINT key if you want SPI bus operation to be |
40 | // Define the SPI_USEINT key if you want SPI bus operation to be |
41 | // interrupt-driven. The primary reason for not using SPI in |
41 | // interrupt-driven. The primary reason for not using SPI in |
42 | // interrupt-driven mode is if the SPI send/transfer commands |
42 | // interrupt-driven mode is if the SPI send/transfer commands |
43 | // will be used from within some other interrupt service routine |
43 | // will be used from within some other interrupt service routine |
44 | // or if interrupts might be globally turned off due to of other |
44 | // or if interrupts might be globally turned off due to of other |
45 | // aspects of your program |
45 | // aspects of your program |
46 | // |
46 | // |
47 | // Comment-out or uncomment this line as necessary |
47 | // Comment-out or uncomment this line as necessary |
48 | //#define SPI_USEINT |
48 | //#define SPI_USEINT |
49 | 49 | ||
50 | // global variables |
50 | // global variables |
51 | volatile unsigned char spiTransferComplete,MessungFertig=0,KompassInUse =0,KompassInUseCal=0,NewX= 0,NewY= 0,NewZ= 0,EEPROMWrite=0; |
51 | volatile unsigned char spiTransferComplete,MessungFertig=0,KompassInUse =0,KompassInUseCal=0,NewX= 0,NewY= 0,NewZ= 0,EEPROMWrite=0; |
52 | volatile signed char nickgrad=0,rollgrad=0,nickgradkomp=0,rollgradkomp=0; |
52 | volatile signed char nickgrad=0,rollgrad=0,nickgradkomp=0,rollgradkomp=0; |
53 | volatile double XE= 0,YE=0,nick=0,roll=0; |
53 | volatile double XE= 0,YE=0,nick=0,roll=0; |
54 | volatile double richtungorgd=0,richtungorgr=0; |
54 | volatile double richtungorgd=0,richtungorgr=0; |
55 | volatile int iXsf=0,iYsf=0,iZsf=0; |
55 | volatile int iXsf=0,iYsf=0,iZsf=0; |
56 | volatile int Xoffset=0, Yoffset=0, Zoffset=0; |
56 | volatile int Xoffset=0, Yoffset=0, Zoffset=0; |
57 | volatile signed int Xmax=-32768 , Xmin=32767, Ymax=-32768, Ymin=32767, Zmin=32767, Zmax=-32768; // where you store max and min X and Ys |
57 | volatile signed int Xmax=-32768 , Xmin=32767, Ymax=-32768, Ymin=32767, Zmin=32767, Zmax=-32768; // where you store max and min X and Ys |
58 | volatile int XrawOld=0, YrawOld=0, ZrawOld=0; // save values you get from the ASIC |
58 | volatile int XrawOld=0, YrawOld=0, ZrawOld=0; // save values you get from the ASIC |
59 | volatile int Xraw=0, Yraw=0, Zraw=0; // values you get from the ASIC |
59 | volatile int Xraw=0, Yraw=0, Zraw=0; // values you get from the ASIC |
60 | volatile double XrawCal=0, YrawCal=0, ZrawCal=0; // values you get from the ASIC |
60 | volatile double XrawCal=0, YrawCal=0, ZrawCal=0; // values you get from the ASIC |
61 | double XEx=0,XEy=0,XEz=0,YEy=0,YEz=0; |
61 | double XEx=0,XEy=0,XEz=0,YEy=0,YEz=0; |
62 | int MagnetG=0; |
62 | int MagnetG=0; |
63 | typedef struct |
63 | typedef struct |
64 | { |
64 | { |
65 | int Xoffsetm; |
65 | int Xoffsetm; |
66 | int Yoffsetm; |
66 | int Yoffsetm; |
67 | int Zoffsetm; |
67 | int Zoffsetm; |
68 | int iXsfm; |
68 | int iXsfm; |
69 | int iYsfm; |
69 | int iYsfm; |
70 | int iZsfm; |
70 | int iZsfm; |
71 | int dummy; |
71 | int dummy; |
72 | 72 | ||
73 | } CALPARAKOMPASS ; |
73 | } CALPARAKOMPASS ; |
74 | 74 | ||
75 | CALPARAKOMPASS CalParaKompass; |
75 | CALPARAKOMPASS CalParaKompass; |
76 | 76 | ||
77 | 77 | ||
78 | volatile int azimuthgrad= 0,azimuthold = 0; |
78 | volatile int azimuthgrad= 0,azimuthold = 0; |
79 | // SPI interrupt service handler |
79 | // SPI interrupt service handler |
80 | #ifdef SPI_USEINT |
80 | #ifdef SPI_USEINT |
81 | SIGNAL(SIG_SPI) |
81 | SIGNAL(SIG_SPI) |
82 | { |
82 | { |
83 | spiTransferComplete = 1; |
83 | spiTransferComplete = 1; |
84 | } |
84 | } |
85 | #endif |
85 | #endif |
86 | 86 | ||
87 | // access routines |
87 | // access routines |
88 | //------------------------------------------------------------------------------ |
88 | //------------------------------------------------------------------------------ |
89 | // Name: |
89 | // Name: |
90 | // Function: |
90 | // Function: |
91 | // |
91 | // |
92 | // Parameter: |
92 | // Parameter: |
93 | // Return: |
93 | // Return: |
94 | //------------------------------------------------------------------------------ |
94 | //------------------------------------------------------------------------------ |
95 | void spiInit(void) |
95 | void spiInit(void) |
96 | { |
96 | { |
97 | // setup SPI I/O pins |
97 | // setup SPI I/O pins |
98 | sbi(PORTB, 7); // set SCK hi |
98 | sbi(PORTB, 7); // set SCK hi |
99 | sbi(DDRB, 7); // set SCK as output |
99 | sbi(DDRB, 7); // set SCK as output |
100 | cbi(DDRB, 6); // set MISO as input |
100 | cbi(DDRB, 6); // set MISO as input |
101 | sbi(DDRB, 5); // set MOSI as output |
101 | sbi(DDRB, 5); // set MOSI as output |
102 | sbi(DDRD, 5); // SS must be output for Master mode to work |
102 | sbi(DDRD, 5); // SS must be output for Master mode to work |
103 | sbi(DDRD, 3); // Reset output |
103 | sbi(DDRD, 3); // Reset output |
104 | cbi(DDRD, 4); // DataReady as input |
104 | cbi(DDRD, 4); // DataReady as input |
105 | 105 | ||
106 | // setup SPI interface : |
106 | // setup SPI interface : |
107 | // master mode |
107 | // master mode |
108 | sbi(SPCR, MSTR); |
108 | sbi(SPCR, MSTR); |
109 | 109 | ||
110 | // clock = f/32 -> 20 MHZ /32 |
110 | // clock = f/32 -> 20 MHZ /32 |
111 | sbi(SPCR, SPR0); |
111 | sbi(SPCR, SPR0); |
112 | sbi(SPCR, SPR1); |
112 | sbi(SPCR, SPR1); |
113 | cbi(SPSR,SPI2X); |
113 | cbi(SPSR,SPI2X); |
114 | 114 | ||
115 | // select clock phase positive-going in middle of data |
115 | // select clock phase positive-going in middle of data |
116 | cbi(SPCR, CPOL); |
116 | cbi(SPCR, CPOL); |
117 | 117 | ||
118 | // Data order MSB first |
118 | // Data order MSB first |
119 | cbi(SPCR,DORD); |
119 | cbi(SPCR,DORD); |
120 | 120 | ||
121 | // enable SPI |
121 | // enable SPI |
122 | sbi(SPCR, SPE); |
122 | sbi(SPCR, SPE); |
123 | 123 | ||
124 | // clear status |
124 | // clear status |
125 | inb(SPSR); |
125 | inb(SPSR); |
126 | sbi(SPSR,SPI2X); |
126 | sbi(SPSR,SPI2X); |
127 | spiTransferComplete = 1; |
127 | spiTransferComplete = 1; |
128 | 128 | ||
129 | // enable SPI interrupt |
129 | // enable SPI interrupt |
130 | #ifdef SPI_USEINT |
130 | #ifdef SPI_USEINT |
131 | sbi(SPCR, SPIE); |
131 | sbi(SPCR, SPIE); |
132 | #endif |
132 | #endif |
133 | } |
133 | } |
134 | 134 | ||
135 | //------------------------------------------------------------------------------ |
135 | //------------------------------------------------------------------------------ |
136 | // Name: |
136 | // Name: |
137 | // Function: |
137 | // Function: |
138 | // |
138 | // |
139 | // Parameter: |
139 | // Parameter: |
140 | // Return: |
140 | // Return: |
141 | //------------------------------------------------------------------------------ |
141 | //------------------------------------------------------------------------------ |
142 | void spiSendByte(unsigned char data) |
142 | void spiSendByte(unsigned char data) |
143 | { |
143 | { |
144 | // send a byte over SPI and ignore reply |
144 | // send a byte over SPI and ignore reply |
145 | #ifdef SPI_USEINT |
145 | #ifdef SPI_USEINT |
146 | while(!spiTransferComplete); |
146 | while(!spiTransferComplete); |
147 | spiTransferComplete = FALSE; |
147 | spiTransferComplete = FALSE; |
148 | #else |
148 | #else |
149 | while(!(inb(SPSR) & (1<<SPIF))); |
149 | while(!(inb(SPSR) & (1<<SPIF))); |
150 | #endif |
150 | #endif |
151 | 151 | ||
152 | outb(SPDR, data); |
152 | outb(SPDR, data); |
153 | } |
153 | } |
154 | 154 | ||
155 | 155 | ||
156 | //------------------------------------------------------------------------------ |
156 | //------------------------------------------------------------------------------ |
157 | // Name: |
157 | // Name: |
158 | // Function: |
158 | // Function: |
159 | // |
159 | // |
160 | // Parameter: |
160 | // Parameter: |
161 | // Return: |
161 | // Return: |
162 | //------------------------------------------------------------------------------ |
162 | //------------------------------------------------------------------------------ |
163 | unsigned char spiTransferByte(unsigned char data) |
163 | unsigned char spiTransferByte(unsigned char data) |
164 | { |
164 | { |
165 | #ifdef SPI_USEINT |
165 | #ifdef SPI_USEINT |
166 | // send the given data |
166 | // send the given data |
167 | spiTransferComplete = FALSE; |
167 | spiTransferComplete = FALSE; |
168 | outb(SPDR, data); |
168 | outb(SPDR, data); |
169 | // wait for transfer to complete |
169 | // wait for transfer to complete |
170 | while(!spiTransferComplete); |
170 | while(!spiTransferComplete); |
171 | #else |
171 | #else |
172 | // send the given data |
172 | // send the given data |
173 | outb(SPDR, data); |
173 | outb(SPDR, data); |
174 | // wait for transfer to complete |
174 | // wait for transfer to complete |
175 | while(!(inb(SPSR) & (1<<SPIF))); |
175 | while(!(inb(SPSR) & (1<<SPIF))); |
176 | #endif |
176 | #endif |
177 | // return the received data |
177 | // return the received data |
178 | return inb(SPDR); |
178 | return inb(SPDR); |
179 | } |
179 | } |
180 | 180 | ||
181 | //------------------------------------------------------------------------------ |
181 | //------------------------------------------------------------------------------ |
182 | // Name: |
182 | // Name: |
183 | // Function: |
183 | // Function: |
184 | // |
184 | // |
185 | // Parameter: |
185 | // Parameter: |
186 | // Return: |
186 | // Return: |
187 | //------------------------------------------------------------------------------ |
187 | //------------------------------------------------------------------------------ |
188 | unsigned int spiTransferWord(unsigned int data) |
188 | unsigned int spiTransferWord(unsigned int data) |
189 | { |
189 | { |
190 | unsigned int rxData = 0; |
190 | unsigned int rxData = 0; |
191 | 191 | ||
192 | // send MS byte of given data |
192 | // send MS byte of given data |
193 | rxData = (spiTransferByte((data>>8) & 0x00FF))<<8; |
193 | rxData = (spiTransferByte((data>>8) & 0x00FF))<<8; |
194 | // send LS byte of given data |
194 | // send LS byte of given data |
195 | rxData |= (spiTransferByte(data & 0x00FF)); |
195 | rxData |= (spiTransferByte(data & 0x00FF)); |
196 | 196 | ||
197 | // return the received data |
197 | // return the received data |
198 | return rxData; |
198 | return rxData; |
199 | } |
199 | } |
200 | 200 | ||
201 | //------------------------------------------------------------------------------ |
201 | //------------------------------------------------------------------------------ |
202 | //---[ compassFilter ]---------------------------------------------------------- |
202 | //---[ compassFilter ]---------------------------------------------------------- |
203 | //------------------------------------------------------------------------------ |
203 | //------------------------------------------------------------------------------ |
204 | /** Digital filter for the compass readings |
204 | /** Digital filter for the compass readings |
205 | * \n Filter by Vadym Grygorenko |
205 | * \n Filter by Vadym Grygorenko |
206 | * \n fn = (1/4)*(cn + 2*c_(n-1) + c_(n-2)) + (f_(n-1)) - (1/4)*f_(n-2) |
206 | * \n fn = (1/4)*(cn + 2*c_(n-1) + c_(n-2)) + (f_(n-1)) - (1/4)*f_(n-2) |
207 | * |
207 | * |
208 | * \param axiss The axis this measurement is for |
208 | * \param axiss The axis this measurement is for |
209 | * \param reading The latest reading from the compass |
209 | * \param reading The latest reading from the compass |
210 | * \return The newest output from the filter |
210 | * \return The newest output from the filter |
211 | * |
211 | * |
212 | */ |
212 | */ |
213 | 213 | ||
214 | 214 | ||
215 | int compassFilter(unsigned char axiss, int reading) |
215 | int compassFilter(unsigned char axiss, int reading) |
216 | { |
216 | { |
217 | static int input[3][3]={{0,0,0},{0,0,0},{0,0,0}}; //[n, n-1, n-2][axis] |
217 | static int input[3][3]={{0,0,0},{0,0,0},{0,0,0}}; //[n, n-1, n-2][axis] |
218 | static int output[3][3]={{0,0,0},{0,0,0},{0,0,0}}; |
218 | static int output[3][3]={{0,0,0},{0,0,0},{0,0,0}}; |
219 | 219 | ||
220 | input[0][axiss] = reading; |
220 | input[0][axiss] = reading; |
221 | output[0][axiss] = ((input[0][axiss] + 2*input[1][axiss] + input[2][axiss]) / 4); |
221 | output[0][axiss] = ((input[0][axiss] + 2*input[1][axiss] + input[2][axiss]) / 4); |
222 | input[2][axiss] = input[1][axiss]; |
222 | input[2][axiss] = input[1][axiss]; |
223 | input[1][axiss] = input[0][axiss]; |
223 | input[1][axiss] = input[0][axiss]; |
224 | output[2][axiss] = output[1][axiss]; |
224 | output[2][axiss] = output[1][axiss]; |
225 | output[1][axiss] = output[0][axiss]; |
225 | output[1][axiss] = output[0][axiss]; |
226 | return output[0][axiss]; |
226 | return output[0][axiss]; |
227 | } |
227 | } |
228 | 228 | ||
229 | 229 | ||
230 | //------------------------------------------------------------------------------ |
230 | //------------------------------------------------------------------------------ |
231 | // Name: |
231 | // Name: |
232 | // Function: |
232 | // Function: |
233 | // |
233 | // |
234 | // Parameter: |
234 | // Parameter: |
235 | // Return: |
235 | // Return: |
236 | //------------------------------------------------------------------------------ |
236 | //------------------------------------------------------------------------------ |
237 | int MAG_READ(char axis) |
237 | int MAG_READ(char axis) |
238 | { |
238 | { |
239 | int t; |
239 | int t; |
240 | int Data; |
240 | int Data; |
241 | char dontCare; // For a reading we don't care about |
241 | char dontCare; // For a reading we don't care about |
242 | 242 | ||
243 | if (MessungFertig == 0) |
243 | if (MessungFertig == 0) |
244 | { |
244 | { |
245 | PORTD &= ~(1<<PB5); //SS auf low |
245 | PORTD &= ~(1<<PB3); //SS auf low |
246 | 246 | ||
247 | PORTD |= (1<<PB3); //Reset auf high |
247 | PORTD |= (1<<PB8); //Reset auf high |
248 | for(t=0;t<5;t++); //5 Zyklen warten |
248 | for(t=0;t<5;t++); //5 Zyklen warten |
249 | PORTD &= ~(1<<PB3); //Reset auf low |
249 | PORTD &= ~(1<<PB8); //Reset auf low |
250 | 250 | ||
251 | dontCare = (int)spiTransferByte(0x40+axis); // Achse + Mess. Kommando /512 |
251 | dontCare = (int)spiTransferByte(0x40+axis); // Achse + Mess. Kommando /512 |
252 | 252 | ||
253 | PORTD |= (1<<PB5); //SS auf high |
253 | PORTD |= (1<<PB3); //SS auf high |
254 | MessungFertig = 1; |
254 | MessungFertig = 1; |
255 | } |
255 | } |
256 | 256 | ||
257 | if (bit_is_set(PIND,PINB4) && MessungFertig == 1) // Warten bis Messung fertig |
257 | if (bit_is_set(PIND,PINB4) && MessungFertig == 1) // Warten bis Messung fertig |
258 | { |
258 | { |
259 | PORTD &= ~(1<<PB5); //SS auf low |
259 | PORTD &= ~(1<<PB3); //SS auf low |
260 | Data=spiTransferWord(0x00); |
260 | Data=spiTransferWord(0x00); |
261 | PORTD |= (1<<PB5); //SS auf high |
261 | PORTD |= (1<<PB3); //SS auf high |
262 | MessungFertig = 0; |
262 | MessungFertig = 0; |
263 | return (Data); |
263 | return (Data); |
264 | } |
264 | } |
265 | else return -32000; |
265 | else return -32000; |
266 | } |
266 | } |
267 | 267 | ||
268 | //------------------------------------------------------------------------------ |
268 | //------------------------------------------------------------------------------ |
269 | // Name: |
269 | // Name: |
270 | // Function: |
270 | // Function: |
271 | // |
271 | // |
272 | // Parameter: |
272 | // Parameter: |
273 | // Return: |
273 | // Return: |
274 | //------------------------------------------------------------------------------ |
274 | //------------------------------------------------------------------------------ |
275 | 275 | ||
276 | int MAG_READ_CAL(char axis) |
276 | int MAG_READ_CAL(char axis) |
277 | { |
277 | { |
278 | int t; |
278 | int t; |
279 | int Data=0; |
279 | int Data=0; |
280 | char dontCare; // For a reading we don't care about |
280 | char dontCare; // For a reading we don't care about |
281 | 281 | ||
282 | PORTD &= ~(1<<PB5); //SS auf low |
282 | PORTD &= ~(1<<PB3); //SS auf low |
283 | 283 | ||
284 | PORTD |= (1<<PB3); //Reset auf high |
284 | PORTD |= (1<<PB8); //Reset auf high |
285 | for(t=0;t<5;t++); //5 Zyklen warten |
285 | for(t=0;t<5;t++); //5 Zyklen warten |
286 | PORTD &= ~(1<<PB3); //Reset auf low |
286 | PORTD &= ~(1<<PB8); //Reset auf low |
287 | dontCare = (int)spiTransferByte(0x40+axis); // Achse + Mess. Kommando /512 |
287 | dontCare = (int)spiTransferByte(0x40+axis); // Achse + Mess. Kommando /512 |
288 | 288 | ||
289 | PORTD |= (1<<PB5); //SS auf high |
289 | PORTD |= (1<<PB3); //SS auf high |
290 | 290 | ||
291 | while(!bit_is_set(PIND,PINB4)) // Warten bis Messung fertig |
291 | while(!bit_is_set(PIND,PINB4)) // Warten bis Messung fertig |
292 | 292 | ||
293 | PORTD &= ~(1<<PB5); //SS auf low |
293 | PORTD &= ~(1<<PB3); //SS auf low |
294 | Data=spiTransferWord(0x00); |
294 | Data=spiTransferWord(0x00); |
295 | PORTD |= (1<<PB5); //SS auf high |
295 | PORTD |= (1<<PB5); //SS auf high |
296 | 296 | ||
297 | return (Data); |
297 | return (Data); |
298 | 298 | ||
299 | } |
299 | } |
300 | 300 | ||
301 | 301 | ||
302 | 302 | ||
303 | //------------------------------------------------------------------------------ |
303 | //------------------------------------------------------------------------------ |
304 | // Name: |
304 | // Name: |
305 | // Function: |
305 | // Function: |
306 | // |
306 | // |
307 | // Parameter: |
307 | // Parameter: |
308 | // Return: |
308 | // Return: |
309 | //------------------------------------------------------------------------------ |
309 | //------------------------------------------------------------------------------ |
310 | 310 | ||
311 | 311 | ||
312 | void ReadEepromKompassValues(void) |
312 | void ReadEepromKompassValues(void) |
313 | { |
313 | { |
314 | eeprom_read_block(&CalParaKompass, &EEPromArray[4], sizeof(CALPARAKOMPASS)); |
314 | eeprom_read_block(&CalParaKompass, &EEPromArray[4], sizeof(CALPARAKOMPASS)); |
315 | 315 | ||
316 | Xoffset = CalParaKompass.Xoffsetm; |
316 | Xoffset = CalParaKompass.Xoffsetm; |
317 | Yoffset = CalParaKompass.Yoffsetm; |
317 | Yoffset = CalParaKompass.Yoffsetm; |
318 | Zoffset = CalParaKompass.Zoffsetm; |
318 | Zoffset = CalParaKompass.Zoffsetm; |
319 | 319 | ||
320 | 320 | ||
321 | iXsf = CalParaKompass.iXsfm; |
321 | iXsf = CalParaKompass.iXsfm; |
322 | iYsf = CalParaKompass.iYsfm; |
322 | iYsf = CalParaKompass.iYsfm; |
323 | iZsf = CalParaKompass.iZsfm; |
323 | iZsf = CalParaKompass.iZsfm; |
324 | 324 | ||
325 | } |
325 | } |
326 | 326 | ||
327 | //------------------------------------------------------------------------------ |
327 | //------------------------------------------------------------------------------ |
328 | // Name: |
328 | // Name: |
329 | // Function: |
329 | // Function: |
330 | // |
330 | // |
331 | // Parameter: |
331 | // Parameter: |
332 | // Return: |
332 | // Return: |
333 | //------------------------------------------------------------------------------ |
333 | //------------------------------------------------------------------------------ |
334 | void KalibrierKompass(void) |
334 | void KalibrierKompass(void) |
335 | { |
335 | { |
336 | 336 | ||
337 | 337 | ||
338 | // The numbers here will be 16-bit signed values. |
338 | // The numbers here will be 16-bit signed values. |
339 | // Positive = 0x0000 to 0x7FFF |
339 | // Positive = 0x0000 to 0x7FFF |
340 | // Negative = 0x8000 to 0xFFFF |
340 | // Negative = 0x8000 to 0xFFFF |
341 | int t = 0; |
341 | int t = 0; |
342 | int tempx=0,tempy=0,tempz=0; |
342 | int tempx=0,tempy=0,tempz=0; |
343 | 343 | ||
344 | 344 | ||
345 | if(Poti4 > 70 && Poti4 < 100) |
345 | if(Poti4 > 70 && Poti4 < 100) |
346 | { |
346 | { |
347 | LED1_OFF; |
347 | LED1_OFF; |
348 | LED2_OFF; |
348 | LED2_OFF; |
349 | EEPROMWrite = 0; |
349 | EEPROMWrite = 0; |
350 | /* |
350 | /* |
351 | Xmax = -32768; |
351 | Xmax = -32768; |
352 | Ymax = -32768; // start with lowest possible value |
352 | Ymax = -32768; // start with lowest possible value |
353 | Zmax = -32768; |
353 | Zmax = -32768; |
354 | 354 | ||
355 | Xmin = 32767; |
355 | Xmin = 32767; |
356 | Ymin = 32767; // start with highest possible value |
356 | Ymin = 32767; // start with highest possible value |
357 | Zmin = 32767; |
357 | Zmin = 32767; |
358 | */ |
358 | */ |
359 | } |
359 | } |
360 | 360 | ||
361 | if(Poti4 > 70) // wenn Schalter ausgeschaltet, dann werden die LED ausgeschaltet und das GPS Steuerkommando gelöscht // (199797Kö) |
361 | if(Poti4 > 70) // wenn Schalter ausgeschaltet, dann werden die LED ausgeschaltet und das GPS Steuerkommando gelöscht // (199797Kö) |
362 | { |
362 | { |
363 | 363 | ||
364 | if(Poti4 > 100 && Poti4 < 150) // X-Y Achse Kalibrieren |
364 | if(Poti4 > 100 && Poti4 < 150) // X-Y Achse Kalibrieren |
365 | { |
365 | { |
366 | LED1_ON; |
366 | LED1_ON; |
367 | Xraw=MAG_READ_CAL(1); //Kompass x-Achse auslesen |
367 | Xraw=MAG_READ_CAL(1); //Kompass x-Achse auslesen |
368 | Xraw = -Xraw; |
368 | Xraw = -Xraw; |
369 | if(((Xraw > (XrawOld + 200)) || (Xraw < (XrawOld - 200)))) Xraw = XrawOld; //Filter gegen Spikes |
369 | if(((Xraw > (XrawOld + 200)) || (Xraw < (XrawOld - 200)))) Xraw = XrawOld; //Filter gegen Spikes |
370 | tempx = Xraw; |
370 | tempx = Xraw; |
371 | Xraw = compassFilter(0,tempx); |
371 | Xraw = compassFilter(0,tempx); |
372 | XrawOld = Xraw; |
372 | XrawOld = Xraw; |
373 | 373 | ||
374 | Yraw=MAG_READ_CAL(2); //Kompass y-Achse auslesen |
374 | Yraw=MAG_READ_CAL(2); //Kompass y-Achse auslesen |
375 | Yraw =-Yraw; |
375 | Yraw =-Yraw; |
376 | if(((Yraw > (YrawOld + 200)) || (Yraw < (YrawOld - 200)))) Yraw = YrawOld; //Filter gegen Spikes |
376 | if(((Yraw > (YrawOld + 200)) || (Yraw < (YrawOld - 200)))) Yraw = YrawOld; //Filter gegen Spikes |
377 | tempy = Yraw; |
377 | tempy = Yraw; |
378 | Yraw = compassFilter(1,tempy); |
378 | Yraw = compassFilter(1,tempy); |
379 | YrawOld = Yraw; |
379 | YrawOld = Yraw; |
380 | 380 | ||
381 | if( Xraw > Xmax ) Xmax = Xraw; |
381 | if( Xraw > Xmax ) Xmax = Xraw; |
382 | if( Xraw < Xmin ) Xmin = Xraw; |
382 | if( Xraw < Xmin ) Xmin = Xraw; |
383 | if( Yraw > Ymax ) Ymax = Yraw; |
383 | if( Yraw > Ymax ) Ymax = Yraw; |
384 | if( Yraw < Ymin ) Ymin = Yraw; |
384 | if( Yraw < Ymin ) Ymin = Yraw; |
385 | 385 | ||
386 | Xoffset = ( abs(Xmax) - abs(Xmin) ) / 2; // calculate offsets |
386 | Xoffset = ( abs(Xmax) - abs(Xmin) ) / 2; // calculate offsets |
387 | Yoffset = ( abs(Ymax) - abs(Ymin) ) / 2; |
387 | Yoffset = ( abs(Ymax) - abs(Ymin) ) / 2; |
388 | 388 | ||
389 | iXsf = ( abs(Xmax) + abs(Xmin)) / 2; // calculate the range |
389 | iXsf = ( abs(Xmax) + abs(Xmin)) / 2; // calculate the range |
390 | iYsf = ( abs(Ymax) + abs(Ymin)) / 2; |
390 | iYsf = ( abs(Ymax) + abs(Ymin)) / 2; |
391 | 391 | ||
392 | XrawCal = (double)(Xraw - Xoffset)/(double)iXsf; |
392 | XrawCal = (double)(Xraw - Xoffset)/(double)iXsf; |
393 | YrawCal = (double)(Yraw - Yoffset) /(double)iYsf; |
393 | YrawCal = (double)(Yraw - Yoffset) /(double)iYsf; |
394 | } |
394 | } |
395 | 395 | ||
396 | if(Poti4 > 150) |
396 | if(Poti4 > 150) |
397 | { |
397 | { |
398 | LED1_OFF; |
398 | LED1_OFF; |
399 | LED2_ON; |
399 | LED2_ON; |
400 | Zraw=MAG_READ_CAL(3); //Kompass Z-Achse auslesen |
400 | Zraw=MAG_READ_CAL(3); //Kompass Z-Achse auslesen |
401 | Zraw = -Zraw; |
401 | Zraw = -Zraw; |
402 | if(((Zraw > (ZrawOld + 200)) || (Zraw < (ZrawOld - 200)))) Zraw = ZrawOld; //Filter gegen Spikes |
402 | if(((Zraw > (ZrawOld + 200)) || (Zraw < (ZrawOld - 200)))) Zraw = ZrawOld; //Filter gegen Spikes |
403 | tempz = Zraw; |
403 | tempz = Zraw; |
404 | Zraw = compassFilter(2,tempz); |
404 | Zraw = compassFilter(2,tempz); |
405 | ZrawOld = Zraw; |
405 | ZrawOld = Zraw; |
406 | 406 | ||
407 | if( Zraw > Zmax ) Zmax = Zraw; |
407 | if( Zraw > Zmax ) Zmax = Zraw; |
408 | if( Zraw < Zmin ) Zmin = Zraw; |
408 | if( Zraw < Zmin ) Zmin = Zraw; |
409 | 409 | ||
410 | Zoffset = ( abs(Zmax) - abs(Zmin) ) / 2; |
410 | Zoffset = ( abs(Zmax) - abs(Zmin) ) / 2; |
411 | iZsf = ( abs(Zmax) + abs(Zmin)) / 2; |
411 | iZsf = ( abs(Zmax) + abs(Zmin)) / 2; |
412 | ZrawCal = (double)(Zraw- Zoffset) / (double)iZsf; |
412 | ZrawCal = (double)(Zraw- Zoffset) / (double)iZsf; |
413 | 413 | ||
414 | } |
414 | } |
415 | 415 | ||
416 | } |
416 | } |
417 | 417 | ||
418 | if(Poti4 > 170) |
418 | if(Poti4 > 170) |
419 | { |
419 | { |
420 | if (EEPROMWrite == 0) |
420 | if (EEPROMWrite == 0) |
421 | { |
421 | { |
422 | LED2_ON; |
422 | LED2_ON; |
423 | LED1_ON; |
423 | LED1_ON; |
424 | CalParaKompass.Xoffsetm = Xoffset; |
424 | CalParaKompass.Xoffsetm = Xoffset; |
425 | CalParaKompass.Yoffsetm = Yoffset; |
425 | CalParaKompass.Yoffsetm = Yoffset; |
426 | CalParaKompass.Zoffsetm = Zoffset; |
426 | CalParaKompass.Zoffsetm = Zoffset; |
427 | 427 | ||
428 | CalParaKompass.iXsfm = iXsf; |
428 | CalParaKompass.iXsfm = iXsf; |
429 | CalParaKompass.iYsfm = iYsf; |
429 | CalParaKompass.iYsfm = iYsf; |
430 | CalParaKompass.iZsfm = iZsf; |
430 | CalParaKompass.iZsfm = iZsf; |
431 | 431 | ||
432 | eeprom_write_block(&CalParaKompass, &EEPromArray[4], sizeof(CALPARAKOMPASS)); |
432 | eeprom_write_block(&CalParaKompass, &EEPromArray[4], sizeof(CALPARAKOMPASS)); |
433 | 433 | ||
434 | Xmax = -32768; |
434 | Xmax = -32768; |
435 | Ymax = -32768; // start with lowest possible value |
435 | Ymax = -32768; // start with lowest possible value |
436 | Zmax = -32768; |
436 | Zmax = -32768; |
437 | 437 | ||
438 | Xmin = 32767; |
438 | Xmin = 32767; |
439 | Ymin = 32767; // start with highest possible value |
439 | Ymin = 32767; // start with highest possible value |
440 | Zmin = 32767; |
440 | Zmin = 32767; |
441 | EEPROMWrite = 1; |
441 | EEPROMWrite = 1; |
442 | } |
442 | } |
443 | } |
443 | } |
444 | 444 | ||
445 | } |
445 | } |
446 | 446 | ||
447 | 447 | ||
448 | //------------------------------------------------------------------------------ |
448 | //------------------------------------------------------------------------------ |
449 | // Name: |
449 | // Name: |
450 | // Function: |
450 | // Function: |
451 | // |
451 | // |
452 | // Parameter: |
452 | // Parameter: |
453 | // Return: |
453 | // Return: |
454 | //------------------------------------------------------------------------------ |
454 | //------------------------------------------------------------------------------ |
455 | int KompassRead(void) |
455 | int KompassRead(void) |
456 | { |
456 | { |
457 | /* |
457 | /* |
458 | Declination = 1,209° changing by 0,118 °/year |
458 | Declination = 1,209° changing by 0,118 °/year |
459 | Inclination = 64,779° changing by 0,004 °/year |
459 | Inclination = 64,779° changing by 0,004 °/year |
460 | X component = 20.529,42 changing by 10,87 nT/year |
460 | X component = 20.529,42 changing by 10,87 nT/year |
461 | Y component = 433,23 changing by 42,54 nT/year |
461 | Y component = 433,23 changing by 42,54 nT/year |
462 | Z component = 43.596,06 changing by 32,61 nT/year |
462 | Z component = 43.596,06 changing by 32,61 nT/year |
463 | Horizontal Intensity = 20.533,99 changing by 11,8 nT/year |
463 | Horizontal Intensity = 20.533,99 changing by 11,8 nT/year |
464 | Total Intensity = 48.189,85 changing by 34,53 nT/year |
464 | Total Intensity = 48.189,85 changing by 34,53 nT/year |
465 | */ |
465 | */ |
466 | /* |
466 | /* |
467 | Messung Real Nick |
467 | Messung Real Nick |
468 | 14 10 |
468 | 14 10 |
469 | 30 20 |
469 | 30 20 |
470 | 40 30 |
470 | 40 30 |
471 | 52 40 |
471 | 52 40 |
472 | 61 50 |
472 | 61 50 |
473 | 473 | ||
474 | 30 20 roll |
474 | 30 20 roll |
475 | 39 28 |
475 | 39 28 |
476 | 19 12 |
476 | 19 12 |
477 | 50 35 |
477 | 50 35 |
478 | */ |
478 | */ |
479 | 479 | ||
480 | double azimuthrad = 0; |
480 | double azimuthrad = 0; |
481 | double magnet = 68 /180 * M_PI; |
481 | double magnet = 68 /180 * M_PI; |
482 | //Look-Up Tabelle für Korrekturfaktoren |
482 | //Look-Up Tabelle für Korrekturfaktoren |
483 | //float winkelkorr[5]={0.5,0.6,0.65,0.65,0.7}; |
483 | //float winkelkorr[5]={0.5,0.6,0.65,0.65,0.7}; |
484 | int lookup_nick =0,lookup_roll=0; |
484 | int lookup_nick =0,lookup_roll=0; |
485 | int tempx=0,tempy=0,tempz=0; |
485 | int tempx=0,tempy=0,tempz=0; |
486 | 486 | ||
487 | nickgrad = -(IntegralNick / 1430); //Teiler ermittelt durch Test bestätigung durch Holger |
487 | nickgrad = -(IntegralNick / 1430); //Teiler ermittelt durch Test bestätigung durch Holger |
488 | rollgrad = -(IntegralRoll / 1430); //Teiler ermittelt durch Test bestätigung durch Holger |
488 | rollgrad = -(IntegralRoll / 1430); //Teiler ermittelt durch Test bestätigung durch Holger |
489 | 489 | ||
490 | /* if(nickgrad > 45) nickgradkomp = 45; |
490 | /* if(nickgrad > 45) nickgradkomp = 45; |
491 | else nickgradkomp = nickgrad; |
491 | else nickgradkomp = nickgrad; |
492 | if(nickgrad < -45) nickgradkomp = -45; |
492 | if(nickgrad < -45) nickgradkomp = -45; |
493 | else nickgradkomp = nickgrad; |
493 | else nickgradkomp = nickgrad; |
494 | 494 | ||
495 | 495 | ||
496 | if(rollgrad > 45) rollgradkomp = 45; |
496 | if(rollgrad > 45) rollgradkomp = 45; |
497 | else rollgradkomp = rollgrad; |
497 | else rollgradkomp = rollgrad; |
498 | if(rollgrad < -45) rollgradkomp = -45; |
498 | if(rollgrad < -45) rollgradkomp = -45; |
499 | else rollgradkomp = rollgrad; |
499 | else rollgradkomp = rollgrad; |
500 | 500 | ||
501 | 501 | ||
502 | if(abs(nickgrad) > 5 && abs(nickgrad) <= 15) lookup_nick = 0; |
502 | if(abs(nickgrad) > 5 && abs(nickgrad) <= 15) lookup_nick = 0; |
503 | if(abs(nickgrad) > 15 && abs(nickgrad) <= 25) lookup_nick = 1; |
503 | if(abs(nickgrad) > 15 && abs(nickgrad) <= 25) lookup_nick = 1; |
504 | if(abs(nickgrad) > 25 && abs(nickgrad) <= 35) lookup_nick = 2; |
504 | if(abs(nickgrad) > 25 && abs(nickgrad) <= 35) lookup_nick = 2; |
505 | if(abs(nickgrad) > 35 && abs(nickgrad) <= 45) lookup_nick = 3; |
505 | if(abs(nickgrad) > 35 && abs(nickgrad) <= 45) lookup_nick = 3; |
506 | if(abs(nickgrad) > 45) lookup_nick = 4; |
506 | if(abs(nickgrad) > 45) lookup_nick = 4; |
507 | 507 | ||
508 | if(abs(rollgrad) > 5 && abs(rollgrad) <= 15) lookup_roll = 0; |
508 | if(abs(rollgrad) > 5 && abs(rollgrad) <= 15) lookup_roll = 0; |
509 | if(abs(rollgrad) > 15 && abs(rollgrad) <= 25) lookup_roll = 1; |
509 | if(abs(rollgrad) > 15 && abs(rollgrad) <= 25) lookup_roll = 1; |
510 | if(abs(rollgrad) > 25 && abs(rollgrad) <= 35) lookup_roll = 2; |
510 | if(abs(rollgrad) > 25 && abs(rollgrad) <= 35) lookup_roll = 2; |
511 | if(abs(rollgrad) > 35 && abs(rollgrad) <= 45) lookup_roll = 3; |
511 | if(abs(rollgrad) > 35 && abs(rollgrad) <= 45) lookup_roll = 3; |
512 | if(abs(rollgrad) > 45) lookup_roll = 4; |
512 | if(abs(rollgrad) > 45) lookup_roll = 4; |
513 | */ |
513 | */ |
514 | nick = (float)nickgrad/180 * M_PI;// * (float)winkelkorr[lookup_nick]*(float); |
514 | nick = (float)nickgrad/180 * M_PI;// * (float)winkelkorr[lookup_nick]*(float); |
515 | roll = (float)rollgrad/180 * M_PI;// * (float)winkelkorr[lookup_roll]*(float); |
515 | roll = (float)rollgrad/180 * M_PI;// * (float)winkelkorr[lookup_roll]*(float); |
516 | 516 | ||
517 | if(Poti4 > 70) // wenn Schalter ausgeschaltet, dann werden die LED ausgeschaltet und das GPS Steuerkommando gelöscht // (199797Kö) |
517 | if(Poti4 > 70) // wenn Schalter ausgeschaltet, dann werden die LED ausgeschaltet und das GPS Steuerkommando gelöscht // (199797Kö) |
518 | { |
518 | { |
519 | KalibrierKompass(); |
519 | KalibrierKompass(); |
520 | } |
520 | } |
521 | else |
521 | else |
522 | { |
522 | { |
523 | 523 | ||
524 | switch(KompassInUse) |
524 | switch(KompassInUse) |
525 | { |
525 | { |
526 | case 0: |
526 | case 0: |
527 | Xraw=MAG_READ(1); //Kompass x-Achse auslesen |
527 | Xraw=MAG_READ(1); //Kompass x-Achse auslesen |
528 | Xraw = -Xraw; |
528 | Xraw = -Xraw; |
529 | if (Xraw == 32000) Xraw = XrawOld; |
529 | if (Xraw == 32000) Xraw = XrawOld; |
530 | else |
530 | else |
531 | { |
531 | { |
532 | //if(((Xraw > (XrawOld + 200)) || (Xraw < (XrawOld - 200)))) Xraw = XrawOld; //Filter gegen Spikes |
532 | //if(((Xraw > (XrawOld + 200)) || (Xraw < (XrawOld - 200)))) Xraw = XrawOld; //Filter gegen Spikes |
533 | XrawOld = Xraw; |
533 | XrawOld = Xraw; |
534 | tempx = Xraw; |
534 | tempx = Xraw; |
535 | Xraw = compassFilter(0, tempx); |
535 | Xraw = compassFilter(0, tempx); |
536 | NewX = 1; |
536 | NewX = 1; |
537 | KompassInUse = 1; |
537 | KompassInUse = 1; |
538 | } |
538 | } |
539 | break; |
539 | break; |
540 | 540 | ||
541 | case 1: |
541 | case 1: |
542 | Yraw=MAG_READ(2); //Kompass y-Achse auslesen |
542 | Yraw=MAG_READ(2); //Kompass y-Achse auslesen |
543 | Yraw = -Yraw; |
543 | Yraw = -Yraw; |
544 | if (Yraw == 32000) Yraw = YrawOld; |
544 | if (Yraw == 32000) Yraw = YrawOld; |
545 | else |
545 | else |
546 | { |
546 | { |
547 | //if(((Yraw > (YrawOld + 200)) || (Yraw < (YrawOld - 200)))) Yraw = YrawOld; //Filter gegen Spikes |
547 | //if(((Yraw > (YrawOld + 200)) || (Yraw < (YrawOld - 200)))) Yraw = YrawOld; //Filter gegen Spikes |
548 | YrawOld = Yraw; |
548 | YrawOld = Yraw; |
549 | tempy = Yraw; |
549 | tempy = Yraw; |
550 | Yraw = compassFilter(1, tempy); |
550 | Yraw = compassFilter(1, tempy); |
551 | NewY = 1; |
551 | NewY = 1; |
552 | KompassInUse = 2; |
552 | KompassInUse = 2; |
553 | } |
553 | } |
554 | break; |
554 | break; |
555 | 555 | ||
556 | case 2: |
556 | case 2: |
557 | Zraw=MAG_READ(3); //Kompass z-Achse auslesen |
557 | Zraw=MAG_READ(3); //Kompass z-Achse auslesen |
558 | Zraw = -Zraw; |
558 | Zraw = -Zraw; |
559 | if (Zraw == 32000) Zraw = ZrawOld; |
559 | if (Zraw == 32000) Zraw = ZrawOld; |
560 | else |
560 | else |
561 | { |
561 | { |
562 | //if(((Zraw > (ZrawOld + 200)) || (Zraw < (ZrawOld - 200)))) Zraw = ZrawOld; //Filter gegen Spikes |
562 | //if(((Zraw > (ZrawOld + 200)) || (Zraw < (ZrawOld - 200)))) Zraw = ZrawOld; //Filter gegen Spikes |
563 | ZrawOld = Zraw; |
563 | ZrawOld = Zraw; |
564 | tempz = Zraw; |
564 | tempz = Zraw; |
565 | Zraw = compassFilter(2, tempz); |
565 | Zraw = compassFilter(2, tempz); |
566 | NewZ = 1; |
566 | NewZ = 1; |
567 | KompassInUse = 0; |
567 | KompassInUse = 0; |
568 | 568 | ||
569 | } |
569 | } |
570 | break; |
570 | break; |
571 | } |
571 | } |
572 | 572 | ||
573 | 573 | ||
574 | /* Xoffset =-44; |
574 | /* Xoffset =-44; |
575 | Yoffset =-36; |
575 | Yoffset =-36; |
576 | Zoffset =35; |
576 | Zoffset =35; |
577 | 577 | ||
578 | iXsf =242; |
578 | iXsf =242; |
579 | iYsf =256; |
579 | iYsf =256; |
580 | iZsf =265; |
580 | iZsf =265; |
581 | */ |
581 | */ |
582 | 582 | ||
583 | if(NewX == 1 && NewY == 1 && NewZ == 1) |
583 | if(NewX == 1 && NewY == 1 && NewZ == 1) |
584 | { |
584 | { |
585 | // Kalibrierung |
585 | // Kalibrierung |
586 | XrawCal = (double)(Xraw - Xoffset)/ (double)iXsf; |
586 | XrawCal = (double)(Xraw - Xoffset)/ (double)iXsf; |
587 | YrawCal = (double)(Yraw - Yoffset) / (double)iYsf; |
587 | YrawCal = (double)(Yraw - Yoffset) / (double)iYsf; |
588 | ZrawCal = (double)(Zraw- Zoffset) / (double)iZsf; |
588 | ZrawCal = (double)(Zraw- Zoffset) / (double)iZsf; |
589 | // 1.2 Grad Missweisung durch Geographischer und Magnetischer Nordpol |
589 | // 1.2 Grad Missweisung durch Geographischer und Magnetischer Nordpol |
590 | 590 | ||
591 | 591 | ||
592 | //MagnetG = (int)sqrt((long)(XrawCal * XrawCal) + (long)(YrawCal * YrawCal) + (long)(ZrawCal * ZrawCal)); |
592 | //MagnetG = (int)sqrt((long)(XrawCal * XrawCal) + (long)(YrawCal * YrawCal) + (long)(ZrawCal * ZrawCal)); |
593 | 593 | ||
594 | //Kompensationsroutine |
594 | //Kompensationsroutine |
595 | 595 | ||
596 | 596 | ||
597 | XEx = XrawCal * (double)cos(nick); |
597 | XEx = XrawCal * (double)cos(nick); |
598 | XEy = YrawCal * (double)sin(nick) * (double)sin(roll); |
598 | XEy = YrawCal * (double)sin(nick) * (double)sin(roll); |
599 | XEz = ZrawCal * (double)sin(nick) * (double)cos(roll); |
599 | XEz = ZrawCal * (double)sin(nick) * (double)cos(roll); |
600 | 600 | ||
601 | YEy = YrawCal * (double)cos(roll); |
601 | YEy = YrawCal * (double)cos(roll); |
602 | YEz = ZrawCal * (double)sin(roll); |
602 | YEz = ZrawCal * (double)sin(roll); |
603 | 603 | ||
604 | XE = XEx + XEy - XEz; |
604 | XE = XEx + XEy - XEz; |
605 | YE = YEy + YEz; |
605 | YE = YEy + YEz; |
606 | 606 | ||
607 | 607 | ||
608 | if(XE==0 && YE <0) azimuthgrad = 90; |
608 | if(XE==0 && YE <0) azimuthgrad = 90; |
609 | if(XE==0 && YE >0) azimuthgrad = 270; |
609 | if(XE==0 && YE >0) azimuthgrad = 270; |
610 | if(XE < 0) azimuthgrad = 180-(atan(YE/XE)*180/M_PI); |
610 | if(XE < 0) azimuthgrad = 180-(atan(YE/XE)*180/M_PI); |
611 | if(XE > 0 && YE < 0) azimuthgrad = -(atan(YE/XE)*180/M_PI); |
611 | if(XE > 0 && YE < 0) azimuthgrad = -(atan(YE/XE)*180/M_PI); |
612 | if(XE > 0 && YE > 0) azimuthgrad = 360 - (atan(YE/XE)*180/M_PI); |
612 | if(XE > 0 && YE > 0) azimuthgrad = 360 - (atan(YE/XE)*180/M_PI); |
613 | 613 | ||
614 | azimuthold = azimuthgrad; |
614 | azimuthold = azimuthgrad; |
615 | 615 | ||
616 | NewX = 0; |
616 | NewX = 0; |
617 | NewY = 0; |
617 | NewY = 0; |
618 | NewZ = 0; |
618 | NewZ = 0; |
619 | return azimuthgrad; |
619 | return azimuthgrad; |
620 | } |
620 | } |
621 | else return azimuthold; |
621 | else return azimuthold; |
622 | } |
622 | } |
623 | } |
623 | } |
624 | 624 |