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