Rev 492 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 492 | Rev 493 | ||
---|---|---|---|
Line 67... | Line 67... | ||
67 | u8 CompassCalStateQueue[10]; |
67 | u8 CompassCalStateQueue[10]; |
68 | fifo_t CompassCalcStateFiFo; |
68 | fifo_t CompassCalcStateFiFo; |
Line 69... | Line 69... | ||
69 | 69 | ||
70 | volatile s16vec_t MagVector; // is written by mk3mag or ncmag implementation |
70 | volatile s16vec_t MagVector; // is written by mk3mag or ncmag implementation |
- | 71 | volatile s16vec_t AccVector; // current acceleration vector of compass, not supported by any HW version |
|
71 | volatile s16vec_t AccVector; // current acceleration vector of compass, not supported by any HW version |
72 | volatile s16vec_t MagVectorHorizontal; // vector componenents in horizontal projection |
72 | volatile s16 Compass_Heading; // is written by mk3mag or ncmag implementation |
73 | volatile s16 Compass_Heading; // is written by mk3mag or ncmag implementation |
73 | volatile u8 Compass_CalState; // is written by mk3mag or ncmag implementation |
- | |
- | 74 | volatile u8 Compass_CalState; // is written by mk3mag or ncmag implementation |
|
74 | s16 Hx = 0, Hy = 0; |
75 | |
75 | s32 EarthMagneticField = 100; |
76 | s32 EarthMagneticField = 100; |
76 | s32 EarthMagneticFieldFiltered = 100; |
77 | s32 EarthMagneticFieldFiltered = 100; |
77 | s32 EarthMagneticInclination = 0; |
78 | s32 EarthMagneticInclination = 0; |
78 | s32 EarthMagneticInclinationFiltered = 0; |
79 | s32 EarthMagneticInclinationFiltered = 0; |
Line 146... | Line 147... | ||
146 | Compass_Heading = -1; |
147 | Compass_Heading = -1; |
147 | } |
148 | } |
148 | else // fc attitude is avialable and no compass calibration active |
149 | else // fc attitude is avialable and no compass calibration active |
149 | { |
150 | { |
150 | // calculate attitude correction |
151 | // calculate attitude correction |
151 | // a float implementation takes too long |
- | |
152 | s16 tmp; |
152 | s16 tmp; |
153 | s32 sinnick, cosnick, sinroll, cosroll; |
153 | s32 sinnick, cosnick, sinroll, cosroll; |
154 | tmp = FromFlightCtrl.AngleNick/10; // in deg |
154 | tmp = FromFlightCtrl.AngleNick/10; // in deg |
155 | sinnick = (s32)c_sin_8192(tmp); |
155 | sinnick = (s32)c_sin_8192(tmp); |
156 | cosnick = (s32)c_cos_8192(tmp); |
156 | cosnick = (s32)c_cos_8192(tmp); |
157 | tmp = FromFlightCtrl.AngleRoll/10; // in deg |
157 | tmp = FromFlightCtrl.AngleRoll/10; // in deg |
158 | sinroll = (s32)c_sin_8192(tmp); |
158 | sinroll = (s32)c_sin_8192(tmp); |
159 | cosroll = (s32)c_cos_8192(tmp); |
159 | cosroll = (s32)c_cos_8192(tmp); |
160 | // tbd. compensation signs and oriantation has to be fixed |
- | |
161 | Hx = (s16)((MagVector.Y * cosnick + MagVector.Z * sinnick)/8192L); |
160 | MagVectorHorizontal.X = (s16)((MagVector.Y * cosnick + MagVector.Z * sinnick)/8192L); |
162 | Hy = (s16)((MagVector.X * cosroll - MagVector.Z * sinroll)/8192L); |
161 | MagVectorHorizontal.Y = (s16)((MagVector.X * cosroll - MagVector.Z * sinroll)/8192L); |
163 | // calculate heading |
162 | // calculate heading |
164 | tmp = (s16)(c_atan2_546(Hy, Hx)/546L); |
163 | tmp = (s16)(c_atan2_546(MagVectorHorizontal.Y, MagVectorHorizontal.X)/546L); |
165 | if (tmp > 0) tmp = 360 - tmp; |
164 | if (tmp > 0) tmp = 360 - tmp; |
166 | else tmp = -tmp; |
165 | else tmp = -tmp; |
167 | Compass_Heading = tmp; |
166 | Compass_Heading = tmp; |
168 | } |
167 | } |
169 | } |
168 | } |