Rev 310 | Rev 321 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 310 | Rev 311 | ||
|---|---|---|---|
| Line 62... | Line 62... | ||
| 62 | #include "mymath.h" |
62 | #include "mymath.h" |
| 63 | #include "uart1.h" |
63 | #include "uart1.h" |
| 64 | #include "fifo.h" |
64 | #include "fifo.h" |
| 65 | #include "led.h" |
65 | #include "led.h" |
| 66 | #include "main.h" |
66 | #include "main.h" |
| 67 | #include "ncmag.h" |
- | |
| Line 68... | Line 67... | ||
| 68 | 67 | ||
| 69 | u8 CompassCalStateQueue[10]; |
68 | u8 CompassCalStateQueue[10]; |
| Line 70... | Line 69... | ||
| 70 | fifo_t CompassCalcStateFiFo; |
69 | fifo_t CompassCalcStateFiFo; |
| Line 93... | Line 92... | ||
| 93 | { |
92 | { |
| 94 | if( MK3MAG_Init() ) Compass_Device = COMPASS_MK3MAG; |
93 | if( MK3MAG_Init() ) Compass_Device = COMPASS_MK3MAG; |
| 95 | else if( NCMAG_Init() ) Compass_Device = COMPASS_NCMAG; |
94 | else if( NCMAG_Init() ) Compass_Device = COMPASS_NCMAG; |
| 96 | } |
95 | } |
| 97 | break; |
96 | break; |
| 98 | 97 | ||
| 99 | case COMPASS_NCMAG: |
98 | case COMPASS_NCMAG: |
| 100 | if( NCMAG_Init() ) Compass_Device = COMPASS_NCMAG; |
99 | if( NCMAG_Init() ) Compass_Device = COMPASS_NCMAG; |
| 101 | else Compass_Device = COMPASS_NONE; |
100 | else Compass_Device = COMPASS_NONE; |
| 102 | break; |
101 | break; |
| Line 103... | Line 102... | ||
| 103 | 102 | ||
| 104 | case COMPASS_MK3MAG: |
103 | case COMPASS_MK3MAG: |
| 105 | default: |
104 | default: |
| Line 121... | Line 120... | ||
| 121 | { |
120 | { |
| 122 | // calculate attitude correction |
121 | // calculate attitude correction |
| 123 | // a float implementation takes too long |
122 | // a float implementation takes too long |
| 124 | s16 tmp, Hx, Hy; |
123 | s16 tmp, Hx, Hy; |
| 125 | s32 sinnick, cosnick, sinroll, cosroll; |
124 | s32 sinnick, cosnick, sinroll, cosroll; |
| 126 | 125 | ||
| 127 | tmp = FromFlightCtrl.AngleNick/10; // in deg |
126 | tmp = FromFlightCtrl.AngleNick/10; // in deg |
| 128 | sinnick = (s32)c_sin_8192(tmp); |
127 | sinnick = (s32)c_sin_8192(tmp); |
| 129 | cosnick = (s32)c_cos_8192(tmp); |
128 | cosnick = (s32)c_cos_8192(tmp); |
| 130 | tmp = FromFlightCtrl.AngleRoll/10; // in deg |
129 | tmp = FromFlightCtrl.AngleRoll/10; // in deg |
| 131 | sinroll = (s32)c_sin_8192(tmp); |
130 | sinroll = (s32)c_sin_8192(tmp); |
| 132 | cosroll = (s32)c_cos_8192(tmp); |
131 | cosroll = (s32)c_cos_8192(tmp); |
| 133 | // tbd. compensation signs and oriantation has to be fixed |
132 | // tbd. compensation signs and oriantation has to be fixed |
| 134 | Hx = (s16)((MagVector.Y * cosnick + MagVector.Z * sinnick)/8192L); |
133 | Hx = (s16)((MagVector.Y * cosnick + MagVector.Z * sinnick)/8192L); |
| 135 | Hy = (s16)((MagVector.X * cosroll - MagVector.Z * sinroll)/8192L); |
134 | Hy = (s16)((MagVector.X * cosroll - MagVector.Z * sinroll)/8192L); |
| 136 | // calculate heading |
135 | // calculate heading |
| 137 | tmp = (s16)(c_atan2_546(Hy, Hx)/546L); |
136 | tmp = (s16)(c_atan2_546(Hy, Hx)/546L); |
| 138 | if (tmp > 0) tmp = 360 - tmp; |
137 | if (tmp > 0) tmp = 360 - tmp; |
| 139 | else tmp = -tmp; |
138 | else tmp = -tmp; |
| 140 | Compass_Heading = tmp; |
139 | Compass_Heading = tmp; |
| Line 150... | Line 149... | ||
| 150 | { |
149 | { |
| 151 | case COMPASS_MK3MAG: |
150 | case COMPASS_MK3MAG: |
| 152 | MK3MAG_Update(); |
151 | MK3MAG_Update(); |
| 153 | break; |
152 | break; |
| 154 | case COMPASS_NCMAG: |
153 | case COMPASS_NCMAG: |
| 155 | NCMAG_Update(); |
154 | NCMAG_Update(); |
| 156 | default: |
155 | default: |
| 157 | break; |
156 | break; |
| 158 | } |
157 | } |
| 159 | DebugOut.Analog[24] = MagVector.X; |
158 | DebugOut.Analog[24] = MagVector.X; |
| 160 | DebugOut.Analog[25] = MagVector.Y; |
159 | DebugOut.Analog[25] = MagVector.Y; |
| 161 | DebugOut.Analog[26] = MagVector.Z; |
160 | DebugOut.Analog[26] = MagVector.Z; |
| 162 | } |
161 | } |
| Line 163... | Line 162... | ||
| 163 | 162 | ||
| 164 | // put cal state into fifo |
163 | // put cal state into fifo |
| 165 | void Compass_SetCalState(u8 CalState) |
164 | void Compass_SetCalState(u8 CalState) |
| 166 | { |
165 | { |
| 167 | fifo_put(&CompassCalcStateFiFo, CalState); |
166 | fifo_put(&CompassCalcStateFiFo, CalState); |
| Line 168... | Line 167... | ||
| 168 | } |
167 | } |
| 169 | 168 | ||
| 170 | // get cal state from fifo |
169 | // get cal state from fifo |
| 171 | void Compass_UpdateCalState() |
170 | void Compass_UpdateCalState() |
| 172 | { |
171 | { |