Subversion Repositories NaviCtrl

Rev

Rev 329 | Rev 338 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 329 Rev 330
Line 69... Line 69...
69 
fifo_t CompassCalcStateFiFo;
 69 
fifo_t CompassCalcStateFiFo;
Line 70... Line 70...
70 
 
 70 
 
71 
volatile s16vec_t MagVector;   // is written by mk3mag or ncmag implementation
 71 
volatile s16vec_t MagVector;   // is written by mk3mag or ncmag implementation
72 
volatile s16 Compass_Heading;   // is written by mk3mag or ncmag implementation
 72 
volatile s16 Compass_Heading;   // is written by mk3mag or ncmag implementation
- 
 
 73 
volatile u8  Compass_CalState; // is written by mk3mag or ncmag implementation
- 
 
 74 
s16 Hx = 0, Hy = 0;
- 
 
 75 
s32 EarthMagneticField = 100;
- 
 
 76 
s32 EarthMagneticFieldFiltered = 100;
- 
 
 77 
s32 EarthMagneticInclination = 0;
Line 73... Line 78...
73 
volatile u8  Compass_CalState; // is written by mk3mag or ncmag implementation
 78 
u8  ErrorDisturbedEarthMagnetField = 0;
74 
 
 79 
 
75 
#define COMPASS_NONE    0
 80 
#define COMPASS_NONE    0
76 
#define COMPASS_MK3MAG  1
 81 
#define COMPASS_MK3MAG  1
Line 118... Line 123...
118 
        }
 123 
        }
119 
        else // fc attitude is avialable and no compass calibration active
 124 
        else // fc attitude is avialable and no compass calibration active
120 
        {
 125 
        {
121 
                // calculate attitude correction
 126 
                // calculate attitude correction
122 
                // a float implementation takes too long
 127 
                // a float implementation takes too long
123 
                s16 tmp, Hx, Hy;
 128 
                s16 tmp;
124 
                s32 sinnick, cosnick, sinroll, cosroll;
 129 
                s32 sinnick, cosnick, sinroll, cosroll;
125 
 
 - 
 
126 
                tmp = FromFlightCtrl.AngleNick/10; // in deg
 130 
                tmp = FromFlightCtrl.AngleNick/10; // in deg
127 
                sinnick = (s32)c_sin_8192(tmp);
 131 
                sinnick = (s32)c_sin_8192(tmp);
128 
                cosnick = (s32)c_cos_8192(tmp);
 132 
                cosnick = (s32)c_cos_8192(tmp);
129 
                tmp = FromFlightCtrl.AngleRoll/10; // in deg
 133 
                tmp = FromFlightCtrl.AngleRoll/10; // in deg
130 
                sinroll = (s32)c_sin_8192(tmp);
 134 
                sinroll = (s32)c_sin_8192(tmp);
Line 138... Line 142...
138 
                else tmp = -tmp;
 142 
                else tmp = -tmp;
139 
                Compass_Heading = tmp;
 143 
                Compass_Heading = tmp;
140 
        }
 144 
        }
141 
}
 145 
}
Line -... Line 146...
- 
 
 146 
 
142 
 
 147 
 
143 
void Compass_Update(void)
 148 
void Compass_Update(void)
144 
{
 149 
{
145 
static s16vec_t old;  
 150 
static s16vec_t old;  
146 
static u32 check_value_counter = 0;
 151 
static u32 check_value_counter = 0;