Subversion Repositories FlightCtrl

Rev

Rev 308 | Details | Compare with Previous | Last modification | View Log | RSS feed

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