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