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 | ||