Rev 32 | Rev 34 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 32 | Rev 33 | ||
---|---|---|---|
Line 156... | Line 156... | ||
156 | 156 | ||
157 | 157 | ||
158 | void CalcHeading(void) |
158 | void CalcHeading(void) |
159 | { |
159 | { |
160 | double nick_rad, roll_rad, Hx, Hy, Cx = 0.0, Cy = 0.0, Cz = 0.0; |
160 | double nick_rad, roll_rad, Hx, Hy, Cx = 0.0, Cy = 0.0, Cz = 0.0; |
Line 161... | Line 161... | ||
161 | int16_t nick, roll; |
161 | int16_t nick, roll; |
162 | int16_t heading = -1; |
162 | int16_t heading = -1; |
163 | 163 | ||
164 | // blink code for normal operation |
164 | // blink code for normal operation |
165 | if(CheckDelay(Led_Timer)) |
165 | if(CheckDelay(Led_Timer)) |
166 | { |
166 | { |
167 | LED_GRN_TOGGLE; |
- | |
168 | Led_Timer = SetDelay(500); |
- | |
Line 169... | Line 167... | ||
169 | } |
167 | LED_GRN_TOGGLE; |
170 | //MagX = 150; |
168 | Led_Timer = SetDelay(500); |
171 | //MagZ = 1000; |
169 | } |
172 | 170 | ||
Line 212... | Line 210... | ||
212 | } |
210 | } |
Line 213... | Line 211... | ||
213 | 211 | ||
214 | nick_rad = ((double)nick) * M_PI / (double)(1800.0); |
212 | nick_rad = ((double)nick) * M_PI / (double)(1800.0); |
Line 215... | Line -... | ||
215 | roll_rad = ((double)roll) * M_PI / (double)(1800.0); |
- | |
216 | - | ||
Line 217... | Line 213... | ||
217 | // nick_rad = 0; |
213 | roll_rad = ((double)roll) * M_PI / (double)(1800.0); |
218 | // roll_rad = 0; |
214 | |
219 | 215 | ||
Line 228... | Line 224... | ||
228 | heading = (int16_t)((180.0 * atan2(Hy, Hx)) / M_PI); |
224 | heading = (int16_t)((180.0 * atan2(Hy, Hx)) / M_PI); |
229 | // atan2 returns angular range from -180 deg to 180 deg in counter clockwise notation |
225 | // atan2 returns angular range from -180 deg to 180 deg in counter clockwise notation |
230 | // but the compass course is defined in a range from 0 deg to 360 deg clockwise notation. |
226 | // but the compass course is defined in a range from 0 deg to 360 deg clockwise notation. |
231 | if (heading < 0) heading = -heading; |
227 | if (heading < 0) heading = -heading; |
232 | else heading = 360 - heading; |
228 | else heading = 360 - heading; |
- | 229 | ||
233 | //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
230 | //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
234 | /* |
231 | /* // Alternative formula |
235 | Hx = Cx * (double)cos(nick_rad) + |
232 | Hx = Cx * (double)cos(nick_rad) + |
236 | Cy * (double)sin(nick_rad) * (double)sin(roll_rad) - |
233 | Cy * (double)sin(nick_rad) * (double)sin(roll_rad) - |
237 | Cz * (double)sin(nick_rad) * (double)cos(roll_rad); |
234 | Cz * (double)sin(nick_rad) * (double)cos(roll_rad); |
238 | |
235 | |
239 | Hy = Cy * (double)cos(roll_rad) + |
236 | Hy = Cy * (double)cos(roll_rad) + |
240 | Cz * (double)sin(roll_rad); |
237 | Cz * (double)sin(roll_rad); |
241 | |
238 | |
242 | if(Hx == 0 && Hy < 0) heading = 90; |
239 | if(Hx == 0 && Hy < 0) heading = 90; |
243 | else if(Hx == 0 && Hy > 0) heading = 270; |
240 | else if(Hx == 0 && Hy > 0) heading = 270; |
244 | else if(Hx < 0) heading = 180 - (atan(Hy / Hx) * 180.0) / M_PI; |
241 | else if(Hx < 0) heading = 180 - (atan(Hy / Hx) * 180.0) / M_PI; |
245 | else if(Hx > 0 && Hy < 0) heading = - (atan(Hy / Hx) * 180.0) / M_PI; |
242 | else if(Hx > 0 && Hy < 0) heading = - (atan(Hy / Hx) * 180.0) / M_PI; |
246 | else if(Hx > 0 && Hy > 0) heading = 360 - (atan(Hy / Hx) * 180.0) / M_PI; |
243 | else if(Hx > 0 && Hy > 0) heading = 360 - (atan(Hy / Hx) * 180.0) / M_PI; |
Line 247... | Line 244... | ||
247 | 244 | ||
248 | if(abs(heading) < 361) Heading = heading; |
245 | if(abs(heading) < 361) Heading = heading; |
249 | */ |
246 | */ |
Line 250... | Line 247... | ||
250 | //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
247 | //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
251 | 248 | ||
252 | cli(); // stop interrupts |
249 | cli(); // stop interrupts |
253 | if(abs(heading) < 361) Heading = heading; |
250 | if(abs(heading) < 361) Heading = heading; |
254 | else (Heading = -1); |
251 | else (Heading = -1); |
255 | sei(); // start interrupts |
252 | sei(); // start interrupts |
Line 256... | Line 253... | ||
256 | 253 | ||
257 | } |
254 | } |
258 | 255 | ||
259 | 256 | ||
260 | void Calibrate(void) |
257 | void Calibrate(void) |
261 | { |
258 | { |
262 | uint8_t cal; |
259 | uint8_t cal; |
263 | static uint8_t calold = 0; |
260 | static uint8_t calold = 0; |
264 | static int16_t Xmin = 0, Xmax = 0, Ymin = 0, Ymax = 0, Zmin = 0, Zmax = 0; |
261 | static int16_t Xmin = 0, Xmax = 0, Ymin = 0, Ymax = 0, Zmin = 0, Zmax = 0; |
265 | static uint8_t blinkcount = 0; |
262 | static uint8_t blinkcount = 0; |
266 | static uint8_t invert_blinking = 0; |
263 | static uint8_t invert_blinking = 0; |