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