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