Subversion Repositories FlightCtrl

Rev

Rev 1179 | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 1179 Rev 1199
Line 25... Line 25...
25
#include "timer0.h"
25
#include "timer0.h"
26
#include "rc.h"
26
#include "rc.h"
27
#include "eeprom.h"
27
#include "eeprom.h"
28
#include "printf_P.h"
28
#include "printf_P.h"
Line -... Line 29...
-
 
29
 
-
 
30
 
-
 
31
// for compatibility reasons gcc3.x <-> gcc4.x
-
 
32
#ifndef SPCR
-
 
33
#define SPCR   SPCR0
-
 
34
#endif
-
 
35
#ifndef SPIE
-
 
36
#define SPIE   SPIE0
-
 
37
#endif
-
 
38
#ifndef SPE
-
 
39
#define SPE    SPE0
-
 
40
#endif
-
 
41
#ifndef DORD
-
 
42
#define DORD   DORD0
-
 
43
#endif
-
 
44
#ifndef MSTR
-
 
45
#define MSTR   MSTR0
-
 
46
#endif
-
 
47
#ifndef CPOL
-
 
48
#define CPOL   CPOL0
-
 
49
#endif
-
 
50
#ifndef CPHA
-
 
51
#define CPHA   CPHA0
-
 
52
#endif
-
 
53
#ifndef SPR1
-
 
54
#define SPR1   SPR01
-
 
55
#endif
-
 
56
#ifndef SPR0
-
 
57
#define SPR0   SPR00
-
 
58
#endif
-
 
59
 
-
 
60
#ifndef SPDR
-
 
61
#define SPDR   SPDR0
-
 
62
#endif
-
 
63
 
-
 
64
#ifndef SPSR
-
 
65
#define SPSR   SPSR0
-
 
66
#endif
-
 
67
#ifndef SPIF
-
 
68
#define SPIF   SPIF0
-
 
69
#endif
-
 
70
#ifndef WCOL
-
 
71
#define WCOL   WCOL0
-
 
72
#endif
-
 
73
#ifndef SPI2X
-
 
74
#define SPI2X  SPI2X0
-
 
75
#endif
-
 
76
// -------------------------
-
 
77
 
29
 
78
 
Line 30... Line 79...
30
#define MAX_AXIS_VALUE          500
79
#define MAX_AXIS_VALUE          500
31
 
80
 
Line 84... Line 133...
84
        #define MM3_SS_PIN     PC4
133
        #define MM3_SS_PIN     PC4
85
        #define MM3_RESET_PORT PORTC
134
        #define MM3_RESET_PORT PORTC
86
        #define MM3_RESET_DDR  DDRC
135
        #define MM3_RESET_DDR  DDRC
87
        #define MM3_RESET_PIN  PC5
136
        #define MM3_RESET_PIN  PC5
88
#endif
137
#endif
89
       
138
 
90
#define MM3_SS_ON      MM3_SS_PORT    &= ~(1<<MM3_SS_PIN);
139
#define MM3_SS_ON      MM3_SS_PORT    &= ~(1<<MM3_SS_PIN);
91
#define MM3_SS_OFF     MM3_SS_PORT    |=  (1<<MM3_SS_PIN);
140
#define MM3_SS_OFF     MM3_SS_PORT    |=  (1<<MM3_SS_PIN);
92
#define MM3_RESET_ON   MM3_RESET_PORT |=  (1<<MM3_RESET_PIN);
141
#define MM3_RESET_ON   MM3_RESET_PORT |=  (1<<MM3_RESET_PIN);
93
#define MM3_RESET_OFF  MM3_RESET_PORT  &= ~(1<<MM3_RESET_PIN);
142
#define MM3_RESET_OFF  MM3_RESET_PORT  &= ~(1<<MM3_RESET_PIN);
Line 414... Line 463...
414
void MM3_Heading(void)
463
void MM3_Heading(void)
415
{
464
{
416
        int32_t sin_nick, cos_nick, sin_roll, cos_roll, sin_yaw, cos_yaw;
465
        int32_t sin_nick, cos_nick, sin_roll, cos_roll, sin_yaw, cos_yaw;
417
        int32_t  Hx, Hy, Hz, Hx_corr, Hy_corr;
466
        int32_t  Hx, Hy, Hz, Hx_corr, Hy_corr;
418
        int16_t angle;
467
        int16_t angle;
419
        uint16_t div_factor;
-
 
420
        int16_t heading;
468
        int16_t heading;
Line 421... Line 469...
421
 
469
 
422
        if (MM3_Timeout)
470
        if (MM3_Timeout)
423
        {
471
        {
Line 447... Line 495...
447
                Hy = (Hx_corr * sin_yaw + Hy_corr  * cos_yaw) / 8192;
495
                Hy = (Hx_corr * sin_yaw + Hy_corr  * cos_yaw) / 8192;
Line 448... Line 496...
448
 
496
 
Line 449... Line -...
449
 
-
 
450
                // tilt compensation
-
 
451
 
-
 
452
                // calibration factor for transforming Gyro Integrals to angular degrees
497
 
453
                div_factor = (uint16_t)ParamSet.UserParam3 * 8;
498
                // tilt compensation
454
 
499
 
455
                // calculate sinus cosinus of nick and tilt angle
500
                // calculate sinus cosinus of nick and tilt angle
Line 456... Line 501...
456
                angle = (IntegralNick/div_factor);
501
                angle = (int16_t)(IntegralGyroNick/GYRO_DEG_FACTOR);
457
                sin_nick = (int32_t)(c_sin_8192(angle));
502
                sin_nick = (int32_t)(c_sin_8192(angle));
458
                cos_nick = (int32_t)(c_cos_8192(angle));
503
                cos_nick = (int32_t)(c_cos_8192(angle));
Line 459... Line 504...
459
 
504
 
460
                angle = (IntegralRoll/div_factor);
505
                angle = (int16_t)(IntegralGyroRoll/GYRO_DEG_FACTOR);